diff --git a/class_model/src/main.cpp b/class_model/src/main.cpp index 02f6373..a38094e 100755 --- a/class_model/src/main.cpp +++ b/class_model/src/main.cpp @@ -36,7 +36,7 @@ int main(int argc, char **argv) { return 0; } -////////////////////////////////////////////////////// + /* ThreadGPSClass gps_object; diff --git a/class_model/src/mqttPubMain.py b/class_model/src/mqttPubMain.py old mode 100644 new mode 100755 index c2e2638..eaeafd4 --- a/class_model/src/mqttPubMain.py +++ b/class_model/src/mqttPubMain.py @@ -18,7 +18,7 @@ from sensor_msgs.msg import NavSatFix from sensor_msgs.msg import Imu from sensor_msgs.msg import Range from geometry_msgs.msg import Vector3 - +''' # test proto import random @@ -35,11 +35,13 @@ class fake_hdg(): class state(): def __init__(self): self.mode = "test" +''' 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() @@ -47,11 +49,15 @@ def init_dataFormat(cfg): 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 +68,16 @@ def on_connect(self, userdata, flags, rc): print("Connected with result code " + str(rc)) def on_publish(self, userdata, mid): - pass - + print("pub ok") 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,12 +87,23 @@ 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() while True: #test proto + gps.latitude = random.uniform(0, 10) gps.longitude = random.uniform(0, 10) gps.altitude = random.uniform(0, 10) @@ -100,12 +117,4 @@ if __name__ == '__main__': # 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/class_model/src/mqttSubMain.py b/class_model/src/mqttSubMain.py old mode 100644 new mode 100755 index 2dfdec5..ede39a2 --- a/class_model/src/mqttSubMain.py +++ b/class_model/src/mqttSubMain.py @@ -12,27 +12,30 @@ from std_msgs.msg import String def init_dataFormat(cfg): if cfg.msg_format == "Proto": - # 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.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_msg = flight_information_pb2.flight_information_message() utils.Proto_msg_to_ros.flyformatioln_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)) @@ -45,22 +48,23 @@ 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_message = utils.Json_msg_to_ros.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) + rospy.init_node(cfg.ROSClientNameSub) # initialize - # init_dataFormat(cfg) - + init_dataFormat(cfg) - # rospy.spin() \ No newline at end of file + client.loop_start() + rospy.spin() \ No newline at end of file diff --git a/class_model/src/utils/mqttConfig.yml b/class_model/src/utils/mqttConfig.yml index f30adcf..86cc5d3 100644 --- a/class_model/src/utils/mqttConfig.yml +++ b/class_model/src/utils/mqttConfig.yml @@ -1,8 +1,8 @@ MQTT_ROS: - msg_format: Proto + msg_format: Json MQTTClientNamePub: Drone650Pub MQTTClientNameSub: Drone650Sub - host: 192.168.50.117 + host: 192.168.50.27 port: 1883 keepalive: 60 willTopic: CheckDoneConnect @@ -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/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py b/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py index 2eb6cc4..8b6c210 100644 --- a/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py +++ b/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py @@ -27,16 +27,20 @@ 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.rate.sleep() + 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.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 +55,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/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py b/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py index 1661f0d..bde5878 100644 --- a/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py +++ b/class_model/src/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):