add bio drones into simulation environment(need debugging)
parent
81cb1c9043
commit
68c9756b4c
@ -1,10 +1,14 @@
|
||||
#ifndef DRONESTATUS_H
|
||||
#define DRONESTATUS_H
|
||||
|
||||
#include <std_msgs/String.h>
|
||||
#include "class_model/gps_location.h"
|
||||
typedef struct DroneStatus
|
||||
{
|
||||
struct DroneStatus{
|
||||
/* data */
|
||||
global_location init_location;
|
||||
int plane,ID;
|
||||
std::string leader;
|
||||
std::string DroneName;
|
||||
} DroneStatus;
|
||||
};
|
||||
|
||||
#endif // DRONESTATUS_H
|
||||
|
||||
@ -0,0 +1,49 @@
|
||||
/*Follow the leader in a fixed formation */
|
||||
#ifndef Leader_H
|
||||
#define Leader_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "class_model/sensor.h"
|
||||
#include "class_model/mode.h"
|
||||
#include "class_model/receiver.h"
|
||||
#include "class_model/control.h"
|
||||
#include "class_model/command.h"
|
||||
#include "class_model/requestData.h"
|
||||
#include "class_model/PID.h"
|
||||
#include "class_model/PubData.h"
|
||||
#include <std_msgs/String.h>
|
||||
|
||||
class LeaderClass {
|
||||
public:
|
||||
LeaderClass();
|
||||
virtual ~LeaderClass();
|
||||
//ClassObject
|
||||
ThreadGPSClass GPS_object;
|
||||
ModeClass mode_object;
|
||||
ReceiverClass receiver_object;
|
||||
CommandClass command_object;
|
||||
RequestClass request_object;
|
||||
PubDataClass PubData_object;
|
||||
PIDClass PID_x;
|
||||
PIDClass PID_y;
|
||||
|
||||
void leader(global_location target);
|
||||
void go2target(float x,float y);
|
||||
|
||||
private:
|
||||
// ROS NodeHandle
|
||||
ros::NodeHandle node_handle_;
|
||||
|
||||
global_location target_location;
|
||||
global_location follower_location;
|
||||
global_location leader_location;
|
||||
global_location current_location;
|
||||
velocity drone_speed;
|
||||
int flag = 0,heading_status = 0,buf = 0;
|
||||
float pre_alt,cur_alt;
|
||||
float error_lon,error_lat;
|
||||
float leader_pid[3] = {1 , 0.000000000001 ,0.5};
|
||||
float ignore_small = 0.50;
|
||||
void face2target(float target_heading);
|
||||
};
|
||||
#endif // Leader_H
|
||||
@ -0,0 +1,62 @@
|
||||
#include "class_model/FindLeader.h"
|
||||
#include "class_model/Leader.h"
|
||||
#include "class_model/follower.h"
|
||||
#include "yaml-cpp/yaml.h"
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
|
||||
// Init ROS node
|
||||
ros::init(argc, argv, "drone2_node");
|
||||
|
||||
// reate Assync spiner
|
||||
ros::AsyncSpinner spinner(0);
|
||||
spinner.start();
|
||||
// ros::Rate rate(5);
|
||||
|
||||
ModeClass mode_object;
|
||||
ControlClass control_object;
|
||||
RequestClass request_object;
|
||||
FollowerClass follower_object;
|
||||
LeaderClass leader_object;
|
||||
|
||||
// SelectionClass selection_object;
|
||||
|
||||
global_location target,self,M1,M2;
|
||||
int ref_ID,checkLeader;
|
||||
|
||||
YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config2.yaml"); //reading yaml parameter(target point)
|
||||
target.lon = config["routh1"]["x"].as<float>();
|
||||
target.lat = config["routh1"]["y"].as<float>();
|
||||
|
||||
|
||||
self = request_object.get_self_GPS();
|
||||
M1 = request_object.get_M1_GPS();
|
||||
M2 = request_object.get_M2_GPS();
|
||||
|
||||
global_location pos[] = {target,self,M1,M2};
|
||||
global_location member[] = {M1,M2};
|
||||
size_t index = sizeof(pos)/sizeof(pos[0]);
|
||||
size_t m_index = sizeof(member)/sizeof(member[0]);
|
||||
|
||||
// mode_object.set_Mode("GUIDED");
|
||||
// control_object.arm();
|
||||
// control_object.takeoff(2);
|
||||
|
||||
checkLeader = check_leader(pos,index).is_leader;
|
||||
if(checkLeader != 1){
|
||||
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
|
||||
}
|
||||
|
||||
while(true){
|
||||
//
|
||||
// if(checkLeader == 1){
|
||||
// leader_object.leader(target);
|
||||
// }else{
|
||||
// follower_object.follower(member,m_index,ref_ID); //follow reference drone position
|
||||
// }
|
||||
}
|
||||
ros::waitForShutdown();
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
@ -0,0 +1,62 @@
|
||||
#include "class_model/FindLeader.h"
|
||||
#include "class_model/Leader.h"
|
||||
#include "class_model/follower.h"
|
||||
#include "yaml-cpp/yaml.h"
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
|
||||
// Init ROS node
|
||||
ros::init(argc, argv, "drone3_node");
|
||||
|
||||
// reate Assync spiner
|
||||
ros::AsyncSpinner spinner(0);
|
||||
spinner.start();
|
||||
// ros::Rate rate(5);
|
||||
|
||||
ModeClass mode_object;
|
||||
ControlClass control_object;
|
||||
RequestClass request_object;
|
||||
FollowerClass follower_object;
|
||||
LeaderClass leader_object;
|
||||
|
||||
// SelectionClass selection_object;
|
||||
|
||||
global_location target,self,M1,M2;
|
||||
int ref_ID,checkLeader;
|
||||
|
||||
YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config3.yaml"); //reading yaml parameter(target point)
|
||||
target.lon = config["routh1"]["x"].as<float>();
|
||||
target.lat = config["routh1"]["y"].as<float>();
|
||||
|
||||
|
||||
self = request_object.get_self_GPS();
|
||||
M1 = request_object.get_M1_GPS();
|
||||
M2 = request_object.get_M2_GPS();
|
||||
|
||||
global_location pos[] = {target,self,M1,M2};
|
||||
global_location member[] = {M1,M2};
|
||||
size_t index = sizeof(pos)/sizeof(pos[0]);
|
||||
size_t m_index = sizeof(member)/sizeof(member[0]);
|
||||
|
||||
// mode_object.set_Mode("GUIDED");
|
||||
// control_object.arm();
|
||||
// control_object.takeoff(2);
|
||||
|
||||
checkLeader = check_leader(pos,index).is_leader;
|
||||
if(checkLeader != 1){
|
||||
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
|
||||
}
|
||||
|
||||
while(true){
|
||||
//
|
||||
// if(checkLeader == 1){
|
||||
// leader_object.leader(target);
|
||||
// }else{
|
||||
// follower_object.follower(member,m_index,ref_ID); //follow reference drone position
|
||||
// }
|
||||
}
|
||||
ros::waitForShutdown();
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
@ -0,0 +1,10 @@
|
||||
DroneParam:
|
||||
ID: 2
|
||||
routh1:
|
||||
x: 120.6743661 #(24.1218859, 120.6743161)
|
||||
y: 24.1219359
|
||||
z: 0.0
|
||||
formation:
|
||||
distance: 4.0
|
||||
angle: 45
|
||||
|
||||
@ -0,0 +1,10 @@
|
||||
DroneParam:
|
||||
ID: 3
|
||||
routh1:
|
||||
x: 120.6743661 #(24.1218859, 120.6743161)
|
||||
y: 24.1219359
|
||||
z: 0.0
|
||||
formation:
|
||||
distance: 4.0
|
||||
angle: 45
|
||||
|
||||
@ -0,0 +1,103 @@
|
||||
#!/usr/bin/env python3
|
||||
#coding:utf-8
|
||||
import paho.mqtt.client as mqtt
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import utils
|
||||
import proto.flight_information_pb2 as flight_information_pb2
|
||||
import proto.flyformatioln_pb2 as flyformatioln_pb2
|
||||
import logging
|
||||
|
||||
|
||||
import rospy
|
||||
from std_msgs.msg import String
|
||||
from std_msgs.msg import Float64
|
||||
from std_msgs.msg import Header
|
||||
from mavros_msgs.srv import ParamGet
|
||||
from sensor_msgs.msg import NavSatFix
|
||||
from sensor_msgs.msg import Imu
|
||||
from sensor_msgs.msg import Range
|
||||
from geometry_msgs.msg import Vector3
|
||||
|
||||
|
||||
|
||||
|
||||
def init_dataFormat(cfg:utils.Read_PUB_Config):
|
||||
ros_namespace="/drone2"
|
||||
if cfg.msg_format == "Proto":
|
||||
utils.Proto_msg_from_ros.client = client
|
||||
utils.Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
|
||||
utils.Proto_msg_from_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
|
||||
utils.Proto_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
|
||||
utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
|
||||
utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
|
||||
rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Proto_msg_from_ros.callBack_gps)
|
||||
rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Proto_msg_from_ros.callBack_compass_hdg)
|
||||
elif cfg.msg_format == "Json":
|
||||
utils.Json_msg_from_ros.client = client
|
||||
utils.Json_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
|
||||
utils.Json_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
|
||||
utils.Json_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
|
||||
rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
|
||||
rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
|
||||
else:
|
||||
logging.debug("msg_format not found")
|
||||
|
||||
|
||||
def on_connect(self, userdata, flags, rc):
|
||||
logger.info("Connected with result code " + str(rc))
|
||||
|
||||
def on_publish(self, userdata, mid):
|
||||
logger.debug("pub success")
|
||||
|
||||
def on_disconnect(client, userdata, rc):
|
||||
logger.debug("disconnecting reason " +str(rc))
|
||||
client.connected_flag=False
|
||||
client.disconnect_flag=True
|
||||
|
||||
if __name__ == '__main__':
|
||||
# Read Config
|
||||
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB2.yml")
|
||||
cfg = utils.Read_PUB_Config(FilePath)
|
||||
client = utils.MQTTClient(cfg.MQTTClientNamePub)
|
||||
|
||||
# set log
|
||||
log_format = "%(asctime)s - %(levelname)s - %(message)s"
|
||||
formatter = logging.Formatter(log_format)
|
||||
|
||||
stream_handler = logging.StreamHandler()
|
||||
stream_handler.setFormatter(formatter)
|
||||
stream_handler.setLevel(logging.DEBUG)
|
||||
|
||||
file_handler = logging.FileHandler(cfg.logFileName)
|
||||
file_handler.setFormatter(formatter)
|
||||
file_handler.setLevel(logging.INFO)
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
logger.setLevel(logging.INFO)
|
||||
logger.addHandler(file_handler)
|
||||
logger.addHandler(stream_handler)
|
||||
logger.info(cfg)
|
||||
|
||||
# Mqtt
|
||||
client.on_connect = on_connect
|
||||
client.on_publish = on_publish
|
||||
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
|
||||
client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
|
||||
client.loop_start()
|
||||
|
||||
# Ros
|
||||
rosClient = cfg.ROSClientNamePub
|
||||
rospy.init_node(rosClient)
|
||||
|
||||
# init data format
|
||||
init_dataFormat(cfg)
|
||||
|
||||
try:
|
||||
rospy.spin()
|
||||
except BaseException as error:
|
||||
client.disconnect()
|
||||
logger.info("End of program")
|
||||
sys.exit()
|
||||
|
||||
@ -0,0 +1,103 @@
|
||||
#!/usr/bin/env python3
|
||||
#coding:utf-8
|
||||
import paho.mqtt.client as mqtt
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import utils
|
||||
import proto.flight_information_pb2 as flight_information_pb2
|
||||
import proto.flyformatioln_pb2 as flyformatioln_pb2
|
||||
import logging
|
||||
|
||||
|
||||
import rospy
|
||||
from std_msgs.msg import String
|
||||
from std_msgs.msg import Float64
|
||||
from std_msgs.msg import Header
|
||||
from mavros_msgs.srv import ParamGet
|
||||
from sensor_msgs.msg import NavSatFix
|
||||
from sensor_msgs.msg import Imu
|
||||
from sensor_msgs.msg import Range
|
||||
from geometry_msgs.msg import Vector3
|
||||
|
||||
|
||||
|
||||
|
||||
def init_dataFormat(cfg:utils.Read_PUB_Config):
|
||||
ros_namespace="/drone3"
|
||||
if cfg.msg_format == "Proto":
|
||||
utils.Proto_msg_from_ros.client = client
|
||||
utils.Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
|
||||
utils.Proto_msg_from_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
|
||||
utils.Proto_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
|
||||
utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
|
||||
utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
|
||||
rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Proto_msg_from_ros.callBack_gps)
|
||||
rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Proto_msg_from_ros.callBack_compass_hdg)
|
||||
elif cfg.msg_format == "Json":
|
||||
utils.Json_msg_from_ros.client = client
|
||||
utils.Json_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
|
||||
utils.Json_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
|
||||
utils.Json_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
|
||||
rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
|
||||
rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
|
||||
else:
|
||||
logging.debug("msg_format not found")
|
||||
|
||||
|
||||
def on_connect(self, userdata, flags, rc):
|
||||
logger.info("Connected with result code " + str(rc))
|
||||
|
||||
def on_publish(self, userdata, mid):
|
||||
logger.debug("pub success")
|
||||
|
||||
def on_disconnect(client, userdata, rc):
|
||||
logger.debug("disconnecting reason " +str(rc))
|
||||
client.connected_flag=False
|
||||
client.disconnect_flag=True
|
||||
|
||||
if __name__ == '__main__':
|
||||
# Read Config
|
||||
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB3.yml")
|
||||
cfg = utils.Read_PUB_Config(FilePath)
|
||||
client = utils.MQTTClient(cfg.MQTTClientNamePub)
|
||||
|
||||
# set log
|
||||
log_format = "%(asctime)s - %(levelname)s - %(message)s"
|
||||
formatter = logging.Formatter(log_format)
|
||||
|
||||
stream_handler = logging.StreamHandler()
|
||||
stream_handler.setFormatter(formatter)
|
||||
stream_handler.setLevel(logging.DEBUG)
|
||||
|
||||
file_handler = logging.FileHandler(cfg.logFileName)
|
||||
file_handler.setFormatter(formatter)
|
||||
file_handler.setLevel(logging.INFO)
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
logger.setLevel(logging.INFO)
|
||||
logger.addHandler(file_handler)
|
||||
logger.addHandler(stream_handler)
|
||||
logger.info(cfg)
|
||||
|
||||
# Mqtt
|
||||
client.on_connect = on_connect
|
||||
client.on_publish = on_publish
|
||||
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
|
||||
client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
|
||||
client.loop_start()
|
||||
|
||||
# Ros
|
||||
rosClient = cfg.ROSClientNamePub
|
||||
rospy.init_node(rosClient)
|
||||
|
||||
# init data format
|
||||
init_dataFormat(cfg)
|
||||
|
||||
try:
|
||||
rospy.spin()
|
||||
except BaseException as error:
|
||||
client.disconnect()
|
||||
logger.info("End of program")
|
||||
sys.exit()
|
||||
|
||||
@ -0,0 +1,107 @@
|
||||
#!/usr/bin/env python3
|
||||
#coding:utf-8
|
||||
import paho.mqtt.client as mqtt
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import utils
|
||||
import proto.flight_information_pb2 as flight_information_pb2
|
||||
import proto.flyformatioln_pb2 as flyformatioln_pb2
|
||||
import rospy
|
||||
from std_msgs.msg import String
|
||||
import logging
|
||||
|
||||
|
||||
def init_dataFormat(cfg:utils.Read_SUB_Config):
|
||||
if cfg.msg_format == "Proto":
|
||||
utils.Proto_msg_to_ros.rate = rospy.Rate(10)
|
||||
utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
|
||||
utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
|
||||
|
||||
utils.Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
|
||||
utils.Proto_msg_to_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
|
||||
|
||||
client.message_callback_add(cfg.Flight_Information_topicToMqtt, utils.Proto_msg_to_ros.on_message_Flight_Information)
|
||||
client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Proto_msg_to_ros.on_message_Fly_Formation)
|
||||
|
||||
utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
|
||||
utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
|
||||
elif cfg.msg_format == "Json":
|
||||
# set ros publisher
|
||||
utils.Json_msg_to_ros.rate = rospy.Rate(10)
|
||||
utils.Json_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
|
||||
utils.Json_msg_to_ros.Drone550_publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,String,queue_size=10)
|
||||
utils.Json_msg_to_ros.Drone380_publisher_Flight_Information = rospy.Publisher(cfg.Dron380_ROStopicName_Flight_Information,String,queue_size=10)
|
||||
utils.Json_msg_to_ros.Drone650_publisher_Flight_Information = rospy.Publisher(cfg.Dron650_ROStopicName_Flight_Information,String,queue_size=10)
|
||||
# set callback to each topic
|
||||
client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Json_msg_to_ros.on_message_Fly_Formation)
|
||||
client.message_callback_add(cfg.Drone550_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone550_on_message_Flight_Information)
|
||||
client.message_callback_add(cfg.Drone380_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone380_on_message_Flight_Information)
|
||||
client.message_callback_add(cfg.Drone650_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone650_on_message_Flight_Information)
|
||||
else:
|
||||
logger.debug("msg_format not found")
|
||||
|
||||
|
||||
def on_connect(self, userdata, flags, rc):
|
||||
logger.info("Connected with result code " + str(rc))
|
||||
client.subscribe(cfg.Fly_Formation_topicToMqtt)
|
||||
client.subscribe(cfg.Drone550_Flight_Information_topicToMqtt)
|
||||
client.subscribe(cfg.Drone380_Flight_Information_topicToMqtt)
|
||||
client.subscribe(cfg.Drone650_Flight_Information_topicToMqtt)
|
||||
|
||||
def on_disconnect(client, userdata, rc):
|
||||
logger.info("disconnecting reason " +str(rc))
|
||||
client.connected_flag=False
|
||||
client.disconnect_flag=True
|
||||
|
||||
def on_publish(self, userdata, mid):
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# Read Config
|
||||
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_SUB2.yml")
|
||||
cfg = utils.Read_SUB_Config(FilePath)
|
||||
client = utils.MQTTClient(cfg.MQTTClientNameSub)
|
||||
|
||||
# set log
|
||||
log_format = "%(asctime)s - %(levelname)s - %(message)s"
|
||||
formatter = logging.Formatter(log_format)
|
||||
|
||||
stream_handler = logging.StreamHandler()
|
||||
stream_handler.setFormatter(formatter)
|
||||
stream_handler.setLevel(logging.DEBUG)
|
||||
|
||||
|
||||
file_handler = logging.FileHandler(cfg.logFileName)
|
||||
file_handler.setFormatter(formatter)
|
||||
file_handler.setLevel(logging.INFO)
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
logger.setLevel(logging.DEBUG)
|
||||
logger.addHandler(file_handler)
|
||||
logger.addHandler(stream_handler)
|
||||
|
||||
logger.info(cfg)
|
||||
|
||||
# Mqtt
|
||||
client = utils.MQTTClient(cfg.MQTTClientNameSub)
|
||||
client.on_connect = on_connect
|
||||
client.on_publish = on_publish
|
||||
client.on_disconnect = on_disconnect
|
||||
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
|
||||
|
||||
|
||||
# Ros
|
||||
rospy.init_node(cfg.ROSClientNameSub)
|
||||
# initialize
|
||||
init_dataFormat(cfg)
|
||||
|
||||
try:
|
||||
client.loop_start()
|
||||
rospy.spin()
|
||||
except BaseException as error:
|
||||
client.loop_stop()
|
||||
client.disconnect()
|
||||
logger.info("End of program")
|
||||
sys.exit(0)
|
||||
@ -0,0 +1,107 @@
|
||||
#!/usr/bin/env python3
|
||||
#coding:utf-8
|
||||
import paho.mqtt.client as mqtt
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import utils
|
||||
import proto.flight_information_pb2 as flight_information_pb2
|
||||
import proto.flyformatioln_pb2 as flyformatioln_pb2
|
||||
import rospy
|
||||
from std_msgs.msg import String
|
||||
import logging
|
||||
|
||||
|
||||
def init_dataFormat(cfg:utils.Read_SUB_Config):
|
||||
if cfg.msg_format == "Proto":
|
||||
utils.Proto_msg_to_ros.rate = rospy.Rate(10)
|
||||
utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
|
||||
utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
|
||||
|
||||
utils.Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
|
||||
utils.Proto_msg_to_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
|
||||
|
||||
client.message_callback_add(cfg.Flight_Information_topicToMqtt, utils.Proto_msg_to_ros.on_message_Flight_Information)
|
||||
client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Proto_msg_to_ros.on_message_Fly_Formation)
|
||||
|
||||
utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
|
||||
utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
|
||||
elif cfg.msg_format == "Json":
|
||||
# set ros publisher
|
||||
utils.Json_msg_to_ros.rate = rospy.Rate(10)
|
||||
utils.Json_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
|
||||
utils.Json_msg_to_ros.Drone550_publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,String,queue_size=10)
|
||||
utils.Json_msg_to_ros.Drone380_publisher_Flight_Information = rospy.Publisher(cfg.Dron380_ROStopicName_Flight_Information,String,queue_size=10)
|
||||
utils.Json_msg_to_ros.Drone650_publisher_Flight_Information = rospy.Publisher(cfg.Dron650_ROStopicName_Flight_Information,String,queue_size=10)
|
||||
# set callback to each topic
|
||||
client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Json_msg_to_ros.on_message_Fly_Formation)
|
||||
client.message_callback_add(cfg.Drone550_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone550_on_message_Flight_Information)
|
||||
client.message_callback_add(cfg.Drone380_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone380_on_message_Flight_Information)
|
||||
client.message_callback_add(cfg.Drone650_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.Drone650_on_message_Flight_Information)
|
||||
else:
|
||||
logger.debug("msg_format not found")
|
||||
|
||||
|
||||
def on_connect(self, userdata, flags, rc):
|
||||
logger.info("Connected with result code " + str(rc))
|
||||
client.subscribe(cfg.Fly_Formation_topicToMqtt)
|
||||
client.subscribe(cfg.Drone550_Flight_Information_topicToMqtt)
|
||||
client.subscribe(cfg.Drone380_Flight_Information_topicToMqtt)
|
||||
client.subscribe(cfg.Drone650_Flight_Information_topicToMqtt)
|
||||
|
||||
def on_disconnect(client, userdata, rc):
|
||||
logger.info("disconnecting reason " +str(rc))
|
||||
client.connected_flag=False
|
||||
client.disconnect_flag=True
|
||||
|
||||
def on_publish(self, userdata, mid):
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# Read Config
|
||||
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_SUB3.yml")
|
||||
cfg = utils.Read_SUB_Config(FilePath)
|
||||
client = utils.MQTTClient(cfg.MQTTClientNameSub)
|
||||
|
||||
# set log
|
||||
log_format = "%(asctime)s - %(levelname)s - %(message)s"
|
||||
formatter = logging.Formatter(log_format)
|
||||
|
||||
stream_handler = logging.StreamHandler()
|
||||
stream_handler.setFormatter(formatter)
|
||||
stream_handler.setLevel(logging.DEBUG)
|
||||
|
||||
|
||||
file_handler = logging.FileHandler(cfg.logFileName)
|
||||
file_handler.setFormatter(formatter)
|
||||
file_handler.setLevel(logging.INFO)
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
logger.setLevel(logging.DEBUG)
|
||||
logger.addHandler(file_handler)
|
||||
logger.addHandler(stream_handler)
|
||||
|
||||
logger.info(cfg)
|
||||
|
||||
# Mqtt
|
||||
client = utils.MQTTClient(cfg.MQTTClientNameSub)
|
||||
client.on_connect = on_connect
|
||||
client.on_publish = on_publish
|
||||
client.on_disconnect = on_disconnect
|
||||
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
|
||||
|
||||
|
||||
# Ros
|
||||
rospy.init_node(cfg.ROSClientNameSub)
|
||||
# initialize
|
||||
init_dataFormat(cfg)
|
||||
|
||||
try:
|
||||
client.loop_start()
|
||||
rospy.spin()
|
||||
except BaseException as error:
|
||||
client.loop_stop()
|
||||
client.disconnect()
|
||||
logger.info("End of program")
|
||||
sys.exit(0)
|
||||
@ -1,22 +1,22 @@
|
||||
MQTT:
|
||||
msg_format: Json
|
||||
MQTTClientNamePub: Drone380Pub
|
||||
MQTTClientNamePub: Drone650Pub
|
||||
host: 140.120.31.133
|
||||
port: 1883
|
||||
keepalive: 60
|
||||
willTopic: CheckDoneConnect
|
||||
willTopicQOS: 1
|
||||
lwt: Dron550 Gone Offline
|
||||
lwt: Dron650 Gone Offline
|
||||
willRetain: False
|
||||
# Mqtt topic
|
||||
Flight_Information_topicToMqtt: Drone380/Flight_Information
|
||||
Fly_Formation_topicToMqtt: Drone380/Formation
|
||||
Flight_Information_topicToMqtt: Drone650/Flight_Information
|
||||
Fly_Formation_topicToMqtt: Drone650/Formation
|
||||
# Change formate qos
|
||||
Fly_Formation_topicToMqtt_QOS: 2
|
||||
#ROS
|
||||
ROS:
|
||||
ROSClientNamePub: Drone380Pub
|
||||
ROStopicName_Flight_Information: Flight_Information_reciver
|
||||
ROSClientNamePub: Drone650Pub
|
||||
ROStopicName_Flight_Information: Dron650_Flight_Information_reciver
|
||||
ROStopicName_Fly_Formation: Fly_Formation_reciver
|
||||
LOG:
|
||||
logFileName: pub.log
|
||||
|
||||
@ -0,0 +1,22 @@
|
||||
MQTT:
|
||||
msg_format: Json
|
||||
MQTTClientNamePub: Drone550Pub
|
||||
host: 140.120.31.133
|
||||
port: 1883
|
||||
keepalive: 60
|
||||
willTopic: CheckDoneConnect
|
||||
willTopicQOS: 1
|
||||
lwt: Drone550 Gone Offline
|
||||
willRetain: False
|
||||
# Mqtt topic
|
||||
Flight_Information_topicToMqtt: Drone550/Flight_Information
|
||||
Fly_Formation_topicToMqtt: Drone550/Formation
|
||||
# Change formate qos
|
||||
Fly_Formation_topicToMqtt_QOS: 2
|
||||
#ROS
|
||||
ROS:
|
||||
ROSClientNamePub: Drone550Pub
|
||||
ROStopicName_Flight_Information: Dron550_Flight_Information_reciver
|
||||
ROStopicName_Fly_Formation: Fly_Formation_reciver
|
||||
LOG:
|
||||
logFileName: pub.log
|
||||
@ -0,0 +1,22 @@
|
||||
MQTT:
|
||||
msg_format: Json
|
||||
MQTTClientNamePub: Drone380Pub
|
||||
host: 140.120.31.133
|
||||
port: 1883
|
||||
keepalive: 60
|
||||
willTopic: CheckDoneConnect
|
||||
willTopicQOS: 1
|
||||
lwt: Dron380 Gone Offline
|
||||
willRetain: False
|
||||
# Mqtt topic
|
||||
Flight_Information_topicToMqtt: Drone380/Flight_Information
|
||||
Fly_Formation_topicToMqtt: Drone380/Formation
|
||||
# Change formate qos
|
||||
Fly_Formation_topicToMqtt_QOS: 2
|
||||
#ROS
|
||||
ROS:
|
||||
ROSClientNamePub: Drone380Pub
|
||||
ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
|
||||
ROStopicName_Fly_Formation: Fly_Formation_reciver
|
||||
LOG:
|
||||
logFileName: pub.log
|
||||
@ -0,0 +1,30 @@
|
||||
MQTT:
|
||||
msg_format: Json
|
||||
MQTTClientNameSub: Drone650Sub
|
||||
host: 140.120.31.133
|
||||
port: 1883
|
||||
keepalive: 60
|
||||
willTopic: CheckDoneConnect
|
||||
willTopicQOS: 1
|
||||
lwt: Dron550 Gone Offline
|
||||
willRetain: False
|
||||
# Mqtt topic
|
||||
Fly_Formation_topicToMqtt: Drone650/Formation
|
||||
Drone550_Flight_Information_topicToMqtt: Drone550/Flight_Information
|
||||
|
||||
Drone380_Flight_Information_topicToMqtt: Drone380/Flight_Information
|
||||
|
||||
Drone650_Flight_Information_topicToMqtt: Drone650/Flight_Information
|
||||
|
||||
# Change formate qos
|
||||
Fly_Formation_topicToMqtt_QOS: 2
|
||||
ROS:
|
||||
ROSClientNameSub: Drone650Sub
|
||||
ROStopicName_Fly_Formation: Fly_Formation_reciver
|
||||
Dron550_ROStopicName_Flight_Information: Dron550_Flight_Information_reciver
|
||||
|
||||
Dron380_ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
|
||||
|
||||
Dron650_ROStopicName_Flight_Information: Dron650_Flight_Information_reciver
|
||||
LOG:
|
||||
logFileName: sub.log
|
||||
@ -0,0 +1,30 @@
|
||||
MQTT:
|
||||
msg_format: Json
|
||||
MQTTClientNameSub: Drone380Sub
|
||||
host: 140.120.31.133
|
||||
port: 1883
|
||||
keepalive: 60
|
||||
willTopic: CheckDoneConnect
|
||||
willTopicQOS: 1
|
||||
lwt: Dron550 Gone Offline
|
||||
willRetain: False
|
||||
# Mqtt topic
|
||||
Fly_Formation_topicToMqtt: Drone380/Formation
|
||||
Drone550_Flight_Information_topicToMqtt: Drone550/Flight_Information
|
||||
|
||||
Drone380_Flight_Information_topicToMqtt: Drone380/Flight_Information
|
||||
|
||||
Drone650_Flight_Information_topicToMqtt: Drone650/Flight_Information
|
||||
|
||||
# Change formate qos
|
||||
Fly_Formation_topicToMqtt_QOS: 2
|
||||
ROS:
|
||||
ROSClientNameSub: Drone380Sub
|
||||
ROStopicName_Fly_Formation: Fly_Formation_reciver
|
||||
Dron550_ROStopicName_Flight_Information: Dron550_Flight_Information_reciver
|
||||
|
||||
Dron380_ROStopicName_Flight_Information: Dron380_Flight_Information_reciver
|
||||
|
||||
Dron650_ROStopicName_Flight_Information: Dron650_Flight_Information_reciver
|
||||
LOG:
|
||||
logFileName: sub.log
|
||||
Loading…
Reference in New Issue