From 3d9c574ed8b1b49e3ec17e06fd6a0f970193f1d6 Mon Sep 17 00:00:00 2001 From: RangeOfGlitching Date: Fri, 10 Feb 2023 03:19:01 +0800 Subject: [PATCH] add mqttCmd --- class_model/src/mqttCmdMain.py | 77 +++++++++++++++++++ class_model/src/mqttPubMain.py | 2 +- class_model/src/mqttSubMain.py | 8 +- class_model/src/tower.py | 2 +- class_model/src/utils/mqttConfig_CMD.yml | 15 ++++ .../utils/protoJson_mqtt_cmd_data_to_ros.py | 32 ++++++++ class_model/src/utils/readConfig.py | 31 +++++++- 7 files changed, 158 insertions(+), 9 deletions(-) create mode 100644 class_model/src/mqttCmdMain.py create mode 100644 class_model/src/utils/mqttConfig_CMD.yml create mode 100644 class_model/src/utils/protoJson_mqtt_cmd_data_to_ros.py diff --git a/class_model/src/mqttCmdMain.py b/class_model/src/mqttCmdMain.py new file mode 100644 index 0000000..cf068d2 --- /dev/null +++ b/class_model/src/mqttCmdMain.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python3 +#coding:utf-8 +import paho.mqtt.client as mqtt +import os +import sys +import time +from utils.readConfig import Read_CMD_Config +from utils.basicMqtt import MQTTClient +from utils.protoJson_mqtt_cmd_data_to_ros import Json_msg_to_ros +import logging + +from std_msgs.msg import String +import rospy + +def init_dataFormat(cfg:Read_CMD_Config): + if cfg.msg_format == "Json": + Json_msg_to_ros.rate = rospy.Rate(10) + Json_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Cmd_Broadcast_Receiver,String,queue_size=10) + client.on_message = Json_msg_to_ros.on_message + Json_msg_to_ros.Cmd_Broadcast_topicToMqtt = cfg.Cmd_Broadcast_topicToMqtt + elif cfg.msg_format == "Proto": + pass + else: + logging.debug("msg_format not found") + +def on_connect(self, userdata, flags, rc): + logger.info("Connected with result code " + str(rc)) + client.subscribe(cfg.Cmd_Broadcast_topicToMqtt) + +def on_disconnect(client, userdata, rc): + logger.info("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_CMD.yml") + cfg = Read_CMD_Config(FilePath) + client = MQTTClient(cfg.MQTTClientNameCmd) + + # 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("__CMD__") + logger.setLevel(logging.DEBUG) + logger.addHandler(file_handler) + logger.addHandler(stream_handler) + logger.info(cfg) + + # Mqtt + client.on_connect = on_connect + client.on_disconnect = on_disconnect + client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) + client.loop_start() + + # Ros + rospy.init_node(cfg.ROSClientNameCmd) + + # init data format + init_dataFormat(cfg) + + try: + rospy.spin() + except BaseException as error: + client.disconnect() + logger.info("End of program") + sys.exit() + \ No newline at end of file diff --git a/class_model/src/mqttPubMain.py b/class_model/src/mqttPubMain.py index 824a9b5..a40a234 100755 --- a/class_model/src/mqttPubMain.py +++ b/class_model/src/mqttPubMain.py @@ -96,7 +96,7 @@ if __name__ == '__main__': try: rospy.spin() - except Exception as e: + except BaseException as error: client.disconnect() logger.info("End of program") sys.exit() diff --git a/class_model/src/mqttSubMain.py b/class_model/src/mqttSubMain.py index dd51a04..a76f56b 100755 --- a/class_model/src/mqttSubMain.py +++ b/class_model/src/mqttSubMain.py @@ -34,16 +34,16 @@ def init_dataFormat(cfg:utils.Read_SUB_Config): utils.Json_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt utils.Json_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt else: - logging.debug("msg_format not found") + logger.debug("msg_format not found") def on_connect(self, userdata, flags, rc): - logging.info("Connected with result code " + str(rc)) + logger.info("Connected with result code " + str(rc)) client.subscribe(cfg.Flight_Information_topicToMqtt) client.subscribe(cfg.Fly_Formation_topicToMqtt) def on_disconnect(client, userdata, rc): - # logging.info("disconnecting reason " +str(rc)) + # logger.info("disconnecting reason " +str(rc)) client.connected_flag=False client.disconnect_flag=True @@ -93,7 +93,7 @@ if __name__ == '__main__': try: client.loop_start() rospy.spin() - except Exception as e: + except BaseException as error: client.loop_stop() client.disconnect() logger.info("End of program") diff --git a/class_model/src/tower.py b/class_model/src/tower.py index 254f93d..e862289 100755 --- a/class_model/src/tower.py +++ b/class_model/src/tower.py @@ -35,7 +35,7 @@ def on_publish(self, userdata, mid): connect_flag = False -mqtt_config = {"host": "140.120.31.133", "port": 1883, "topic": "cmd/broadcast", "name": "Tower"} +mqtt_config = {"host": "192.168.50.118", "port": 1883, "topic": "cmd/broadcast", "name": "Tower"} client = initialise_clients(mqtt_config["name"]) client.on_publish = on_publish client.on_connect = on_connect diff --git a/class_model/src/utils/mqttConfig_CMD.yml b/class_model/src/utils/mqttConfig_CMD.yml new file mode 100644 index 0000000..4bc2e12 --- /dev/null +++ b/class_model/src/utils/mqttConfig_CMD.yml @@ -0,0 +1,15 @@ +MQTT: + msg_format: Json + MQTTClientNameCmd: Drone550Cmd + host: 192.168.50.118 + port: 1883 + keepalive: 600 + # Mqtt topic + Cmd_Broadcast_topicToMqtt: cmd/broadcast + Cmd_Broadcast_topicToMqtt_QOS: 2 + #ROS +ROS: + ROSClientNameCmd: Drone550_Cmd + ROStopicName_Cmd_Broadcast_Receiver: cmd_receiver +LOG: + logFileName: cmd.log diff --git a/class_model/src/utils/protoJson_mqtt_cmd_data_to_ros.py b/class_model/src/utils/protoJson_mqtt_cmd_data_to_ros.py new file mode 100644 index 0000000..8b1ea99 --- /dev/null +++ b/class_model/src/utils/protoJson_mqtt_cmd_data_to_ros.py @@ -0,0 +1,32 @@ +import orjson +import time + +import logging + +# TODO: use native ros type instead of json or str +logger = logging.getLogger("__CMD__") + +class Proto_msg_to_ros: + pass + +class Json_msg_to_ros: + rate = None + # Ros publisher + publisher_Cmd_Broadcast = None + + Cmd_Broadcast_topicToMqtt = None + + + @classmethod + def ros_pub(cls, dataJson): + if dataJson.topic == cls.Cmd_Broadcast_topicToMqtt: + logger.debug(dataJson.payload.decode("UTF-8")) + cls.publisher_Cmd_Broadcast.publish(dataJson.payload.decode("UTF-8")) + cls.rate.sleep() + else: + logger.info("topic not found") + + + @staticmethod + def on_message(client, userdata, msg): + Json_msg_to_ros.ros_pub(msg) \ No newline at end of file diff --git a/class_model/src/utils/readConfig.py b/class_model/src/utils/readConfig.py index 683b5e3..8df612c 100644 --- a/class_model/src/utils/readConfig.py +++ b/class_model/src/utils/readConfig.py @@ -97,11 +97,36 @@ class Read_SUB_Config(Config): def __str__(self): return super().__str__() +class Read_CMD_Config(Config): + def setAttribute(self): + super().setAttribute() + def __init__(self, inFileName): + super().__init__(inFileName) + self.options = { + self.sectionNames[0]:{ + "msg_format": (str,True), + "MQTTClientNameCmd": (str,True), + "host": (str,True), + "port": (int,True), + "keepalive": (int,True), + "Cmd_Broadcast_topicToMqtt": (str,True), + "Cmd_Direct_topicToMqtt": (str,False), + "Cmd_Broadcast_topicToMqtt_QOS":(int,True), + "Cmd_Direct_topicToMqtt_QOS":(int,False)}, + self.sectionNames[1]:{ + "ROSClientNameCmd": (str,True), + "ROStopicName_Cmd_Broadcast_Receiver": (str,True), + "ROStopicName_Cmd_Direct_Receiver": (str,False)}, + self.sectionNames[2]:{ + "logFileName":(str,False)}} + self.setAttribute() + + def __str__(self): + return super().__str__() if __name__ == "__main__": - cfg=SUB_Config("mqttConfig_PUB.yml") - cfg.setAttribute() - print(cfg.msg_format) + cfg=Read_CMD_Config("mqttConfig_CMD.yml") + print(cfg)