modified: Stream/uav_proto_msg/utils/mqttConfig.yml

|_ change hivemq broker ip
	modified:   Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.py
	modified:   Stream/uav_proto_msg/utils/protoJson_mqtt_sub_data_from_ros.py
        |_ sub and pub should migrade to controlDrone
	|_ test it fine without ros

Stream/uav_proto_msg/mqttPubMain.py

@@ -31,6 +31,12 @@ class fakeGps():
 class fake_hdg():
     def __init__(self):
         self.data = 40
+
+class fake_formation():
+    def __init__(self):
+        self.velocity = 100
+        self.type = 3
+
 # test json
 class state():
     def __init__(self):
@@ -39,19 +45,25 @@ class state():
 
 
 
+
 def init_dataFormat(cfg):
+    ros_namespace="/drone1"
     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.fly_formation_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:
         print("msg_format not found")
 
@@ -62,16 +74,17 @@ def on_connect(self, userdata, flags, rc):
     print("Connected with result code " + str(rc))
 
 def on_publish(self, userdata, mid):
+    # print("pub ok")
     pass
 
-
 if __name__ == '__main__':
     # Read Config
-    cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml")
+    FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig.yml")
+    cfg = utils.MQTT_ROS_Config(FilePath)
     client = utils.MQTTClient(cfg.MQTTClientNamePub)
-
+    print(cfg)
     # Setting Config
-    init_dataFormat(cfg)
+    
 
     # Mqtt
     client.on_connect = on_connect
@@ -81,31 +94,35 @@ if __name__ == '__main__':
     client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
     client.loop_start()
 
+    # Ros
+    # rosClient = cfg.ROSClientNamePub
+    # rospy.init_node(rosClient)
+    init_dataFormat(cfg)
+    
+  
+
+    # rospy.spin()
+
     #test proto
     gps = fakeGps()
     hdg = fake_hdg()
     state = state()
+    formation = fake_formation()
     while True:
         #test proto
+
         gps.latitude = random.uniform(0, 10)
         gps.longitude = random.uniform(0, 10)
         gps.altitude = random.uniform(0, 10)
         hdg.data = random.uniform(0, 10)
+        formation.velocity = random.uniform(0, 10)
 
         utils.Proto_msg_from_ros.callBack_gps(gps)
         utils.Proto_msg_from_ros.callBack_compass_hdg(hdg)
+        utils.Proto_msg_from_ros.callBack_fly_formation(formation)
         time.sleep(0.25)
         #test json
         # utils.Json_msg_from_ros.callBack_gps(gps)
         # utils.Json_msg_from_ros.callBack_compass_hdg(hdg)
         # utils.Json_msg_from_ros.callBack_state(state)
         # time.sleep(1)
-
-    # Ros
-    # rosClient = cfg.ROSClientNamePub
-    # rospy.init_node(rosClient)
-    # subscriber = rospy.Subscriber('/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
-    # subscriber = rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
-
-
-    # rospy.spin()

Stream/uav_proto_msg/mqttSubMain.py

@@ -17,27 +17,30 @@ def init_dataFormat(cfg):
         # 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()
+        utils.Proto_msg_to_ros.fly_formation_msg = flyformatioln_pb2.fly_formation_message()
+        
+        client.on_message = utils.Proto_msg_to_ros.on_message
 
         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":
-        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_topicToMqtt = cfg.Flight_Information_topicToMqtt
-        utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
+        # 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.on_message = utils.Json_msg_to_ros.on_message
+        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:
         print("msg_format not found")
 
-def on_message(client, userdata, msg):
-    utils.Proto_msg_to_ros.ros_pub(msg)
+# def on_message(client, userdata, msg):
+#     utils.Json_msg_to_ros.ros_pub(msg)
 
 def on_connect(self, userdata, flags, rc):
     print("Connected with result code " + str(rc))
     client.subscribe(cfg.Flight_Information_topicToMqtt)
-
+    client.subscribe(cfg.Fly_Formation_topicToMqtt)
 
 def on_publish(self, userdata, mid):
     pass
@@ -45,22 +48,22 @@ def on_publish(self, userdata, mid):
 
 if __name__ == '__main__':
     # Read Config
-    cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml")
-  
+    FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig.yml")
+    # Read Config
+    cfg = utils.MQTT_ROS_Config(FilePath)
+    print(cfg)
    
     # Mqtt
-    init_dataFormat(cfg)
     client = utils.MQTTClient(cfg.MQTTClientNameSub)
     client.on_connect = on_connect
-    client.on_message = on_message
     client.on_publish = on_publish
     client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
-    client.loop_forever()
+    
 
     # Ros
     # rospy.init_node(cfg.ROSClientNameSub)
     # initialize
-    # init_dataFormat(cfg)
-
+    init_dataFormat(cfg)
 
+    client.loop_forever()
     # rospy.spin()

Stream/uav_proto_msg/utils/mqttConfig.yml

@@ -18,4 +18,4 @@ MQTT_ROS:
   ROSClientNamePub: Drone650Pub
   ROSClientNameSub: Drone650Sub
   ROStopicName_Flight_Information: Flight_Information_reciver
-  ROStopicName_Fly_Formation: Fly_Formation_reciver
+  ROStopicName_Fly_Formation: Fly_Formation_reciver

Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.py

@@ -5,6 +5,7 @@ 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
@@ -27,16 +28,22 @@ class Proto_msg_to_ros:
             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.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()
+            # protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True, use_integers_for_enums=True)
+            print(protoTOJson_msg)
+            # cls.publisher_Fly_Formation.publish(protoTOJson_msg)
+            # cls.rate.sleep()
         else:
             print("topic not found")
+            
 
+    @staticmethod
+    def on_message(client, userdata, msg):
+        Proto_msg_to_ros.ros_pub(msg)
 
 
 class Json_msg_to_ros:
@@ -51,10 +58,15 @@ class Json_msg_to_ros:
     @classmethod
     def ros_pub(cls, dataJson):
         if dataJson.topic == cls.Flight_Information_topicToMqtt:
-            cls.publisher_flight_information.publish(dataJson.payload.decode('utf-8'))
+            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")
+            print("topic not found")
+            
+
+    @staticmethod
+    def on_message(client, userdata, msg):
+        Json_msg_to_ros.ros_pub(msg)

Stream/uav_proto_msg/utils/protoJson_mqtt_sub_data_from_ros.py

@@ -18,7 +18,7 @@ class Proto_msg_from_ros:
         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)
+        # print(cls.flight_information_msg)
 
     @classmethod
     def callBack_compass_hdg(cls, Compass):
@@ -33,7 +33,7 @@ class Proto_msg_from_ros:
     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
+        cls.fly_formation_msg.fly_formation = Formation.type
         flyFormationMsg = cls.fly_formation_msg.SerializeToString()
         cls.mqtt_Pub(message=flyFormationMsg, topics=cls.Fly_Formation_topicToMqtt, qos=cls.Fly_Formation_topicToMqtt_QOS)