proto and speed test

Stream/localRosData/local_mqtt_pub_data_to_ros.py

@@ -5,13 +5,13 @@ import ssl
 import rospy
 from std_msgs.msg import String
 import paho.mqtt.client as mqtt
-import json
+import orjson
 
 
 # Ros
 def ros_pub(dataJson):
     global publisher, rate
-    # data = json.loads(dataJson)
+    # data = orjson.loads(dataJson)
     publisher.publish(dataJson)     #將date字串發布到topic
     rate.sleep()
     # print(f"publish data {data}")
@@ -25,8 +25,7 @@ def initialise_clients(clientname):
 
 def on_connect(client, userdata, flags, rc):
     print("Connected with result code " + str(rc))
-    # client.subscribe(mqtt_config["topic"])
-    client.subscribe("data/pub")
+    client.subscribe(mqtt_config["topic"])
 
 
 def on_message(client, userdata, msg):

Stream/localRosData/local_mqtt_sub_data_from_ros.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 #coding:utf-8
 import sys
-import json
+import orjson
 import paho.mqtt.client as mqtt
 
 
@@ -43,7 +43,7 @@ def callBack_gps(GPS):
 def callBack_rng(RNG):
     dataRngUpdate = {"range": RNG.range}
     data.update(dataRngUpdate)
-    dataJsonFormate = json.dumps(data)
+    dataJsonFormate = orjson.dumps(data)
     # print ('range:'+range+'\n')
     print (dataJsonFormate)
     mqtt_Pub(message=dataJsonFormate)

Stream/uav_proto_msg/.idea/.gitignore

@@ -0,0 +1,3 @@
+# Default ignored files
+/shelf/
+/workspace.xml

Stream/uav_proto_msg/.idea/inspectionProfiles/Project_Default.xml

@@ -0,0 +1,21 @@
+<component name="InspectionProjectProfileManager">
+  <profile version="1.0">
+    <option name="myName" value="Project Default" />
+    <inspection_tool class="PyPep8NamingInspection" enabled="true" level="WEAK WARNING" enabled_by_default="true">
+      <option name="ignoredErrors">
+        <list>
+          <option value="N802" />
+          <option value="N803" />
+        </list>
+      </option>
+    </inspection_tool>
+    <inspection_tool class="PyUnresolvedReferencesInspection" enabled="true" level="WARNING" enabled_by_default="true">
+      <option name="ignoredIdentifiers">
+        <list>
+          <option value="cv2.imread" />
+          <option value="cv2.*" />
+        </list>
+      </option>
+    </inspection_tool>
+  </profile>
+</component>

Stream/uav_proto_msg/.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

Stream/uav_proto_msg/.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.10 (protocolBufferUdemy)" project-jdk-type="Python SDK" />
+</project>

Stream/uav_proto_msg/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/protocolBufferUdemy.iml" filepath="$PROJECT_DIR$/.idea/protocolBufferUdemy.iml" />
+    </modules>
+  </component>
+</project>

Stream/uav_proto_msg/.idea/protocolBufferUdemy.iml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>

Stream/uav_proto_msg/.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/proto/protobuf" vcs="Git" />
+  </component>
+</project>

Stream/uav_proto_msg/Makefile

@@ -0,0 +1,17 @@
+BIN_NAME = uav_proto_msg
+PROTO_DIR = proto
+
+
+
+run:	generate
+	${BIN} main.py
+
+generate:
+	protoc -I ${PROTO_DIR} --python_out=${PROTO_DIR} ${PROTO_DIR}/*.proto
+
+bump:
+	./venv/bin/python -m pip list -o --format columns | cut -d' ' -f1 | xargs -n1 ./venv/bin/python -m pip install -U
+	./venv/bin/python -m pip freeze > requirements.txt
+
+clean:
+	rm ${PROTO_DIR}/*_pb2.py

Stream/uav_proto_msg/flight_information.bin

@@ -0,0 +1,2 @@
+
+
 KG?þ¶F~l=FEC

Stream/uav_proto_msg/flight_information.json

@@ -0,0 +1,6 @@
+gps {
+  LAT: 34123.125
+  LON: 23423.123046875
+  ALT: 12123.123046875
+}
+heading: 155.1221466064453

Stream/uav_proto_msg/fly_formation.json

@@ -0,0 +1,2 @@
+velocity: 128.0
+fly_formation: FLY_FORMATION_v

Stream/uav_proto_msg/local_mqtt_pub_data_to_ros.py

@@ -0,0 +1,92 @@
+#!/usr/bin/env python3
+#coding:utf-8
+# license removed for brevity
+import ssl
+import rospy
+from std_msgs.msg import String
+import paho.mqtt.client as mqtt
+
+import sys
+
+import paho.mqtt.client as mqtt
+import proto.flyformatioln_pb2 as flyformatioln_pb2
+import proto.flight_information_pb2 as flight_information_pb2
+import google.protobuf.json_format as json_format
+import time
+# import proto.duration_pb2 as duration_pb2
+flight_information_msg = flight_information_pb2.flight_information_message()
+# Ros
+
+
+deserialize_time = 0
+count = 1
+def ros_pub(dataJson):
+    start = time.perf_counter()
+    proto_msg = flight_information_msg.FromString(dataJson)
+    js = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True)
+    finish = time.perf_counter()
+    deserialize_time += finish - start
+    print(f"Data{count}Finished in {finish-start}   "
+          f", total time{deserialize_time} ", end="\r")
+    count += 1
+
+    # data = json.loads(dataJson)
+    # publisher.publish(dataJson)     #將date字串發布到topic
+    # rate.sleep()
+    # print(f"publish data {data}")
+
+
+# MQTT
+def initialise_clients(clientname):
+    # callback assignment
+    initialise_client = mqtt.Client(clientname, False) 
+    # initialise_client.topic_ack = []
+    return initialise_client
+
+def on_connect(client, userdata, flags, rc):
+    # print("Connected with result code " + str(rc))
+    client.subscribe(mqtt_config["topic"])
+    # client.subscribe("data/sensor")
+    # client.subscribe("data/message")
+
+
+def on_message(client, userdata, msg):
+    # print(f"got {msg.payload.decode('utf-8')}")
+    # print(f"msg.topic {msg.payload.decode('utf-8')}")
+    ros_pub(msg.payload)
+    # PubMessage(msg.payload.decode('utf-8'))
+
+
+if __name__ == '__main__':
+    # Mqtt
+    mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/pub", "topic1":"data/message"}
+     
+    client = initialise_clients("123456")
+    client.on_connect = on_connect
+    client.on_message = on_message
+    client.connect(mqtt_config["host"], mqtt_config["port"], 60)
+
+
+    
+    # Ros
+    Mqtt_Node = 'publisher_py'
+    rospy.init_node(Mqtt_Node)
+    # initialize Ros node
+    topicName = 'uav_message'
+    publisher = rospy.Publisher(topicName,String,queue_size=10)
+
+
+    rate = rospy.Rate(10)
+
+
+
+    client.loop_forever()
+
+# mqtt connect code list
+# 0: Connection successful
+# 1: Connection refused – incorrect protocol version
+# 2: Connection refused – invalid client identifier
+# 3: Connection refused – server unavailable
+# 4: Connection refused – bad username or password
+# 5: Connection refused – not authorised
+# 6-255: Currently unused.

Stream/uav_proto_msg/local_mqtt_sub_data_from_ros.py

@@ -0,0 +1,130 @@
+#!/usr/bin/env python3
+#coding:utf-8
+import sys
+import json
+import paho.mqtt.client as mqtt
+import proto.flyformatioln_pb2 as flyformatioln_pb2
+import proto.flight_information_pb2 as flight_information_pb2
+import google.protobuf.json_format as json_format
+import datetime
+# import proto.duration_pb2 as duration_pb2
+
+
+from traceback import print_tb
+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
+flight_information_msg = flight_information_pb2.flight_information_message()
+mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/pub"}
+
+# Ros
+# def callBack_imu(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}
+#     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')
+
+
+def callBack_gps(GPS):
+    # flight_information_msg = flight_information_pb2.flight_information_message()
+    a = round(GPS.latitude, 3)
+    flight_information_msg.gps.LAT = GPS.latitude
+    flight_information_msg.gps.LON = GPS.longitude
+    flight_information_msg.gps.ALT = GPS.altitude
+    # print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
+    # print(flight_information_msg)
+
+
+def callBack_compass_hdg(Compass):
+    flight_information_msg.heading = round(Compass.data, 2)
+    mqtt_Pub(message=flight_information_msg)
+    # print(flight_information_msg)
+
+
+# def callBack_rng(RNG):
+#     dataRngUpdate = {"range": RNG.range}
+#     data.update(dataRngUpdate)
+#     dataJsonFormate = json.dumps(data)
+#     # print ('range:'+range+'\n')
+#     print (dataJsonFormate)
+#     mqtt_Pub(message=dataJsonFormate)
+
+
+# MQTT
+def initialise_clients(clientname):
+    # callback assignment
+    initialise_client = mqtt.Client(clientname, False) 
+    initialise_client.topic_ack = []
+    return initialise_client
+
+
+# publish a message
+def mqtt_Pub(message, topics = mqtt_config["topic"], waitForAck=False):
+    # json = json_format.MessageToJson(message, indent=None, preserving_proto_field_name=True)
+    message = message.SerializeToString()
+    mid = client.publish(topics, message, 0)[1]
+    # print(f"just published {message} to topic")
+    if waitForAck:
+        while mid not in client.topic_ack:
+            print("wait for ack")
+            time.sleep(0.25)
+        client.topic_ack.remove(mid)
+
+
+def on_publish(self, userdata, mid):
+    # client.topic_ack.append(mid)
+    print("send")
+    
+
+def on_connect(self, userdata, flags, rc):
+    print("Connected with result code " + str(rc))
+
+ 
+if __name__ == '__main__':
+    # Mqtt
+    client = initialise_clients("client1")
+    client.on_publish = on_publish
+    client.on_connect = on_connect
+    client.connect(mqtt_config["host"], mqtt_config["port"], 60)
+    client.loop_start()
+
+    # Ros
+    nodeName = 'save_data_py'
+    rospy.init_node(nodeName)
+
+    ros_namespace = ""
+    if  not rospy.has_param("namespace"):
+        print("using default namespace")
+    else:
+        rospy.get_param("namespace", ros_namespace)
+        print("using namespace "+ros_namespace)
+	
+    ros_namespace="/drone1"
+    # topicName = 'data_topic'
+    # subscriber = rospy.Subscriber('/mavros/imu/data_raw',Imu,callBack_imu)  #從topic獲取string再呼叫callback
+    subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/global',NavSatFix,callBack_gps)  #從topic獲取string再呼叫callback
+    subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg',Float64,callBack_compass_hdg)  #從topic獲取string再呼叫callback
+    # subscriber = rospy.Subscriber(ros_namespace+"/mavros/next_command",String,callBack_message)
+    #subscriber = rospy.Subscriber(ros_namespace+'/mavros/mavros_msgs/State',Header,callBack_state)
+    # ID = rospy.get_param(ros_namespace+'/leader/droneID')
+    # subscriber = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng)  #從topic獲取string再呼叫callback
+
+    # ID = rospy.GetParam("droneID")
+    # print(ID)
+
+    rospy.spin()

Stream/uav_proto_msg/main.py

@@ -0,0 +1,50 @@
+import proto.flyformatioln_pb2 as flyformatioln_pb2
+import proto.flight_information_pb2 as flight_information_pb2
+import google.protobuf.json_format as json_format
+import datetime
+import proto.duration_pb2 as duration_pb2
+import google.protobuf.internal.well_known_types as well_known_types
+
+def flight_information():
+    flight_information_msg = flight_information_pb2.flight_information_message()
+    flight_information_msg.gps.LAT = 34123.1231515
+    flight_information_msg.gps.LON = 23423.1231515
+    flight_information_msg.gps.ALT = 12123.1231515
+    flight_information_msg.heading = 155.12215
+    return flight_information_msg
+
+
+def fly_formation():
+    fly_formation_msg = flyformatioln_pb2.fly_formation_message()
+    fly_formation_msg.velocity = 128
+    fly_formation_msg.fly_formation = flyformatioln_pb2.FLY_FORMATION_v
+    return fly_formation_msg
+
+
+def file(message):
+    path = "fly_formation.bin"
+    print("write to file")
+    with open(path, "wb") as f:
+        f.write(message.SerializeToString())
+
+    with open(path, "rb") as f:
+        msg = message.FromString(f.read())
+        print(msg)
+
+def dura():
+    t = duration_pb2.Duration()
+    t.seconds=1
+    t.ToMilliseconds()
+    print(t.ToMilliseconds())
+
+
+if __name__ == '__main__':
+    # flight_information_serial = flight_information()
+    # fly_formation_serial = fly_formation()
+    #
+    #
+    # file(fly_formation_serial)
+    # json = to_json(complexfun())
+    # print(json)
+    # print(from_json(json, complexfun))
+    dura()

Stream/uav_proto_msg/timeit_test.py

@@ -0,0 +1,56 @@
+import proto.flyformatioln_pb2 as flyformatioln_pb2
+import proto.flight_information_pb2 as flight_information_pb2
+import google.protobuf.json_format as json_format
+import timeit
+import orjson
+import json
+a = '{"gps": {"LAT": 34123.125, "LON": 23423.123, "ALT": 12123.123}, "heading": 155.12215}'
+data ={}
+
+flight_information_msg = flight_information_pb2.flight_information_message()
+flight_information_msg.gps.LAT = 34123.1231515
+flight_information_msg.gps.LON = 23423.1231515
+flight_information_msg.gps.ALT = 12123.1231515
+flight_information_msg.heading = 155.12215
+proto_deserialize_msg = flight_information_msg.SerializeToString()
+
+
+
+def proto_serialize():
+    flight_information_msg = flight_information_pb2.flight_information_message()
+    flight_information_msg.gps.LAT = 34123.1231515
+    flight_information_msg.gps.LON = 23423.1231515
+    flight_information_msg.gps.ALT = 12123.1231515
+    flight_information_msg.heading = 155.12215
+    proto_deserialize_msg = flight_information_msg.SerializeToString()
+
+
+def proto_deserialize(proto):
+    proto_msg = flight_information_msg.FromString(proto)
+    # js = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True) 
+
+def json_serialize(lib):
+    lat = 34123.1231515
+    lon = 23423.1231515
+    alt = 12123.1231515
+    heading = 155.12215
+    dataGpsUpdate = {"gps": {"lat": lat, "lon": lon, "ALT": alt}, "heading":heading}
+    data.update(dataGpsUpdate)
+    js = lib.dumps(data).decode("utf-8")
+    # print(js)
+def json_deserialize(lib):
+    js = lib.loads(a)
+    # print(js)
+
+
+
+
+proto_serialize_time = timeit.timeit(lambda: proto_serialize(), number = 150000)
+proto_deserialize_time = timeit.timeit(lambda: proto_deserialize(proto_deserialize_msg), number = 150000)
+json_serialize_time = timeit.timeit(lambda: json_serialize(orjson), number = 150000)
+json_deserialize_time = timeit.timeit(lambda: json_deserialize(orjson), number = 150000)
+print(f"proto_serialize_time:   {proto_serialize_time}")
+print(f"json_serialize_time:    {json_serialize_time}")
+print()
+print(f"proto_deserialize_time: {proto_deserialize_time}")
+print(f"json_deserialize_time:  {json_deserialize_time}")