From bb0db35248705a760e4065e667d52aa2f98d9859 Mon Sep 17 00:00:00 2001 From: MajorTop Date: Tue, 13 Dec 2022 01:38:35 +0800 Subject: [PATCH] local test without ros --- .../local_mqtt_pub_data_to_ros.py | 92 ------------- .../local_mqtt_sub_data_from_ros.py | 130 ------------------ Stream/uav_proto_msg/main.py | 50 ------- Stream/uav_proto_msg/mqttPubMain.py | 111 +++++++++++++++ Stream/uav_proto_msg/mqttSubMain.py | 66 +++++++++ Stream/uav_proto_msg/proto/duration_pb2.py | 78 +++++++++++ .../proto/flight_information.proto | 14 ++ .../proto/flight_information_pb2.py | 27 ++++ .../uav_proto_msg/proto/flyformatioln.proto | 17 +++ .../uav_proto_msg/proto/flyformatioln_pb2.py | 27 ++++ Stream/uav_proto_msg/protomsg/Data/timeData | 2 +- Stream/uav_proto_msg/protomsg/timeit_test.py | 13 +- Stream/uav_proto_msg/utils/__init__.py | 6 + Stream/uav_proto_msg/utils/__init__.pyc | Bin 0 -> 291 bytes .../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 500 bytes .../__pycache__/basicMqtt.cpython-310.pyc | Bin 0 -> 844 bytes ...oJson_mqtt_pub_data_to_ros.cpython-310.pyc | Bin 0 -> 1823 bytes ...son_mqtt_sub_data_from_ros.cpython-310.pyc | Bin 0 -> 3584 bytes .../__pycache__/readConfig.cpython-310.pyc | Bin 0 -> 1839 bytes Stream/uav_proto_msg/utils/basicMqtt.py | 17 +++ Stream/uav_proto_msg/utils/mqttConfig.yml | 21 +++ .../utils/protoJson_mqtt_pub_data_to_ros.py | 60 ++++++++ .../utils/protoJson_mqtt_pub_data_to_ros.pyc | Bin 0 -> 2060 bytes .../utils/protoJson_mqtt_sub_data_from_ros.py | 113 +++++++++++++++ Stream/uav_proto_msg/utils/readConfig.py | 65 +++++++++ 25 files changed, 624 insertions(+), 285 deletions(-) delete mode 100644 Stream/uav_proto_msg/local_mqtt_pub_data_to_ros.py delete mode 100644 Stream/uav_proto_msg/local_mqtt_sub_data_from_ros.py delete mode 100644 Stream/uav_proto_msg/main.py create mode 100644 Stream/uav_proto_msg/mqttPubMain.py create mode 100644 Stream/uav_proto_msg/mqttSubMain.py create mode 100644 Stream/uav_proto_msg/proto/duration_pb2.py create mode 100644 Stream/uav_proto_msg/proto/flight_information.proto create mode 100644 Stream/uav_proto_msg/proto/flight_information_pb2.py create mode 100644 Stream/uav_proto_msg/proto/flyformatioln.proto create mode 100644 Stream/uav_proto_msg/proto/flyformatioln_pb2.py create mode 100644 Stream/uav_proto_msg/utils/__init__.py create mode 100644 Stream/uav_proto_msg/utils/__init__.pyc create mode 100644 Stream/uav_proto_msg/utils/__pycache__/__init__.cpython-310.pyc create mode 100644 Stream/uav_proto_msg/utils/__pycache__/basicMqtt.cpython-310.pyc create mode 100644 Stream/uav_proto_msg/utils/__pycache__/protoJson_mqtt_pub_data_to_ros.cpython-310.pyc create mode 100644 Stream/uav_proto_msg/utils/__pycache__/protoJson_mqtt_sub_data_from_ros.cpython-310.pyc create mode 100644 Stream/uav_proto_msg/utils/__pycache__/readConfig.cpython-310.pyc create mode 100644 Stream/uav_proto_msg/utils/basicMqtt.py create mode 100644 Stream/uav_proto_msg/utils/mqttConfig.yml create mode 100644 Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.py create mode 100644 Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.pyc create mode 100644 Stream/uav_proto_msg/utils/protoJson_mqtt_sub_data_from_ros.py create mode 100644 Stream/uav_proto_msg/utils/readConfig.py 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 deleted file mode 100644 index 24eaf6f..0000000 --- a/Stream/uav_proto_msg/local_mqtt_pub_data_to_ros.py +++ /dev/null @@ -1,92 +0,0 @@ -#!/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 deleted file mode 100644 index e4f8c97..0000000 --- a/Stream/uav_proto_msg/local_mqtt_sub_data_from_ros.py +++ /dev/null @@ -1,130 +0,0 @@ -#!/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 deleted file mode 100644 index cb3f196..0000000 --- a/Stream/uav_proto_msg/main.py +++ /dev/null @@ -1,50 +0,0 @@ -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/mqttPubMain.py b/Stream/uav_proto_msg/mqttPubMain.py new file mode 100644 index 0000000..c2e2638 --- /dev/null +++ b/Stream/uav_proto_msg/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() diff --git a/Stream/uav_proto_msg/mqttSubMain.py b/Stream/uav_proto_msg/mqttSubMain.py new file mode 100644 index 0000000..2dfdec5 --- /dev/null +++ b/Stream/uav_proto_msg/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() \ No newline at end of file diff --git a/Stream/uav_proto_msg/proto/duration_pb2.py b/Stream/uav_proto_msg/proto/duration_pb2.py new file mode 100644 index 0000000..1a30c51 --- /dev/null +++ b/Stream/uav_proto_msg/proto/duration_pb2.py @@ -0,0 +1,78 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: duration.proto + +from google.protobuf import descriptor as _descriptor +from google.protobuf import message as _message +from google.protobuf import reflection as _reflection +from google.protobuf import symbol_database as _symbol_database +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor.FileDescriptor( + name='duration.proto', + package='google.protobuf', + syntax='proto3', + serialized_options=b'\n\023com.google.protobufB\rDurationProtoP\001Z1google.golang.org/protobuf/types/known/durationpb\370\001\001\242\002\003GPB\252\002\036Google.Protobuf.WellKnownTypes', + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\x0e\x64uration.proto\x12\x0fgoogle.protobuf\"*\n\x08\x44uration\x12\x0f\n\x07seconds\x18\x01 \x01(\x03\x12\r\n\x05nanos\x18\x02 \x01(\x05\x42\x83\x01\n\x13\x63om.google.protobufB\rDurationProtoP\x01Z1google.golang.org/protobuf/types/known/durationpb\xf8\x01\x01\xa2\x02\x03GPB\xaa\x02\x1eGoogle.Protobuf.WellKnownTypesb\x06proto3' +) + + + + +_DURATION = _descriptor.Descriptor( + name='Duration', + full_name='google.protobuf.Duration', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='seconds', full_name='google.protobuf.Duration.seconds', index=0, + number=1, type=3, cpp_type=2, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='nanos', full_name='google.protobuf.Duration.nanos', index=1, + number=2, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=35, + serialized_end=77, +) + +DESCRIPTOR.message_types_by_name['Duration'] = _DURATION +_sym_db.RegisterFileDescriptor(DESCRIPTOR) + +Duration = _reflection.GeneratedProtocolMessageType('Duration', (_message.Message,), { + 'DESCRIPTOR' : _DURATION, + '__module__' : 'duration_pb2' + # @@protoc_insertion_point(class_scope:google.protobuf.Duration) + }) +_sym_db.RegisterMessage(Duration) + + +DESCRIPTOR._options = None +# @@protoc_insertion_point(module_scope) diff --git a/Stream/uav_proto_msg/proto/flight_information.proto b/Stream/uav_proto_msg/proto/flight_information.proto new file mode 100644 index 0000000..a53b4f3 --- /dev/null +++ b/Stream/uav_proto_msg/proto/flight_information.proto @@ -0,0 +1,14 @@ +syntax = 'proto3'; + +// GPS + compass + +message GPS { + optional float LAT = 1; + optional float LON = 2; + optional float ALT = 3; +} + +message flight_information_message { + GPS gps = 1; + optional float heading = 2; +} \ No newline at end of file diff --git a/Stream/uav_proto_msg/proto/flight_information_pb2.py b/Stream/uav_proto_msg/proto/flight_information_pb2.py new file mode 100644 index 0000000..63c28d4 --- /dev/null +++ b/Stream/uav_proto_msg/proto/flight_information_pb2.py @@ -0,0 +1,27 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: flight_information.proto +"""Generated protocol buffer code.""" +from google.protobuf.internal import builder as _builder +from google.protobuf import descriptor as _descriptor +from google.protobuf import descriptor_pool as _descriptor_pool +from google.protobuf import symbol_database as _symbol_database +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x18\x66light_information.proto\"S\n\x03GPS\x12\x10\n\x03LAT\x18\x01 \x01(\x02H\x00\x88\x01\x01\x12\x10\n\x03LON\x18\x02 \x01(\x02H\x01\x88\x01\x01\x12\x10\n\x03\x41LT\x18\x03 \x01(\x02H\x02\x88\x01\x01\x42\x06\n\x04_LATB\x06\n\x04_LONB\x06\n\x04_ALT\"Q\n\x1a\x66light_information_message\x12\x11\n\x03gps\x18\x01 \x01(\x0b\x32\x04.GPS\x12\x14\n\x07heading\x18\x02 \x01(\x02H\x00\x88\x01\x01\x42\n\n\x08_headingb\x06proto3') + +_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) +_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'flight_information_pb2', globals()) +if _descriptor._USE_C_DESCRIPTORS == False: + + DESCRIPTOR._options = None + _GPS._serialized_start=28 + _GPS._serialized_end=111 + _FLIGHT_INFORMATION_MESSAGE._serialized_start=113 + _FLIGHT_INFORMATION_MESSAGE._serialized_end=194 +# @@protoc_insertion_point(module_scope) diff --git a/Stream/uav_proto_msg/proto/flyformatioln.proto b/Stream/uav_proto_msg/proto/flyformatioln.proto new file mode 100644 index 0000000..8825dcb --- /dev/null +++ b/Stream/uav_proto_msg/proto/flyformatioln.proto @@ -0,0 +1,17 @@ +syntax = "proto3"; + +// leader only + +enum FLY_FORMATION{ + FLY_FORMATION_UNSPECIFIED = 0; + FLY_FORMATION_v = 1; + FLY_FORMATION_X = 2; + FLY_FORMATION_O = 3; + FLY_FORMATION_LINE = 4; + FLY_FORMATION_ROW = 5; + FLY_FORMATION_HEX = 6; + } + message fly_formation_message{ + optional float velocity = 1; + FLY_FORMATION fly_formation= 2; + } \ No newline at end of file diff --git a/Stream/uav_proto_msg/proto/flyformatioln_pb2.py b/Stream/uav_proto_msg/proto/flyformatioln_pb2.py new file mode 100644 index 0000000..ef2a0e0 --- /dev/null +++ b/Stream/uav_proto_msg/proto/flyformatioln_pb2.py @@ -0,0 +1,27 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: flyformatioln.proto +"""Generated protocol buffer code.""" +from google.protobuf.internal import builder as _builder +from google.protobuf import descriptor as _descriptor +from google.protobuf import descriptor_pool as _descriptor_pool +from google.protobuf import symbol_database as _symbol_database +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x13\x66lyformatioln.proto\"b\n\x15\x66ly_formation_message\x12\x15\n\x08velocity\x18\x01 \x01(\x02H\x00\x88\x01\x01\x12%\n\rfly_formation\x18\x02 \x01(\x0e\x32\x0e.FLY_FORMATIONB\x0b\n\t_velocity*\xb3\x01\n\rFLY_FORMATION\x12\x1d\n\x19\x46LY_FORMATION_UNSPECIFIED\x10\x00\x12\x13\n\x0f\x46LY_FORMATION_v\x10\x01\x12\x13\n\x0f\x46LY_FORMATION_X\x10\x02\x12\x13\n\x0f\x46LY_FORMATION_O\x10\x03\x12\x16\n\x12\x46LY_FORMATION_LINE\x10\x04\x12\x15\n\x11\x46LY_FORMATION_ROW\x10\x05\x12\x15\n\x11\x46LY_FORMATION_HEX\x10\x06\x62\x06proto3') + +_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) +_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'flyformatioln_pb2', globals()) +if _descriptor._USE_C_DESCRIPTORS == False: + + DESCRIPTOR._options = None + _FLY_FORMATION._serialized_start=124 + _FLY_FORMATION._serialized_end=303 + _FLY_FORMATION_MESSAGE._serialized_start=23 + _FLY_FORMATION_MESSAGE._serialized_end=121 +# @@protoc_insertion_point(module_scope) diff --git a/Stream/uav_proto_msg/protomsg/Data/timeData b/Stream/uav_proto_msg/protomsg/Data/timeData index 3960028..e4e8f27 100644 --- a/Stream/uav_proto_msg/protomsg/Data/timeData +++ b/Stream/uav_proto_msg/protomsg/Data/timeData @@ -1 +1 @@ -{"serialize":{"json":0.710234867001418,"proto":2.2478437370155007},"deserializ":{"json":0.41798062800080515,"proto":0.49253950000274926}} \ No newline at end of file +{"serialize":{"json":0.35475994200533023,"proto":1.0138140169947292},"deserializ":{"json":0.41626033899956383,"proto":0.4805599120008992}} \ No newline at end of file diff --git a/Stream/uav_proto_msg/protomsg/timeit_test.py b/Stream/uav_proto_msg/protomsg/timeit_test.py index e1eed8c..6616fbd 100644 --- a/Stream/uav_proto_msg/protomsg/timeit_test.py +++ b/Stream/uav_proto_msg/protomsg/timeit_test.py @@ -17,11 +17,6 @@ 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() @@ -30,13 +25,7 @@ def proto_deserialize(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") + js = lib.dumps(a).decode("utf-8") # print(js) def json_deserialize(lib): diff --git a/Stream/uav_proto_msg/utils/__init__.py b/Stream/uav_proto_msg/utils/__init__.py new file mode 100644 index 0000000..71fc5b7 --- /dev/null +++ b/Stream/uav_proto_msg/utils/__init__.py @@ -0,0 +1,6 @@ +from utils.protoJson_mqtt_pub_data_to_ros import Proto_msg_to_ros +from utils.protoJson_mqtt_pub_data_to_ros import Json_msg_to_ros +from utils.protoJson_mqtt_sub_data_from_ros import Proto_msg_from_ros +from utils.protoJson_mqtt_sub_data_from_ros import Json_msg_from_ros +from utils.readConfig import MQTT_ROS_Config +from utils.basicMqtt import MQTTClient \ No newline at end of file diff --git a/Stream/uav_proto_msg/utils/__init__.pyc b/Stream/uav_proto_msg/utils/__init__.pyc new file mode 100644 index 0000000000000000000000000000000000000000..da74d380a914387f25670bb1fc6e974ccc54184a GIT binary patch literal 291 zcmZSn%**xU;DqF41}I>X zPO)AANV8XQeqMZTVM$4RL1|KaN@7VO)Tkh!I))N8T&jyy6kMrWR$PG< zD+Ao%%Bb=$b^I$^ zX(e~UqaWb`Ihfrq7VP16&L*l{6>Bg|95oRIFYV1Jp(nL1gc(<^a_jJ7EvxD!%Tt@~ z`q1w2+J`_-|J9qleqUP$E+cdq!!|?nG;f>G9Y+sZdYPIcn?X$-0sD@0>=Ir0x+G6Z za$;4vAyaOit*XdWtCE9_)k=^|q{c9*axRF`8EG5|V?|k5#>UlV8=t{#E%-V7C^9_= KJ=DYP9O7?Td5$gs literal 0 HcmV?d00001 diff --git a/Stream/uav_proto_msg/utils/__pycache__/basicMqtt.cpython-310.pyc b/Stream/uav_proto_msg/utils/__pycache__/basicMqtt.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..4bfc01f47f3d0cf0819b9e751a5f335eb8a2ac3c GIT binary patch literal 844 zcmZWn&2H2%5FY2R*>o2wTJ^*W?12prfDob;J+%^I56BlQYkRluI>|P6ii-BuO1&W- z0VFPc6Q03WPJIPVjFYWc!9@Nv^JP5qX*~64lmXg{hp+r60r=&EZHq8?gU7r?5Fm(# zk_14Y3@9Nz6VmVmL?FUj5TPU&(M?EWK|a7Zdca$7#yxIZxCzX2bRiewu<-~UfnJp^AW+q4P1~lnG*%;;0O7F`fTtm_zr7e z;lg7i2x2S(#H7QWj!8RzAVTCb5h3n&`9lGyAwHYFTVR9E8k`c0M=mz)N;WQKOjXJ< z<|3;~Nk_UcmaW!hgTd}z=7W9c}XNEUwCi{4QRtU!Hs*>EQ`Y~xk zNYN)f70PU=|B1e}?^>D#mTD_YcZ9rBTpAor&>nuCDWAX_hEFKW^$u9@Uj?YMu9~Wi6!6O~donDsA#vVH7{Tw05$(Zv1n^ zc+4Tf0m(@6&zF(U`+uKv@;JM3?C0p!;T$u@ut}@EKVQj1#xFD$eUk>DBUXpweHSV`L2;K@6uPj QzSn*eOoH$XQgTTC077lm)Bpeg literal 0 HcmV?d00001 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 new file mode 100644 index 0000000000000000000000000000000000000000..1c49beadb69b2d395a2c7e0657a678b62647dfda GIT binary patch literal 1823 zcmaJ>OK;mo5auo)q9jUooQIOSD1zKrv=Y$t&_h}jNZ_IY;uf%h0(&87aV1e9xy*7E zK!$R1&|A;B%E$htz4p{ge?U&1SxPb++m$f0+}WMS&U`bX*XvmD`-goPzw|BZFWfYL zHf(+bsu-}uN=Rn0dL@jQJv2vgJ#QMofXkx#&68nO(Htd|l zg`Eq#PC`zs(EA(gTgPMM&EL-dw;<&f`eZaA%m#9RTp$mq1>^&@d8hVtL0H#VcJ%(Q zQYaDS)ii=dit2bw8$KNPc7dJ)RlfsUk-4?9=HxY%hp^JM4HD;)yrasUdn=01PUs)@ zJ6p9@_65NQpKP^!(E1*<+P7=LX=mMC*_WUR-@&?f35h}*ss#z!*bANG(~xQ}EfQWR zc34VYarrharcr6$Gf8=tL`9tQk9$g#>G*LW)Z>X*6iL|Et`X@kjd%@OvH)q&MQ!_4YA%UURCjwpNf|t z)ouxQm40-TEu*6wUEP_$(Nz@asi64>?HCjmWw zXaa2jZ@5tl1MNqqbWx-`5eBfxID7|D^kxxfSkx$zEifztWPM=z^G3F=RaFm~r;Qp& z4-ItDK-WAs=V~Src^Ac)r3O2|E%pYCg8E&)Hjv@e=@WC~_D=l@bpNkYxxees z_NGJkwuQT~K{23RgU(>nXwbn`gRWr~T*BWpmA7tfn`(0#8*_Non77?lFh6!JXdj=u z3pM%rJBdRg>`Dy7B$PFZalM6P5AR_TwU_WQz{^xn?x7e zF#;BdK?5Ly0^TK_hF#_fImZ#;s5Iw*Qk+i1>q&{qvu|#kHz*%$wU*lnkrhS(r^bIp z>~Jc?G~+{~a<-TZ(ff8R`CF-MLv#kJCeB7$4d@Z-+J9De-VX$JCc(btL+1wMfb{9X GlYaqYX3wDj literal 0 HcmV?d00001 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 new file mode 100644 index 0000000000000000000000000000000000000000..ac08e7997b0117cb93258897c132a7f885bb2d3a GIT binary patch literal 3584 zcmaJ^L37*26~%Y{Qe>gff(9#bfge6#PwevS?vpbfs1%GY{{>*OM=sChg&qa?5wq-TF zmzZU>LXz`O6`dPsX$&E)HWOCc7K~=K9W)osLo1>AvYh|QGuEy^H@^XW3A!gr&n@Ux z=&Pb4Yhp>(MO7|~np_ce!FDTRS*$#FVY6yHYUsv8m1IfKOS{3YN_v4x(pG50kLg1? zwOeTE&mksk-?J+9 zktO}n#%-BC&XU2#ZI$%pt)!o+B;E+)PMQY2M96p}RpG`ki{f-+Kzsf&B?q2lS&$BQ z0@2AjGX$;a=+eqROjmg^k}ljoQ_D`vn9kX&kgu2nIIcb;w^> z2c$U@k_h%UHbYVYLA!Ni>rzjqX{Rf-huuYC+Vr$T-YQyCm-b{wME$N_-I6Nm#L-x8 zCtI1KPrljR!{Ls;vYBIlcMf~A+IDgu2sF7`GK<|xdIJE~Udv-OXMR8J8pv}&c<8m+ zj>CLfuqV2gFk+CZ*cE$?jo&*DskwQl5OcARLF^ioY0d>Wa)jC0$YI12_BXshkGEb@ z;$m~Fc&P@2=1-EeSQsaM`pbuY$72CHf>@_xSo(J&4+)c$WaSn{i-#bwT zCvJw1)$172o|4&6^+Upb`cT$SWAVSgpaYbxLxx;s6Xyj=)Me|yopAO3#HEpa`zcdz zWn9?L7_&0x&=vdzwBymVk=q@56PHFP)2SUft(WBBxJquV?~p)@!e-fCGCZA zB>R~z4Td{$l+XsESGnEu_9ItO|2N3x}aC{YW+%< zD#NW3nyDE7gpyIs07pGz2X>)z=)BM!=#J4{W8)S&*?5IcHYMaapE$@~Zvy)L);sX2 z${dy^KILwP-A>#~pnNpvk~Q%P9p5IUT&Htjs=3!f>GrxKl>}cG?@{p{>ry8SWsHGB z8BLY(*e`~zYgU_{zF5bYW<(B0nfQ9j#b+AQ7X zxHIYvZ@@mK;#{1$Y0pwE2qMGOrw~(KnhfdW;D8qP993Z)^O)3f{a^}>E&TY z5i&g@Rs>P-Z#ha8i1yGuEQEZO9mhrsxW%?vVI`#9at&FuF5H&&0@_=m#)M>1fsMI;#U5TrJLn1UHcdNC*z{ z%w#4zcxVmBoERJ9HNUAN>i|bK7w83xJzulniF@OR_i-*!ql> zjA0ulhU`7tC*Rl438$H`;61U*ET9D2VnA`SPVFM&hvvTdg5|t&Ks^uV)7mcyXLfIL zTZ$4ytwD_4N86jf{8&*jZ~A#ODf3PEw3n!T+yk{MhCN(EJt0HjlNVB|$4xhn?`t%J zmQw^vdzsRoHk(tpJb{y`)PI9v5nLakZ7|%1-aQAPxy#T=ramUDS5mWjhn)KviT6lc zC9xPiv#SW-b7Hek5Zg~r`!?k@MgQ(JSJg+f=&wlpnuLMz4C+#JWmBJ%FuZl2G(%og zr_^srm{Uc!syQcT1w{Q017wk|Lx8WqQ#oH5wyJ}zDrhdT*4xZjHyzc2t@?lj(Vh8v Y*5ca#B9%1mQ@-W1y4|?;Bh4v6^Z)<= literal 0 HcmV?d00001 diff --git a/Stream/uav_proto_msg/utils/__pycache__/readConfig.cpython-310.pyc b/Stream/uav_proto_msg/utils/__pycache__/readConfig.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..09270638200b5e32c0c21b4ecb2fcb529426e2ba GIT binary patch literal 1839 zcmZ`3O>-MHRMKi~dE+=q3C#zjHGzu{)Hgn67$!3b>GY5^ZNg<|7|nJiC)>zgM=K{d zwJ*3=P8_%^g5@{Aj_)yqzIVjs z5VW3-Q9dQO3%O+ZO0B? zUKEHm*x7dgBnq)WAras~kdP+eUHXEy$XDREz)A&HE-)rb7kZTf3kvB}fmLN$Oy~Y; zh29KE$Gq7Bn*-^Xccb9FR$%i5_WA@1nYbwyo}#cK7R3^rfmq%}jW_Jf)`Pu0{_y@T zUr#%&WdHQtpbqOAw;t!ho{mhCcIvT;vMkpN*+kO@?QEa7Qr(WsUO4C2>nf2QvmLeN zPVbSu#@YoIJxDWSX*bozRt}}?Mk+a$VdW%IYA@|3apQ5|C%?_* z$X^Y`E?-l)6iZB?r6#Kkh!1-mcJ?g*g9cdO5g8JL&&YtBSa#nWyb3DRdV8GZ0`t1FoKkx_i80otMI`Eg!uw*KO(kPcZT6jz7 z(9>&RPr?4e+5`gxcBPSr(a}53SFQ{C`cu#>UDp(7tSJz5Re9TDeR^vv$+D!gUw<;J zXEM$kCxPB+?bln0k{Uyrrz<;=hT0ja>#I+OtM!8@tDoo;tW3WvpXECBiKBMZ5s^uC zzuvuK)tIt0?aGd&{ivdC`6K|(Ns%EC+X3r!lYib#Z@kHL~_ATQ) z=_H2pTdqMI2uOe#uHi)-cnq+cUJcj08UaY?^V7x2SYC|vor#0km2|QkUbNjj5C$3i zn80&;0t@mb8NmnH@U}M_IJ_ZbE9xo3TPi)_ndvLppw`Qb)^lKKspz%4E>Mxg5NHGI zWnkwu7=sn3Rpwx-IsX8N%uT>NK3#Y<)?8;B-WB|H+-z|ha95+Aa(I>Vqh6#gB;Zq| zG3VN4pl`ctyrcvF%A+(@dciSW#q_EJ?oBQM7nF8!xSu}T0l*N&loBW@`Iiy$l6mAG zM#XZ8J z|MA5Ke~{o0(C?h>CeaqLh)L#TX3m*&KVS6y_~N%OpL#vo|3vt|kL5}bjbA`HQFpQr zs2kMxA!QMDqxv{U**taU>tlnm1fKdD0G9u6HwR%T_6KDNUY)5 zBGqZLiXM|WtOnLYEcX>;FQ5#RC=2LyNaZW?5-aZC2lOr^FR|_+tST9}SpX}17s;z8 zWi8r^SU6xUiYooNojxS-+AHy+o;8jwHn+SGYGB8{A zDHupAe71#=%~m-!{vvKh`h))W|H09CR#GKeyVhd3TuJ`s&NOB=ANUY-ZV99jHUfN^hLY7fo&OUS7#g_B0t+9p`5hMS$*|YRkO?LL5HR)i z;0p%I;6I#s-IT%Klh>{N+5huv%yn4LHM_LwcqYfbf8)-fW6Ld|SWum3B4y@0s#Ord z1Z{d&o2m;=Fo)4WT0wA8@f=7KaU*iJr?osv<4PM`hc3rP2pxM-x)pT4Q`K4AEAFks z`m*w0d}Ac|=dQX0C0DTybk#JxHfKw6WXr4uC7+_L1&bIp*AL0+^mb-_PD>!hH6;kuCE!2;Z0^ivFkHl;zUQOgcS2`g#q^he#{NA6fA|y KK#|pO?fg%5guS@{ literal 0 HcmV?d00001 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 new file mode 100644 index 0000000..1661f0d --- /dev/null +++ b/Stream/uav_proto_msg/utils/protoJson_mqtt_sub_data_from_ros.py @@ -0,0 +1,113 @@ +import orjson +import time + +class Proto_msg_from_ros: + #Protobuf + flight_information_msg = None + fly_formation_msg = None # delcare in function + #Mqtt + client = None + Flight_Information_topicToMqtt = None + Fly_Formation_topicToMqtt = None + Fly_Formation_topicToMqtt_QOS = None + + @classmethod + def callBack_gps(cls, GPS): + # flight_information_msg = flight_information_pb2.flight_information_message() + cls.flight_information_msg.gps.LAT = GPS.latitude + 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) + + @classmethod + def callBack_compass_hdg(cls, Compass): + cls.flight_information_msg.heading = Compass.data + + flightInformationMsg = cls.flight_information_msg.SerializeToString() + cls.mqtt_Pub(message=flightInformationMsg, topics=cls.Flight_Information_topicToMqtt) + + + # TODO: Formation.velocity check + @classmethod + 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 + flyFormationMsg = cls.fly_formation_msg.SerializeToString() + cls.mqtt_Pub(message=flyFormationMsg, topics=cls.Fly_Formation_topicToMqtt, qos=cls.Fly_Formation_topicToMqtt_QOS) + + + + @classmethod + # publish a message + def mqtt_Pub(cls, message:bytes, topics:str, waitForAck:bool=False, qos:int=0)->None: + mid = cls.client.publish(topics, message, qos)[1] + # print(f"just published {message} to topic") + if waitForAck: + while mid not in cls.client.topic_ack: + print("wait for ack") + time.sleep(0.25) + cls.client.topic_ack.remove(mid) + + + + +class Json_msg_from_ros: + data = {} + client = None + #Mqtt + Flight_Information_topicToMqtt = None + Fly_Formation_topicToMqtt = None + Fly_Formation_topicToMqtt_QOS = None + + @classmethod + def callBack_imu(cls, 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} + cls.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') + + @classmethod + def callBack_gps(cls, GPS): + lat=int(GPS.latitude*10000000) #change the scale to centimeters + lon=int(GPS.longitude*10000000) + alt=int(GPS.altitude*100) + dataGpsUpdate = {"lat": lat, "lon": lon, "alt": alt} + cls.data.update(dataGpsUpdate) + # dataJsonFormate = orjson.dumps(data) + # mqtt_Pub(message=dataJsonFormate) + # print ('lat:'+lat+'\n'+'lon:'+lon+'\n') + + @classmethod + def callBack_state(cls, state): + mode = state.mode + dataGpsUpdate = {"mode": mode} + cls.data.update(dataGpsUpdate) + + @classmethod + def callBack_compass_hdg(cls, Compass): + heading = int(Compass.data*100) + dataGpsUpdate = {"heading": heading} + cls.data.update(dataGpsUpdate) + dataJsonFormate = orjson.dumps(cls.data).decode("UTF-8") + cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Flight_Information_topicToMqtt) + + + @classmethod + def mqtt_Pub(cls, message:str, topics:str, waitForAck:bool=False, qos:int=0): + mid = cls.client.publish(topics, message, qos)[1] + # print(f"just published {message} to topic") + if waitForAck: + while mid not in cls.client.topic_ack: + print("wait for ack") + time.sleep(0.25) + cls.client.topic_ack.remove(mid) \ No newline at end of file diff --git a/Stream/uav_proto_msg/utils/readConfig.py b/Stream/uav_proto_msg/utils/readConfig.py new file mode 100644 index 0000000..c94a9aa --- /dev/null +++ b/Stream/uav_proto_msg/utils/readConfig.py @@ -0,0 +1,65 @@ +import yaml + + +class MQTT_ROS_Config: + """ + MQTT_ROS_Config configuration class + """ + # class variables + sectionName='MQTT_ROS' + options={ + 'msg_format': (str,True), + 'MQTTClientNamePub': (str,True), + 'MQTTClientNameSub': (str,True), + 'host': (str,True), + 'port': (int,True), + 'keepalive': (int,True), + 'willTopic':(str,True), + 'lwt':(str, True), + 'willRetain':(bool,False), + 'willTopicQOS':(int,True), + 'Flight_Information_topicToMqtt': (str,False), + 'Fly_Formation_topicToMqtt': (str,False), + 'Fly_Formation_topicToMqtt_QOS':(int,False), + 'ROSClientNamePub': (str,False), + 'ROSClientNameSub': (str,False), + 'ROStopicName_Flight_Information': (str,False), + 'ROStopicName_Fly_Formation': (str,False)} + + #constructor + def __init__(self, inFileName): + #read YAML config and get EV3 section + infile=open(inFileName,'r') + ymlcfg=yaml.safe_load(infile) + infile.close() + eccfg=ymlcfg.get(self.sectionName,None) + if eccfg is None: raise Exception('Missing {} section in cfg file'.format(self.sectionName)) + + #iterate over options + for opt in self.options: + if opt in eccfg: + optval=eccfg[opt] + + #verify parameter type + if type(optval) != self.options[opt][0]: + raise Exception('Parameter "{}" has wrong type'.format(opt)) + + #create attributes on the fly + setattr(self,opt,optval) + else: + if self.options[opt][1]: + raise Exception('Missing mandatory parameter "{}"'.format(opt)) + else: + setattr(self,opt,None) + + #string representation for class data + def __str__(self): + return str(yaml.dump(self.__dict__,default_flow_style=False)) + + + +if __name__ == '__main__': + cfg=MQTT_ROS_Config("mqttConfig.yml") + print(cfg) + +