add mqttCmd

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()
+     

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()

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")

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

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

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)

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)