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 0000000..da74d38 Binary files /dev/null and b/Stream/uav_proto_msg/utils/__init__.pyc differ diff --git a/Stream/uav_proto_msg/utils/__pycache__/__init__.cpython-310.pyc b/Stream/uav_proto_msg/utils/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000..e72b28a Binary files /dev/null and b/Stream/uav_proto_msg/utils/__pycache__/__init__.cpython-310.pyc differ 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 0000000..4bfc01f Binary files /dev/null and b/Stream/uav_proto_msg/utils/__pycache__/basicMqtt.cpython-310.pyc differ 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 0000000..1c49bea Binary files /dev/null and b/Stream/uav_proto_msg/utils/__pycache__/protoJson_mqtt_pub_data_to_ros.cpython-310.pyc differ 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 0000000..ac08e79 Binary files /dev/null and b/Stream/uav_proto_msg/utils/__pycache__/protoJson_mqtt_sub_data_from_ros.cpython-310.pyc differ 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 0000000..0927063 Binary files /dev/null and b/Stream/uav_proto_msg/utils/__pycache__/readConfig.cpython-310.pyc differ diff --git a/Stream/uav_proto_msg/utils/basicMqtt.py b/Stream/uav_proto_msg/utils/basicMqtt.py new file mode 100644 index 0000000..572e84e --- /dev/null +++ b/Stream/uav_proto_msg/utils/basicMqtt.py @@ -0,0 +1,17 @@ +import paho.mqtt.client as mqtt +import time +class MQTTClient(mqtt.Client): + + def __init__(self,cname,**kwargs): + super().__init__(cname,**kwargs) + self.last_pub_time=time.time() + self.topic_ack=[] + self.run_flag=True + self.subscribe_flag=False + self.bad_connection_flag=False + self.connected_flag=True + self.disconnect_flag=False + self.disconnect_time=0.0 + self.pub_msg_count=0 + self.devices=[] + diff --git a/Stream/uav_proto_msg/utils/mqttConfig.yml b/Stream/uav_proto_msg/utils/mqttConfig.yml new file mode 100644 index 0000000..f30adcf --- /dev/null +++ b/Stream/uav_proto_msg/utils/mqttConfig.yml @@ -0,0 +1,21 @@ +MQTT_ROS: + msg_format: Proto + MQTTClientNamePub: Drone650Pub + MQTTClientNameSub: Drone650Sub + host: 192.168.50.117 + port: 1883 + keepalive: 60 + willTopic: CheckDoneConnect + willTopicQOS: 1 + lwt: Drone650 Gone Offline + willRetain: False + # Mqtt topic + Flight_Information_topicToMqtt: drone/leader/Flight_Information + Fly_Formation_topicToMqtt: drone/leader/Formation + # Change formate qos + Fly_Formation_topicToMqtt_QOS: 2 + #ROS + ROSClientNamePub: Drone650Pub + ROSClientNameSub: Drone650Sub + ROStopicName_Flight_Information: Flight_Information_reciver + ROStopicName_Fly_Formation: Fly_Formation_reciver \ No newline at end of file diff --git a/Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.py b/Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.py new file mode 100644 index 0000000..2eb6cc4 --- /dev/null +++ b/Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.py @@ -0,0 +1,60 @@ +import orjson +import time + +import proto.flight_information_pb2 as flight_information_pb2 +import proto.flyformatioln_pb2 as flyformatioln_pb2 +import google.protobuf.json_format as json_format + +class Proto_msg_to_ros: + #Protobuf + flight_information_msg = None + fly_formation_msg = None # delcare in function + + #Ros publisher + rate = None + publisher_Flight_Information = None + publisher_Fly_Formation = None + + #Mqtt topic: check data from which topic + Flight_Information_topicToMqtt = None + Fly_Formation_topicToMqtt = None + + #Proto + + @classmethod + def ros_pub(cls, dataProto): + if dataProto.topic == cls.Flight_Information_topicToMqtt: + 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() + 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.rate.sleep() + else: + print("topic not found") + + + +class Json_msg_to_ros: + rate = None + #Ros publisher + publisher_Flight_Information = None + publisher_Fly_Formation = None + #Mqtt topic: check data from which topic + Flight_Information_topicToMqtt = None + Fly_Formation_topicToMqtt = None + + @classmethod + def ros_pub(cls, dataJson): + if dataJson.topic == cls.Flight_Information_topicToMqtt: + 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 diff --git a/Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.pyc b/Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.pyc new file mode 100644 index 0000000..7cc220c Binary files /dev/null and b/Stream/uav_proto_msg/utils/protoJson_mqtt_pub_data_to_ros.pyc differ 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) + +