diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..fcabc20 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +__pycache__/ +*pyc \ No newline at end of file diff --git a/Stream/uav_proto_msg/mqttPubMain.py b/Stream/uav_proto_msg/mqttPubMain.py index c2e2638..774ea80 100644 --- a/Stream/uav_proto_msg/mqttPubMain.py +++ b/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() diff --git a/Stream/uav_proto_msg/mqttSubMain.py b/Stream/uav_proto_msg/mqttSubMain.py index 2dfdec5..5ca9a23 100644 --- a/Stream/uav_proto_msg/mqttSubMain.py +++ b/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() \ No newline at end of file diff --git a/Stream/uav_proto_msg/protomsg/__pycache__/protoMsg.cpython-310.pyc b/Stream/uav_proto_msg/protomsg/__pycache__/protoMsg.cpython-310.pyc deleted file mode 100644 index 9d7a7be..0000000 Binary files a/Stream/uav_proto_msg/protomsg/__pycache__/protoMsg.cpython-310.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/__pycache__/protoMsg.cpython-38.pyc b/Stream/uav_proto_msg/protomsg/__pycache__/protoMsg.cpython-38.pyc deleted file mode 100644 index 7f5037e..0000000 Binary files a/Stream/uav_proto_msg/protomsg/__pycache__/protoMsg.cpython-38.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/__pycache__/protoMsg.cpython-39.pyc b/Stream/uav_proto_msg/protomsg/__pycache__/protoMsg.cpython-39.pyc deleted file mode 100644 index c72bbd6..0000000 Binary files a/Stream/uav_proto_msg/protomsg/__pycache__/protoMsg.cpython-39.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/duration_pb2.cpython-310.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/duration_pb2.cpython-310.pyc deleted file mode 100644 index 70a4164..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/duration_pb2.cpython-310.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/duration_pb2.cpython-39.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/duration_pb2.cpython-39.pyc deleted file mode 100644 index 0053d3a..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/duration_pb2.cpython-39.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/flight_information_pb2.cpython-310.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/flight_information_pb2.cpython-310.pyc deleted file mode 100644 index 3b3da9a..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/flight_information_pb2.cpython-310.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/flight_information_pb2.cpython-39.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/flight_information_pb2.cpython-39.pyc deleted file mode 100644 index 7fafdf7..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/flight_information_pb2.cpython-39.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/flyformatioln_pb2.cpython-310.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/flyformatioln_pb2.cpython-310.pyc deleted file mode 100644 index dfcb462..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/flyformatioln_pb2.cpython-310.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/flyformatioln_pb2.cpython-39.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/flyformatioln_pb2.cpython-39.pyc deleted file mode 100644 index 76fd1ac..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/flyformatioln_pb2.cpython-39.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/flymode_pb2.cpython-310.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/flymode_pb2.cpython-310.pyc deleted file mode 100644 index 954d35c..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/flymode_pb2.cpython-310.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/flymode_pb2.cpython-39.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/flymode_pb2.cpython-39.pyc deleted file mode 100644 index 9f1e80d..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/flymode_pb2.cpython-39.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/imu_pb2.cpython-310.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/imu_pb2.cpython-310.pyc deleted file mode 100644 index a03d27d..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/imu_pb2.cpython-310.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/imu_pb2.cpython-39.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/imu_pb2.cpython-39.pyc deleted file mode 100644 index 64a99e8..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/imu_pb2.cpython-39.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/odom_pb2.cpython-310.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/odom_pb2.cpython-310.pyc deleted file mode 100644 index 0f1b1dc..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/odom_pb2.cpython-310.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/odom_pb2.cpython-39.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/odom_pb2.cpython-39.pyc deleted file mode 100644 index edadc0d..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/odom_pb2.cpython-39.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/timestamp_pb2.cpython-310.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/timestamp_pb2.cpython-310.pyc deleted file mode 100644 index ea4569f..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/timestamp_pb2.cpython-310.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/proto/__pycache__/timestamp_pb2.cpython-39.pyc b/Stream/uav_proto_msg/protomsg/proto/__pycache__/timestamp_pb2.cpython-39.pyc deleted file mode 100644 index c21c486..0000000 Binary files a/Stream/uav_proto_msg/protomsg/proto/__pycache__/timestamp_pb2.cpython-39.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/protomsg/protoMsg.pyc b/Stream/uav_proto_msg/protomsg/protoMsg.pyc deleted file mode 100644 index a4b8e56..0000000 Binary files a/Stream/uav_proto_msg/protomsg/protoMsg.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/utils/__init__.pyc b/Stream/uav_proto_msg/utils/__init__.pyc deleted file mode 100644 index da74d38..0000000 Binary files a/Stream/uav_proto_msg/utils/__init__.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/utils/__pycache__/__init__.cpython-310.pyc b/Stream/uav_proto_msg/utils/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index e72b28a..0000000 Binary files a/Stream/uav_proto_msg/utils/__pycache__/__init__.cpython-310.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/utils/__pycache__/basicMqtt.cpython-310.pyc b/Stream/uav_proto_msg/utils/__pycache__/basicMqtt.cpython-310.pyc deleted file mode 100644 index 4bfc01f..0000000 Binary files a/Stream/uav_proto_msg/utils/__pycache__/basicMqtt.cpython-310.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/utils/__pycache__/protoJson_mqtt_pub_data_to_ros.cpython-310.pyc b/Stream/uav_proto_msg/utils/__pycache__/protoJson_mqtt_pub_data_to_ros.cpython-310.pyc deleted file mode 100644 index 1c49bea..0000000 Binary files a/Stream/uav_proto_msg/utils/__pycache__/protoJson_mqtt_pub_data_to_ros.cpython-310.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/utils/__pycache__/protoJson_mqtt_sub_data_from_ros.cpython-310.pyc b/Stream/uav_proto_msg/utils/__pycache__/protoJson_mqtt_sub_data_from_ros.cpython-310.pyc deleted file mode 100644 index ac08e79..0000000 Binary files a/Stream/uav_proto_msg/utils/__pycache__/protoJson_mqtt_sub_data_from_ros.cpython-310.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/utils/__pycache__/readConfig.cpython-310.pyc b/Stream/uav_proto_msg/utils/__pycache__/readConfig.cpython-310.pyc deleted file mode 100644 index 0927063..0000000 Binary files a/Stream/uav_proto_msg/utils/__pycache__/readConfig.cpython-310.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/utils/mqttConfig.yml b/Stream/uav_proto_msg/utils/mqttConfig.yml index f30adcf..a992b2f 100644 --- a/Stream/uav_proto_msg/utils/mqttConfig.yml +++ b/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 \ No newline at end of file + ROStopicName_Fly_Formation: Fly_Formation_reciver diff --git a/Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.py b/Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.py index 2eb6cc4..c344171 100644 --- a/Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.py +++ b/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") \ No newline at end of file + print("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/Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.pyc b/Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.pyc deleted file mode 100644 index 7cc220c..0000000 Binary files a/Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.pyc and /dev/null differ diff --git a/Stream/uav_proto_msg/utils/protoJson_mqtt_sub_data_from_ros.py b/Stream/uav_proto_msg/utils/protoJson_mqtt_sub_data_from_ros.py index 1661f0d..5b143d0 100644 --- a/Stream/uav_proto_msg/utils/protoJson_mqtt_sub_data_from_ros.py +++ b/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)