SUB PUB json and proto

class_model/src/utils/__init__.py

@@ -0,0 +1,6 @@
+from utils.protoJson_mqtt_pub_data_to_ros import Proto_msg_to_ros
+from utils.protoJson_mqtt_pub_data_to_ros import Json_msg_to_ros
+from utils.protoJson_mqtt_sub_data_from_ros import Proto_msg_from_ros
+from utils.protoJson_mqtt_sub_data_from_ros import Json_msg_from_ros
+from utils.readConfig import MQTT_ROS_Config
+from utils.basicMqtt import MQTTClient

class_model/src/utils/basicMqtt.py

@@ -0,0 +1,17 @@
+import paho.mqtt.client as mqtt
+import time
+class MQTTClient(mqtt.Client):
+   
+   def __init__(self,cname,**kwargs):
+      super().__init__(cname,**kwargs)
+      self.last_pub_time=time.time()
+      self.topic_ack=[]
+      self.run_flag=True
+      self.subscribe_flag=False
+      self.bad_connection_flag=False
+      self.connected_flag=True
+      self.disconnect_flag=False
+      self.disconnect_time=0.0
+      self.pub_msg_count=0
+      self.devices=[]
+

class_model/src/utils/mqttConfig.yml

@@ -0,0 +1,21 @@
+MQTT_ROS:
+  msg_format: Proto
+  MQTTClientNamePub: Drone650Pub
+  MQTTClientNameSub: Drone650Sub
+  host: 192.168.50.117
+  port: 1883
+  keepalive: 60
+  willTopic: CheckDoneConnect
+  willTopicQOS: 1
+  lwt: Drone650 Gone Offline
+  willRetain: False
+  # Mqtt topic
+  Flight_Information_topicToMqtt: drone/leader/Flight_Information
+  Fly_Formation_topicToMqtt: drone/leader/Formation
+  # Change formate qos
+  Fly_Formation_topicToMqtt_QOS: 2
+  #ROS
+  ROSClientNamePub: Drone650Pub
+  ROSClientNameSub: Drone650Sub
+  ROStopicName_Flight_Information: Flight_Information_reciver
+  ROStopicName_Fly_Formation: Fly_Formation_reciver

class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py

@@ -0,0 +1,60 @@
+import orjson
+import time
+
+import proto.flight_information_pb2 as flight_information_pb2
+import proto.flyformatioln_pb2 as flyformatioln_pb2
+import google.protobuf.json_format as json_format
+
+class Proto_msg_to_ros:
+    #Protobuf
+    flight_information_msg = None
+    fly_formation_msg = None # delcare in function 
+
+    #Ros publisher
+    rate = None
+    publisher_Flight_Information = None
+    publisher_Fly_Formation = None
+
+    #Mqtt topic: check data from which topic
+    Flight_Information_topicToMqtt = None
+    Fly_Formation_topicToMqtt = None
+
+    #Proto
+
+    @classmethod
+    def ros_pub(cls, dataProto):
+        if dataProto.topic == cls.Flight_Information_topicToMqtt:
+            proto_msg = cls.flight_information_msg.FromString(dataProto.payload)
+            protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True)
+            print(protoTOJson_msg)
+            # cls.publisher_flight_information.publish(protoTOJson_msg)
+            # cls.rate.sleep()
+        elif dataProto.topic == cls.Fly_Formation_topicToMqtt:
+            proto_msg = cls.fly_formation_msg.FromString(dataProto.payload)
+            protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True)
+            cls.publisher_Formation.publish(protoTOJson_msg)
+            cls.rate.sleep()
+        else:
+            print("topic not found")
+
+
+
+class Json_msg_to_ros:
+    rate = None
+    #Ros publisher
+    publisher_Flight_Information = None
+    publisher_Fly_Formation = None
+    #Mqtt topic: check data from which topic
+    Flight_Information_topicToMqtt = None
+    Fly_Formation_topicToMqtt = None
+
+    @classmethod
+    def ros_pub(cls, dataJson):
+        if dataJson.topic == cls.Flight_Information_topicToMqtt:
+            cls.publisher_flight_information.publish(dataJson.payload.decode('utf-8'))
+            cls.rate.sleep()
+        elif dataJson.topic == cls.Fly_Formation_topicToMqtt:
+            cls.publisher_Fly_Formation.publish(dataJson.payload.decode('utf-8'))
+            cls.rate.sleep()
+        else:
+            print("topic not found")

class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py

@@ -0,0 +1,113 @@
+import orjson
+import time
+
+class Proto_msg_from_ros:
+    #Protobuf
+    flight_information_msg = None
+    fly_formation_msg = None # delcare in function 
+    #Mqtt
+    client = None
+    Flight_Information_topicToMqtt = None
+    Fly_Formation_topicToMqtt = None
+    Fly_Formation_topicToMqtt_QOS = None
+
+    @classmethod
+    def callBack_gps(cls, GPS):
+        # flight_information_msg = flight_information_pb2.flight_information_message()
+        cls.flight_information_msg.gps.LAT = GPS.latitude
+        cls.flight_information_msg.gps.LON = GPS.longitude
+        cls.flight_information_msg.gps.ALT = GPS.altitude
+        # print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
+        # print(flight_information_msg)
+
+    @classmethod
+    def callBack_compass_hdg(cls, Compass):
+        cls.flight_information_msg.heading = Compass.data
+        
+        flightInformationMsg = cls.flight_information_msg.SerializeToString()
+        cls.mqtt_Pub(message=flightInformationMsg, topics=cls.Flight_Information_topicToMqtt)
+    
+
+    # TODO: Formation.velocity check
+    @classmethod
+    def callBack_fly_formation(cls, Formation):
+        cls.fly_formation_msg.velocity = Formation.velocity
+        # cls.fly_formation_msg.fly_formation = flyformatioln_pb2.FLY_FORMATION_v
+        cls.fly_formation_msg.fly_formation = 2
+        flyFormationMsg = cls.fly_formation_msg.SerializeToString()
+        cls.mqtt_Pub(message=flyFormationMsg, topics=cls.Fly_Formation_topicToMqtt, qos=cls.Fly_Formation_topicToMqtt_QOS)
+
+
+
+    @classmethod
+     # publish a message
+    def mqtt_Pub(cls, message:bytes, topics:str, waitForAck:bool=False, qos:int=0)->None:
+        mid = cls.client.publish(topics, message, qos)[1]
+        # print(f"just published {message} to topic")
+        if waitForAck:
+            while mid not in cls.client.topic_ack:
+                print("wait for ack")
+                time.sleep(0.25)
+            cls.client.topic_ack.remove(mid)
+
+
+
+
+class Json_msg_from_ros:
+    data = {}
+    client = None
+    #Mqtt
+    Flight_Information_topicToMqtt = None
+    Fly_Formation_topicToMqtt = None
+    Fly_Formation_topicToMqtt_QOS = None
+
+    @classmethod 
+    def callBack_imu(cls, IMU):
+        gyro_x=str(IMU.linear_acceleration.x)
+        gyro_y=str(IMU.linear_acceleration.y)
+        gyro_z=str(IMU.linear_acceleration.z)
+
+        accel_x=str(IMU.angular_velocity.x)
+        accel_y=str(IMU.angular_velocity.y)
+        accel_z=str(IMU.angular_velocity.z)
+
+        dataImuUpdate = {"gyro_x": gyro_x, "gyro_y": gyro_y,"gyro_z":gyro_z, "accel_x": accel_x, "accel_y": accel_y, "accel_z": accel_z}
+        cls.data.update(dataImuUpdate)
+        # print ('gyro_x:'+gyro_x+'\n'+'gyro_y:'+gyro_y+'\n'+'gyro_z:'+gyro_z +'\n')
+        print ('accel_x:'+accel_x+'\n'+'accel_y:'+accel_y+'\n'+'accel_z:'+accel_z +'\n')
+    
+    @classmethod 
+    def callBack_gps(cls, GPS):
+        lat=int(GPS.latitude*10000000)   #change the scale to centimeters
+        lon=int(GPS.longitude*10000000)
+        alt=int(GPS.altitude*100)
+        dataGpsUpdate = {"lat": lat, "lon": lon, "alt": alt}
+        cls.data.update(dataGpsUpdate)
+        # dataJsonFormate = orjson.dumps(data)
+        # mqtt_Pub(message=dataJsonFormate)
+        # print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
+
+    @classmethod 
+    def callBack_state(cls, state):
+        mode = state.mode
+        dataGpsUpdate = {"mode": mode}
+        cls.data.update(dataGpsUpdate)
+
+    @classmethod 
+    def callBack_compass_hdg(cls, Compass):
+        heading = int(Compass.data*100)
+        dataGpsUpdate = {"heading": heading}
+        cls.data.update(dataGpsUpdate)
+        dataJsonFormate = orjson.dumps(cls.data).decode("UTF-8")
+        cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Flight_Information_topicToMqtt)
+
+        
+    @classmethod
+    def mqtt_Pub(cls, message:str, topics:str, waitForAck:bool=False, qos:int=0):
+        mid = cls.client.publish(topics, message, qos)[1]
+        # print(f"just published {message} to topic")
+        if waitForAck:
+            while mid not in cls.client.topic_ack:
+                print("wait for ack")
+                time.sleep(0.25)
+            cls.client.topic_ack.remove(mid)

class_model/src/utils/readConfig.py

@@ -0,0 +1,65 @@
+import yaml
+
+
+class MQTT_ROS_Config:
+    """
+    MQTT_ROS_Config configuration class
+    """
+    # class variables
+    sectionName='MQTT_ROS'
+    options={
+            'msg_format': (str,True),
+            'MQTTClientNamePub': (str,True),
+            'MQTTClientNameSub': (str,True),
+            'host': (str,True),
+            'port': (int,True),
+            'keepalive': (int,True),
+            'willTopic':(str,True),
+            'lwt':(str, True),
+            'willRetain':(bool,False),
+            'willTopicQOS':(int,True),
+            'Flight_Information_topicToMqtt': (str,False),
+            'Fly_Formation_topicToMqtt': (str,False),
+            'Fly_Formation_topicToMqtt_QOS':(int,False),
+            'ROSClientNamePub': (str,False),
+            'ROSClientNameSub': (str,False),
+            'ROStopicName_Flight_Information': (str,False),
+            'ROStopicName_Fly_Formation': (str,False)}
+     
+    #constructor
+    def __init__(self, inFileName):
+        #read YAML config and get EV3 section
+        infile=open(inFileName,'r')
+        ymlcfg=yaml.safe_load(infile)
+        infile.close()
+        eccfg=ymlcfg.get(self.sectionName,None)
+        if eccfg is None: raise Exception('Missing {} section in cfg file'.format(self.sectionName))
+         
+        #iterate over options
+        for opt in self.options:
+            if opt in eccfg:
+                optval=eccfg[opt]
+ 
+                #verify parameter type
+                if type(optval) != self.options[opt][0]:
+                    raise Exception('Parameter "{}" has wrong type'.format(opt))
+                 
+                #create attributes on the fly
+                setattr(self,opt,optval)
+            else:
+                if self.options[opt][1]:
+                    raise Exception('Missing mandatory parameter "{}"'.format(opt))
+                else:
+                    setattr(self,opt,None)
+     
+    #string representation for class data    
+    def __str__(self):
+        return str(yaml.dump(self.__dict__,default_flow_style=False))
+
+   
+
+if __name__ == '__main__':
+    cfg=MQTT_ROS_Config("mqttConfig.yml")
+    print(cfg)
+
+