merge json and proto

class_model/src/mqttPubMain.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+#coding:utf-8
+import paho.mqtt.client as mqtt
+import os
+import sys
+import time
+import utils
+import proto.flight_information_pb2 as flight_information_pb2
+import proto.flyformatioln_pb2 as flyformatioln_pb2
+
+
+import rospy
+from std_msgs.msg import String
+from std_msgs.msg import Float64
+from std_msgs.msg import Header
+from mavros_msgs.srv import ParamGet
+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
+
+class fakeGps():
+    def __init__(self):
+        self.latitude = 0.0
+        self.longitude = 20
+        self.altitude = 30
+
+class fake_hdg():
+    def __init__(self):
+        self.data = 40
+# test json
+class state():
+    def __init__(self):
+        self.mode = "test"
+
+
+
+
+def init_dataFormat(cfg):
+    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.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
+    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
+    else:
+        print("msg_format not found")
+
+def on_message(client, userdata, msg):
+    pass
+
+def on_connect(self, userdata, flags, rc):
+    print("Connected with result code " + str(rc))
+
+def on_publish(self, userdata, mid):
+    pass
+
+
+if __name__ == '__main__':
+    # Read Config
+    cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml")
+    client = utils.MQTTClient(cfg.MQTTClientNamePub)
+
+    # Setting Config
+    init_dataFormat(cfg)
+
+    # Mqtt
+    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.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
+    client.loop_start()
+
+    #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)
+        hdg.data = random.uniform(0, 10)
+
+        utils.Proto_msg_from_ros.callBack_gps(gps)
+        utils.Proto_msg_from_ros.callBack_compass_hdg(hdg)
+        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()

class_model/src/mqttSubMain.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+#coding:utf-8
+import paho.mqtt.client as mqtt
+import os
+import sys
+import time
+import utils
+import proto.flight_information_pb2 as flight_information_pb2
+import proto.flyformatioln_pb2 as flyformatioln_pb2
+import rospy
+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.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.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
+    else:
+        print("msg_format not found")
+
+def on_message(client, userdata, msg):
+    utils.Proto_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)
+
+
+def on_publish(self, userdata, mid):
+    pass
+
+
+if __name__ == '__main__':
+    # Read Config
+    cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml")
+  
+   
+    # 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)
+
+
+    # rospy.spin()