From 58a2f3e7c1689ca7b78943989967b1ee7eafe6d4 Mon Sep 17 00:00:00 2001 From: RangeOfGlitching Date: Thu, 2 Mar 2023 23:43:38 +0800 Subject: [PATCH] sub all 3 drone --- class_model/src/mqttSubMain.py | 15 ++++++++---- class_model/src/utils/mqttConfig_SUB.yml | 13 +++++++++-- .../utils/protoJson_mqtt_pub_data_to_ros.py | 23 +++++++++++++++---- 3 files changed, 40 insertions(+), 11 deletions(-) diff --git a/class_model/src/mqttSubMain.py b/class_model/src/mqttSubMain.py index 2e78760..eb8e697 100755 --- a/class_model/src/mqttSubMain.py +++ b/class_model/src/mqttSubMain.py @@ -27,12 +27,17 @@ def init_dataFormat(cfg:utils.Read_SUB_Config): 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_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10) utils.Json_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10) - - client.message_callback_add(cfg.Flight_Information_topicToMqtt, utils.Json_msg_to_ros.on_message_Flight_Information) + 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) utils.Json_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt utils.Json_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt @@ -42,8 +47,10 @@ def init_dataFormat(cfg:utils.Read_SUB_Config): def on_connect(self, userdata, flags, rc): logger.info("Connected with result code " + str(rc)) - client.subscribe(cfg.Flight_Information_topicToMqtt) 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)) diff --git a/class_model/src/utils/mqttConfig_SUB.yml b/class_model/src/utils/mqttConfig_SUB.yml index a19bf8b..8e6adc7 100644 --- a/class_model/src/utils/mqttConfig_SUB.yml +++ b/class_model/src/utils/mqttConfig_SUB.yml @@ -9,13 +9,22 @@ MQTT: lwt: Dron550 Gone Offline willRetain: False # Mqtt topic - Flight_Information_topicToMqtt: Drone550/Flight_Information Fly_Formation_topicToMqtt: Drone550/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: Drone550Sub - ROStopicName_Flight_Information: Flight_Information_reciver 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 diff --git a/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py b/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py index 91e116b..22ad2cf 100644 --- a/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py +++ b/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py @@ -39,18 +39,31 @@ class Proto_msg_to_ros: class Json_msg_to_ros: rate = None #Ros publisher - publisher_Flight_Information = None publisher_Fly_Formation = None + Drone550_publisher_Flight_Information = None + Drone380_publisher_Flight_Information = None + Drone650_publisher_Flight_Information = None #Mqtt topic: check data from which topic Flight_Information_topicToMqtt = None Fly_Formation_topicToMqtt = None + @classmethod - def on_message_Flight_Information(cls, client, userdata, msg): - cls.publisher_Flight_Information.publish(msg.payload.decode("UTF-8")) + def on_message_Fly_Formation(cls, client, userdata, msg): + cls.publisher_Fly_Formation.publish(msg.payload.decode("UTF-8")) + cls.rate.sleep() + + @classmethod + def Drone550_on_message_Flight_Information(cls, client, userdata, msg): + cls.Drone550_publisher_Flight_Information.publish(msg.payload.decode("UTF-8")) + cls.rate.sleep() + + @classmethod + def Drone380_on_message_Flight_Information(cls, client, userdata, msg): + cls.Drone380_publisher_Flight_Information.publish(msg.payload.decode("UTF-8")) cls.rate.sleep() @classmethod - def on_message_Fly_Formation(cls, client, userdata, msg): - cls.publisher_Fly_Formation.publish(msg.payload.decode("UTF-8")) + def Drone650_on_message_Flight_Information(cls, client, userdata, msg): + cls.Drone650_publisher_Flight_Information.publish(msg.payload.decode("UTF-8")) cls.rate.sleep() \ No newline at end of file