diff --git a/Stream/localRosData/local_mqtt_pub_data_to_ros.py b/Stream/localRosData/local_mqtt_pub_data_to_ros.py index 55f4b33..efeb57b 100644 --- a/Stream/localRosData/local_mqtt_pub_data_to_ros.py +++ b/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): diff --git a/Stream/localRosData/local_mqtt_sub_data_from_ros.py b/Stream/localRosData/local_mqtt_sub_data_from_ros.py index 1b71e5e..ac8db0f 100644 --- a/Stream/localRosData/local_mqtt_sub_data_from_ros.py +++ b/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) diff --git a/Stream/uav_proto_msg/.idea/.gitignore b/Stream/uav_proto_msg/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/Stream/uav_proto_msg/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/Stream/uav_proto_msg/.idea/inspectionProfiles/Project_Default.xml b/Stream/uav_proto_msg/.idea/inspectionProfiles/Project_Default.xml new file mode 100644 index 0000000..ffff49d --- /dev/null +++ b/Stream/uav_proto_msg/.idea/inspectionProfiles/Project_Default.xml @@ -0,0 +1,21 @@ + + + + \ No newline at end of file diff --git a/Stream/uav_proto_msg/.idea/inspectionProfiles/profiles_settings.xml b/Stream/uav_proto_msg/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/Stream/uav_proto_msg/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/Stream/uav_proto_msg/.idea/misc.xml b/Stream/uav_proto_msg/.idea/misc.xml new file mode 100644 index 0000000..15abfca --- /dev/null +++ b/Stream/uav_proto_msg/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Stream/uav_proto_msg/.idea/modules.xml b/Stream/uav_proto_msg/.idea/modules.xml new file mode 100644 index 0000000..9154ab2 --- /dev/null +++ b/Stream/uav_proto_msg/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Stream/uav_proto_msg/.idea/protocolBufferUdemy.iml b/Stream/uav_proto_msg/.idea/protocolBufferUdemy.iml new file mode 100644 index 0000000..d0876a7 --- /dev/null +++ b/Stream/uav_proto_msg/.idea/protocolBufferUdemy.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Stream/uav_proto_msg/.idea/vcs.xml b/Stream/uav_proto_msg/.idea/vcs.xml new file mode 100644 index 0000000..1d3e46c --- /dev/null +++ b/Stream/uav_proto_msg/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/Stream/uav_proto_msg/Makefile b/Stream/uav_proto_msg/Makefile new file mode 100644 index 0000000..f66a9be --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Stream/uav_proto_msg/flight_information.bin b/Stream/uav_proto_msg/flight_information.bin new file mode 100644 index 0000000..5da638e --- /dev/null +++ b/Stream/uav_proto_msg/flight_information.bin @@ -0,0 +1,2 @@ + + KG?F~l=FEC \ No newline at end of file diff --git a/Stream/uav_proto_msg/flight_information.json b/Stream/uav_proto_msg/flight_information.json new file mode 100644 index 0000000..249c5cc --- /dev/null +++ b/Stream/uav_proto_msg/flight_information.json @@ -0,0 +1,6 @@ +gps { + LAT: 34123.125 + LON: 23423.123046875 + ALT: 12123.123046875 +} +heading: 155.1221466064453 diff --git a/Stream/uav_proto_msg/fly_formation.bin b/Stream/uav_proto_msg/fly_formation.bin new file mode 100644 index 0000000..6976857 Binary files /dev/null and b/Stream/uav_proto_msg/fly_formation.bin differ diff --git a/Stream/uav_proto_msg/fly_formation.json b/Stream/uav_proto_msg/fly_formation.json new file mode 100644 index 0000000..45397cf --- /dev/null +++ b/Stream/uav_proto_msg/fly_formation.json @@ -0,0 +1,2 @@ +velocity: 128.0 +fly_formation: FLY_FORMATION_v \ No newline at end of file diff --git a/Stream/uav_proto_msg/local_mqtt_pub_data_to_ros.py b/Stream/uav_proto_msg/local_mqtt_pub_data_to_ros.py new file mode 100644 index 0000000..24eaf6f --- /dev/null +++ b/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. \ No newline at end of file diff --git a/Stream/uav_proto_msg/local_mqtt_sub_data_from_ros.py b/Stream/uav_proto_msg/local_mqtt_sub_data_from_ros.py new file mode 100644 index 0000000..e4f8c97 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/Stream/uav_proto_msg/main.py b/Stream/uav_proto_msg/main.py new file mode 100644 index 0000000..cb3f196 --- /dev/null +++ b/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() diff --git a/Stream/uav_proto_msg/timeit_test.py b/Stream/uav_proto_msg/timeit_test.py new file mode 100644 index 0000000..86d2b3d --- /dev/null +++ b/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}") \ No newline at end of file