diff --git a/Stream/measure_rtl/allinone.bash b/Stream/RTL_test/MQTT_measure_rtl/allinone.bash similarity index 100% rename from Stream/measure_rtl/allinone.bash rename to Stream/RTL_test/MQTT_measure_rtl/allinone.bash diff --git a/Stream/measure_rtl/delayClientA_PUB.py b/Stream/RTL_test/MQTT_measure_rtl/delayClientA_PUB.py similarity index 100% rename from Stream/measure_rtl/delayClientA_PUB.py rename to Stream/RTL_test/MQTT_measure_rtl/delayClientA_PUB.py diff --git a/Stream/measure_rtl/delayClientA_SUB.py b/Stream/RTL_test/MQTT_measure_rtl/delayClientA_SUB.py similarity index 100% rename from Stream/measure_rtl/delayClientA_SUB.py rename to Stream/RTL_test/MQTT_measure_rtl/delayClientA_SUB.py diff --git a/Stream/measure_rtl/delayClientB.py b/Stream/RTL_test/MQTT_measure_rtl/delayClientB.py similarity index 100% rename from Stream/measure_rtl/delayClientB.py rename to Stream/RTL_test/MQTT_measure_rtl/delayClientB.py diff --git a/Stream/measure_rtl/delayData/measureDelay.py b/Stream/RTL_test/MQTT_measure_rtl/delayData/measureDelay.py similarity index 100% rename from Stream/measure_rtl/delayData/measureDelay.py rename to Stream/RTL_test/MQTT_measure_rtl/delayData/measureDelay.py diff --git a/Stream/measure_rtl/ex b/Stream/RTL_test/MQTT_measure_rtl/ex similarity index 100% rename from Stream/measure_rtl/ex rename to Stream/RTL_test/MQTT_measure_rtl/ex diff --git a/Stream/measure_rtl/proto/duration_pb2.py b/Stream/RTL_test/MQTT_measure_rtl/proto/duration_pb2.py similarity index 100% rename from Stream/measure_rtl/proto/duration_pb2.py rename to Stream/RTL_test/MQTT_measure_rtl/proto/duration_pb2.py diff --git a/Stream/measure_rtl/proto/flight_information.proto b/Stream/RTL_test/MQTT_measure_rtl/proto/flight_information.proto similarity index 100% rename from Stream/measure_rtl/proto/flight_information.proto rename to Stream/RTL_test/MQTT_measure_rtl/proto/flight_information.proto diff --git a/Stream/measure_rtl/proto/flight_information_pb2.py b/Stream/RTL_test/MQTT_measure_rtl/proto/flight_information_pb2.py similarity index 100% rename from Stream/measure_rtl/proto/flight_information_pb2.py rename to Stream/RTL_test/MQTT_measure_rtl/proto/flight_information_pb2.py diff --git a/Stream/measure_rtl/proto/flyformatioln.proto b/Stream/RTL_test/MQTT_measure_rtl/proto/flyformatioln.proto similarity index 100% rename from Stream/measure_rtl/proto/flyformatioln.proto rename to Stream/RTL_test/MQTT_measure_rtl/proto/flyformatioln.proto diff --git a/Stream/measure_rtl/proto/flyformatioln_pb2.py b/Stream/RTL_test/MQTT_measure_rtl/proto/flyformatioln_pb2.py similarity index 100% rename from Stream/measure_rtl/proto/flyformatioln_pb2.py rename to Stream/RTL_test/MQTT_measure_rtl/proto/flyformatioln_pb2.py diff --git a/Stream/measure_rtl/utils/__init__.py b/Stream/RTL_test/MQTT_measure_rtl/utils/__init__.py similarity index 100% rename from Stream/measure_rtl/utils/__init__.py rename to Stream/RTL_test/MQTT_measure_rtl/utils/__init__.py diff --git a/Stream/measure_rtl/utils/basicMqtt.py b/Stream/RTL_test/MQTT_measure_rtl/utils/basicMqtt.py similarity index 100% rename from Stream/measure_rtl/utils/basicMqtt.py rename to Stream/RTL_test/MQTT_measure_rtl/utils/basicMqtt.py diff --git a/Stream/measure_rtl/utils/mqttConfig_delayA_PUB.yml b/Stream/RTL_test/MQTT_measure_rtl/utils/mqttConfig_delayA_PUB.yml similarity index 100% rename from Stream/measure_rtl/utils/mqttConfig_delayA_PUB.yml rename to Stream/RTL_test/MQTT_measure_rtl/utils/mqttConfig_delayA_PUB.yml diff --git a/Stream/measure_rtl/utils/mqttConfig_delayA_SUB.yml b/Stream/RTL_test/MQTT_measure_rtl/utils/mqttConfig_delayA_SUB.yml similarity index 100% rename from Stream/measure_rtl/utils/mqttConfig_delayA_SUB.yml rename to Stream/RTL_test/MQTT_measure_rtl/utils/mqttConfig_delayA_SUB.yml diff --git a/Stream/measure_rtl/utils/mqttConfig_delayB_SUB.yml b/Stream/RTL_test/MQTT_measure_rtl/utils/mqttConfig_delayB_SUB.yml similarity index 100% rename from Stream/measure_rtl/utils/mqttConfig_delayB_SUB.yml rename to Stream/RTL_test/MQTT_measure_rtl/utils/mqttConfig_delayB_SUB.yml diff --git a/Stream/measure_rtl/utils/protoJson_delayClientA_PUB.py b/Stream/RTL_test/MQTT_measure_rtl/utils/protoJson_delayClientA_PUB.py similarity index 96% rename from Stream/measure_rtl/utils/protoJson_delayClientA_PUB.py rename to Stream/RTL_test/MQTT_measure_rtl/utils/protoJson_delayClientA_PUB.py index 68ae9dd..51f9b92 100644 --- a/Stream/measure_rtl/utils/protoJson_delayClientA_PUB.py +++ b/Stream/RTL_test/MQTT_measure_rtl/utils/protoJson_delayClientA_PUB.py @@ -55,7 +55,7 @@ class Proto_msg: while mid not in cls.client.topic_ack: # TODO: logging print("wait for ack") - time.sleep(0.25) + time.sleep(0.25)# You can download security extensions from the HiveMQ Marke cls.client.topic_ack.remove(mid) diff --git a/Stream/measure_rtl/utils/protoJson_delayClientA_SUB.py b/Stream/RTL_test/MQTT_measure_rtl/utils/protoJson_delayClientA_SUB.py similarity index 100% rename from Stream/measure_rtl/utils/protoJson_delayClientA_SUB.py rename to Stream/RTL_test/MQTT_measure_rtl/utils/protoJson_delayClientA_SUB.py diff --git a/Stream/measure_rtl/utils/protoJson_delayClientB_SUB.py b/Stream/RTL_test/MQTT_measure_rtl/utils/protoJson_delayClientB_SUB.py similarity index 100% rename from Stream/measure_rtl/utils/protoJson_delayClientB_SUB.py rename to Stream/RTL_test/MQTT_measure_rtl/utils/protoJson_delayClientB_SUB.py diff --git a/Stream/measure_rtl/utils/readConfig.py b/Stream/RTL_test/MQTT_measure_rtl/utils/readConfig.py similarity index 100% rename from Stream/measure_rtl/utils/readConfig.py rename to Stream/RTL_test/MQTT_measure_rtl/utils/readConfig.py diff --git a/Stream/RTL_test/uavlink_measrue_rtl/delayClientA_PUB.py b/Stream/RTL_test/uavlink_measrue_rtl/delayClientA_PUB.py new file mode 100755 index 0000000..2838604 --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/delayClientA_PUB.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python3 +#coding:utf-8 +import serial +import time +import sys +import os +import proto.flight_information_pb2 as flight_information_pb2 +import logging +from utils.readConfig import Read_PUB_Config +from utils.proto_delayClientA_PUB import Proto_msg_from_ros +import random +import time + +class fakeGps(): + def __init__(self): + self.latitude = 8.0 + self.longitude = 8.1 + self.altitude = 8.88 + +class fake_hdg(): + def __init__(self): + self.data = 1 + +def init_dataFormat(cfg:Read_PUB_Config): + + Proto_msg_from_ros.sel = sel + Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message() + + +if __name__ == '__main__': + FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_PUB.yml") + cfg = Read_PUB_Config(FilePath) + gps = fakeGps() + hdg = fake_hdg() + + # set log + stream_log_format = "%(asctime)s - %(levelname)s - %(message)s" + file_log_format = "%(message)s" + file_formatter = logging.Formatter(file_log_format) + stream_formatter = logging.Formatter(stream_log_format) + + stream_handler = logging.StreamHandler() + stream_handler.setFormatter(stream_formatter) + stream_handler.setLevel(logging.DEBUG) + + file_handler = logging.FileHandler(cfg.logFileName, mode='w') + file_handler.setFormatter(file_formatter) + file_handler.setLevel(logging.INFO) + + logger = logging.getLogger("__UAVLINKSUBPUB__") + logger.setLevel(logging.DEBUG) + logger.addHandler(file_handler) + logger.addHandler(stream_handler) + logger.debug(cfg) + + sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE) + init_dataFormat(cfg) + + + while hdg.data <= 100: + gps.latitude = random.uniform(10, 100) + gps.longitude = random.uniform(10, 100) + gps.altitude = random.uniform(10, 100) + + + try: + # test json + Proto_msg_from_ros.callBack_gps(gps) + Proto_msg_from_ros.callBack_compass_hdg(hdg) + time.sleep(0.5) + hdg.data += 1 + except KeyboardInterrupt as e: + sel.write(b'\xf2' + b'......................' + b'\x0d\x2a') + readTenByte = sel.read_until(size=5) + + print("End of program") + sys.exit() \ No newline at end of file diff --git a/Stream/RTL_test/uavlink_measrue_rtl/delayClientA_SUB.py b/Stream/RTL_test/uavlink_measrue_rtl/delayClientA_SUB.py new file mode 100755 index 0000000..a489fbc --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/delayClientA_SUB.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 +#coding:utf-8 +import serial +import time +import sys +import os +import proto.flight_information_pb2 as flight_information_pb2 +import logging +from utils.readConfig import Read_SUB_Config +from utils.proto_delayClientA_SUB import Proto_msg_to_ros +import google.protobuf.message + + +def init_dataFormat(cfg:Read_SUB_Config): + # ros_namespace="/drone1" + # deviceData = b'\xf12222222222222222222222\r\x1a' + + # sel.write(deviceData) + # time.sleep(1) + # readTenByte = sel.read(size=5) + # print(readTenByte) + + + Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message() + + +if __name__ == '__main__': + FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_SUB.yml") + cfg = Read_SUB_Config(FilePath) + + sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE) + # set log + stream_log_format = "%(asctime)s - %(levelname)s - %(message)s" + file_log_format = "%(message)s" + file_formatter = logging.Formatter(file_log_format) + stream_formatter = logging.Formatter(stream_log_format) + + stream_handler = logging.StreamHandler() + stream_handler.setFormatter(stream_formatter) + stream_handler.setLevel(logging.DEBUG) + + file_handler = logging.FileHandler(cfg.logFileName, mode='w') + file_handler.setFormatter(file_formatter) + file_handler.setLevel(logging.INFO) + + logger = logging.getLogger("__UAVLINKSUB__") + logger.setLevel(logging.DEBUG) + logger.addHandler(file_handler) + logger.addHandler(stream_handler) + logger.debug(cfg) + + init_dataFormat(cfg) + + while True: + try: + # test json + if sel.in_waiting >= 25: + readTenByte = sel.read_until(expected= b'\x01\x18', size=25) + timeSub = time.perf_counter_ns() + Proto_msg_to_ros.on_message_Flight_Information(readTenByte, timeSub) + # time.sleep(3) + + except google.protobuf.message.DecodeError as e: + print(readTenByte) + print(e) + + + + + diff --git a/Stream/RTL_test/uavlink_measrue_rtl/delayClientB.py b/Stream/RTL_test/uavlink_measrue_rtl/delayClientB.py new file mode 100644 index 0000000..b5d1a05 --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/delayClientB.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 +#coding:utf-8 +import serial +import time +import sys +import os +import proto.flight_information_pb2 as flight_information_pb2 +import logging +from utils.readConfig import Read_SUB_Config +from utils.proto_delayClientB_SUB import Proto_msg_to_ros +import google.protobuf.message + + +def init_dataFormat(cfg:Read_SUB_Config): + # ros_namespace="/drone1" + # deviceData = b'\xf12222222222222222222222\r\x1a' + + # sel.write(deviceData) + # time.sleep(1) + # readTenByte = sel.read(size=5) + # print(readTenByte) + + Proto_msg_to_ros.sel = sel + Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message() + + +if __name__ == '__main__': + FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_SUB.yml") + cfg = Read_SUB_Config(FilePath) + + # set log + stream_log_format = "%(asctime)s - %(levelname)s - %(message)s" + file_log_format = "%(message)s" + file_formatter = logging.Formatter(file_log_format) + stream_formatter = logging.Formatter(stream_log_format) + + stream_handler = logging.StreamHandler() + stream_handler.setFormatter(stream_formatter) + stream_handler.setLevel(logging.DEBUG) + + file_handler = logging.FileHandler(cfg.logFileName, mode='w') + file_handler.setFormatter(file_formatter) + file_handler.setLevel(logging.INFO) + + logger = logging.getLogger("__UAVLINKSUB__") + logger.setLevel(logging.DEBUG) + logger.addHandler(file_handler) + logger.addHandler(stream_handler) + logger.debug(cfg) + last_packet = None + count = 0 + sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE) + init_dataFormat(cfg) + + while True: + try: + # test json + if sel.in_waiting >= 25: + readTenByte = sel.read_until(expected= b'\x01\x17', size=25) + if readTenByte == last_packet: + count += 1 + continue + last_packet = readTenByte + timeSub = time.perf_counter_ns() + Proto_msg_to_ros.on_message_Flight_Information(readTenByte, timeSub) + except google.protobuf.message.DecodeError as e: + logger.debug(readTenByte) + logger.debug(len(readTenByte)) + logger.debug(e) + logger.debug(count) + + + + + + diff --git a/Stream/RTL_test/uavlink_measrue_rtl/delayData/measureDelay.py b/Stream/RTL_test/uavlink_measrue_rtl/delayData/measureDelay.py new file mode 100644 index 0000000..a26853a --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/delayData/measureDelay.py @@ -0,0 +1,72 @@ +import pandas as pd +import matplotlib.pyplot as plt +import numpy as np +import argparse +import sys + +def checkNumberofLines(df, log_name): + if df.shape[0] != args.number: + print(f"Warning: Number of lines in the log file {log_name} is not correct") + # check if the count is continuous + checkContinuousCount(df, log_file) + return False + else: + return True + +def checkContinuousCount(df, log_name): + duplicates = df[df["count"].duplicated()]["count"] + if not duplicates.empty: + print(f"Warning: Non-continuous count in the log file {log_name} at index {duplicates.index.tolist()}") + max_count = args.number + all_counts = set(range(1, max_count + 1)) + actual_counts = set(df["count"]) + missing = all_counts - actual_counts + if missing != set(): + print(f"Warning: missing msg in {log_name} at index {missing}") + +def filter_dataframes(dataframes): + common_counts = set(dataframes[0]["count"]) & set(dataframes[1]["count"]) & set(dataframes[2]["count"]) + return [df[df["count"].isin(common_counts)].reset_index(drop=True) for df in dataframes] +''' +df["count"].isin(common_counts): This part creates a boolean Series where each value is True if the corresponding "count" value is present in common_counts, and False otherwise. + +df[df["count"].isin(common_counts)]: This part filters the DataFrame df using the boolean Series created in the previous step, keeping only the rows where the "count" value is present in common_counts. + +df[df["count"].isin(common_counts)].reset_index(drop=True): This part resets the index of the filtered DataFrame, dropping the old index and creating a new one with default values (0, 1, 2, ...). The drop=True argument ensures that the old index is not added as a new column in the resulting DataFrame. +''' + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-n", "--number", type=int, default="1000", help="number of packet", required=True) + parser.add_argument("-t", "--test", type=str, default="1", help="number of test", required=True) + args = parser.parse_args() + + log_files = [f"rtl/test{args.test}/ProtodelayA_PUB.log", f"rtl/test{args.test}/ProtodelayA_SUB.log", f"rtl/test{args.test}/ProtodelayB_SUB.log"] + dataframes = [] + dataerrorFlag = [] + for log_file in log_files: + df = pd.read_csv(log_file, delim_whitespace=True, header=None, names=["count", "time"]) + dataframes.append(df) + + # check if the number of lines is correct + dataerrorFlag.append(checkNumberofLines(df, log_file)) + + if all(dataerrorFlag) == False: + dataframes = filter_dataframes(dataframes) + + df_Proto_delayA_PUB, df_Proto_delayA_SUB, df_Proto_delayB_SUB = dataframes + + # print the resulting DataFrame + diff_Proto_df = pd.DataFrame({"Difftime":(df_Proto_delayA_SUB["time"] - df_Proto_delayA_PUB["time"] - df_Proto_delayB_SUB["time"])/1e9/2}) + + array_Proto = diff_Proto_df["Difftime"].to_numpy() + + avg_Proto = np.average(array_Proto) + + fig = plt.figure() + plt.plot(array_Proto, color="blue", label=f"Avg_delay: {avg_Proto}") + + plt.legend() + plt.xlabel("Number of messages") + plt.ylabel("Delay (s)") + plt.savefig(f"rtl/test{args.test}/test{args.test}.png", dpi=100) \ No newline at end of file diff --git a/Stream/RTL_test/uavlink_measrue_rtl/proto/duration_pb2.py b/Stream/RTL_test/uavlink_measrue_rtl/proto/duration_pb2.py new file mode 100644 index 0000000..1a30c51 --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/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/RTL_test/uavlink_measrue_rtl/proto/flight_information.proto b/Stream/RTL_test/uavlink_measrue_rtl/proto/flight_information.proto new file mode 100644 index 0000000..d377017 --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/proto/flight_information.proto @@ -0,0 +1,14 @@ +syntax = 'proto3'; + +// GPS + compass + +message GPS { + float LAT = 1; + float LON = 2; + float ALT = 3; +} + +message flight_information_message { + GPS gps = 1; + float heading = 2; +} \ No newline at end of file diff --git a/Stream/RTL_test/uavlink_measrue_rtl/proto/flight_information_pb2.py b/Stream/RTL_test/uavlink_measrue_rtl/proto/flight_information_pb2.py new file mode 100644 index 0000000..24e9adc --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/proto/flight_information_pb2.py @@ -0,0 +1,130 @@ +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: flight_information.proto + +import sys +_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) +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='flight_information.proto', + package='', + syntax='proto3', + serialized_options=None, + serialized_pb=_b('\n\x18\x66light_information.proto\",\n\x03GPS\x12\x0b\n\x03LAT\x18\x01 \x01(\x02\x12\x0b\n\x03LON\x18\x02 \x01(\x02\x12\x0b\n\x03\x41LT\x18\x03 \x01(\x02\"@\n\x1a\x66light_information_message\x12\x11\n\x03gps\x18\x01 \x01(\x0b\x32\x04.GPS\x12\x0f\n\x07heading\x18\x02 \x01(\x02\x62\x06proto3') +) + + + + +_GPS = _descriptor.Descriptor( + name='GPS', + full_name='GPS', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='LAT', full_name='GPS.LAT', index=0, + number=1, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='LON', full_name='GPS.LON', index=1, + number=2, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='ALT', full_name='GPS.ALT', index=2, + number=3, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=28, + serialized_end=72, +) + + +_FLIGHT_INFORMATION_MESSAGE = _descriptor.Descriptor( + name='flight_information_message', + full_name='flight_information_message', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='gps', full_name='flight_information_message.gps', index=0, + number=1, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='heading', full_name='flight_information_message.heading', index=1, + number=2, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=74, + serialized_end=138, +) + +_FLIGHT_INFORMATION_MESSAGE.fields_by_name['gps'].message_type = _GPS +DESCRIPTOR.message_types_by_name['GPS'] = _GPS +DESCRIPTOR.message_types_by_name['flight_information_message'] = _FLIGHT_INFORMATION_MESSAGE +_sym_db.RegisterFileDescriptor(DESCRIPTOR) + +GPS = _reflection.GeneratedProtocolMessageType('GPS', (_message.Message,), dict( + DESCRIPTOR = _GPS, + __module__ = 'flight_information_pb2' + # @@protoc_insertion_point(class_scope:GPS) + )) +_sym_db.RegisterMessage(GPS) + +flight_information_message = _reflection.GeneratedProtocolMessageType('flight_information_message', (_message.Message,), dict( + DESCRIPTOR = _FLIGHT_INFORMATION_MESSAGE, + __module__ = 'flight_information_pb2' + # @@protoc_insertion_point(class_scope:flight_information_message) + )) +_sym_db.RegisterMessage(flight_information_message) + + +# @@protoc_insertion_point(module_scope) diff --git a/Stream/RTL_test/uavlink_measrue_rtl/proto/flyformatioln.proto b/Stream/RTL_test/uavlink_measrue_rtl/proto/flyformatioln.proto new file mode 100644 index 0000000..56a6d58 --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/proto/flyformatioln.proto @@ -0,0 +1,17 @@ +syntax = "proto3"; +package fly_formation_proto; +// 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/RTL_test/uavlink_measrue_rtl/proto/flyformatioln_pb2.py b/Stream/RTL_test/uavlink_measrue_rtl/proto/flyformatioln_pb2.py new file mode 100644 index 0000000..34014f4 --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/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\x12\x13\x66ly_formation_proto\"v\n\x15\x66ly_formation_message\x12\x15\n\x08velocity\x18\x01 \x01(\x02H\x00\x88\x01\x01\x12\x39\n\rfly_formation\x18\x02 \x01(\x0e\x32\".fly_formation_proto.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=165 + _FLY_FORMATION._serialized_end=344 + _FLY_FORMATION_MESSAGE._serialized_start=44 + _FLY_FORMATION_MESSAGE._serialized_end=162 +# @@protoc_insertion_point(module_scope) diff --git a/Stream/RTL_test/uavlink_measrue_rtl/utils/__init__.py b/Stream/RTL_test/uavlink_measrue_rtl/utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/Stream/RTL_test/uavlink_measrue_rtl/utils/basicMqtt.py b/Stream/RTL_test/uavlink_measrue_rtl/utils/basicMqtt.py new file mode 100644 index 0000000..572e84e --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/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/RTL_test/uavlink_measrue_rtl/utils/proto_delayClientA_PUB.py b/Stream/RTL_test/uavlink_measrue_rtl/utils/proto_delayClientA_PUB.py new file mode 100644 index 0000000..d412a67 --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/utils/proto_delayClientA_PUB.py @@ -0,0 +1,28 @@ +import orjson +import time +import logging + +logger = logging.getLogger("__UAVLINKSUBPUB__") + +class Proto_msg_from_ros: + #Protobuf + flight_information_msg = None + sel = None + + + @classmethod + def callBack_gps(cls, GPS): + cls.flight_information_msg.gps.LAT = GPS.latitude + cls.flight_information_msg.gps.LON = GPS.longitude + cls.flight_information_msg.gps.ALT = GPS.altitude + + + @classmethod + def callBack_compass_hdg(cls, Compass): + cls.flight_information_msg.heading = Compass.data + flightInformationMsg = cls.flight_information_msg.SerializeToString() + cls.sel.write(b'\xf2' + flightInformationMsg + b'\x0d\x0a') + timePub = time.perf_counter_ns() + logger.info("{} {}".format(Compass.data,timePub)) + + diff --git a/Stream/RTL_test/uavlink_measrue_rtl/utils/proto_delayClientA_SUB.py b/Stream/RTL_test/uavlink_measrue_rtl/utils/proto_delayClientA_SUB.py new file mode 100644 index 0000000..d64cb31 --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/utils/proto_delayClientA_SUB.py @@ -0,0 +1,25 @@ +import time + +import proto.flight_information_pb2 as flight_information_pb2 +import google.protobuf.json_format as json_format +import logging + +# TODO: use native ros type instead of json or str +logger = logging.getLogger("__UAVLINKSUB__") + +class Proto_msg_to_ros: + #Protobuf + flight_information_msg = None + + #Ros publisher + rate = None + publisher_Flight_Information = None + + + #Proto + @classmethod + def on_message_Flight_Information(cls, msg, timeSub): + proto = msg[1:-2] + proto_msg = cls.flight_information_msg.FromString(proto) + count = proto_msg.heading + logger.info("{} {}".format(count,timeSub)) \ No newline at end of file diff --git a/Stream/RTL_test/uavlink_measrue_rtl/utils/proto_delayClientB_SUB.py b/Stream/RTL_test/uavlink_measrue_rtl/utils/proto_delayClientB_SUB.py new file mode 100644 index 0000000..932a684 --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/utils/proto_delayClientB_SUB.py @@ -0,0 +1,32 @@ +import time + +import proto.flight_information_pb2 as flight_information_pb2 +import google.protobuf.json_format as json_format +import logging + +# TODO: use native ros type instead of json or str +logger = logging.getLogger("__UAVLINKSUB__") + +class Proto_msg_to_ros: + #Protobuf + flight_information_msg = None + + #Ros publisher + rate = None + publisher_Flight_Information = None + sel = None + # count = None + + #Proto + @classmethod + def on_message_Flight_Information(cls, msg, timeSub): + proto = msg[1:-2] + proto_msg = cls.flight_information_msg.FromString(proto) + # cls.sel.write(b'\xf2' + proto + b'\x0d\x0a') + timePub = time.perf_counter_ns() + # readTenByte = cls.sel.read_until(size=5) + timeDiff = timePub - timeSub + # cls.count = proto_msg.heading + logger.info("{} {}".format(proto_msg.heading, timeDiff)) + + \ No newline at end of file diff --git a/Stream/RTL_test/uavlink_measrue_rtl/utils/readConfig.py b/Stream/RTL_test/uavlink_measrue_rtl/utils/readConfig.py new file mode 100644 index 0000000..54386da --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/utils/readConfig.py @@ -0,0 +1,152 @@ +import yaml + +class Config: + def __init__(self, inFileName): + self.sectionNames = ["MQTT","ROS", "LOG", "UAVLINK"] + self.options = {} + self.inFileName = inFileName + + def setAttribute(self): + with open(self.inFileName,"r") as f: + self.ymlcfg=yaml.safe_load(f) + + ecfgs = [self.ymlcfg.get(name) for name in self.sectionNames] + if None in ecfgs: + nameIndex = ecfgs.index(None) + raise Exception("Missing {} section in cfg file".format(self.sectionNames[nameIndex])) + #iterate over options + for opts, ecfg in zip(self.options, ecfgs): + for opt in self.options[opts]: + if opt in ecfg: + optval=ecfg[opt] + #verify parameter type + if type(optval) != self.options[opts][opt][0]: + raise Exception("Parameter {} has wrong type".format(self.opt)) + + #create attributes on the fly + setattr(self,opt,optval) + else: + if self.options[opts][opt][1]: + raise Exception("Missing mandatory parameter {}".format(self.opt)) + else: + setattr(self,opt,None) + + def __str__(self): + return str(yaml.dump(self.ymlcfg, default_flow_style=False)) + + +class Read_PUB_Config(Config): + def setAttribute(self): + super().setAttribute() + + def __init__(self, inFileName): + super().__init__(inFileName) + self.options = { + self.sectionNames[0]:{ + "msg_format": (str,False), + "MQTTClientNamePub": (str,False), + "host": (str,False), + "port": (int,False), + "keepalive": (int,False), + "willTopic":(str,False), + "lwt":(str, False), + "willRetain":(bool,False), + "willTopicQOS":(int,False), + "Flight_Information_topicToMqtt": (str,False), + "Fly_Formation_topicToMqtt": (str,False), + "Fly_Formation_topicToMqtt_QOS":(int,False)}, + self.sectionNames[1]:{ + "ROSClientNamePub": (str,True), + "ROStopicName_Flight_Information": (str,False), + "ROStopicName_Fly_Formation": (str,False)}, + self.sectionNames[2]:{ + "logFileName":(str,True)}, + self.sectionNames[3]:{ + "uavlink_msg_format": (str,False), + "uav_id": (str,False), + "baudrate": (int,False), + "ttyport": (str,False)}} + self.setAttribute() + + def __str__(self): + return super().__str__() + +class Read_SUB_Config(Config): + def setAttribute(self): + super().setAttribute() + + def __init__(self, inFileName): + super().__init__(inFileName) + self.options = { + self.sectionNames[0]:{ + "msg_format": (str,False), + "MQTTClientNameSub": (str,False), + "host": (str,False), + "port": (int,False), + "keepalive": (int,False), + "willTopic":(str,False), + "lwt":(str, False), + "willRetain":(bool,False), + "willTopicQOS":(int,False), + "Drone550_Flight_Information_topicToMqtt": (str,False), + "Drone380_Flight_Information_topicToMqtt":(str,False), + "Drone650_Flight_Information_topicToMqtt":(str,False), + "Drone888_Flight_Information_topicToMqtt":(str,False), + "Drone555_Flight_Information_topicToMqtt":(str,False), + "Fly_Formation_topicToMqtt": (str,False), + "Fly_Formation_topicToMqtt_QOS":(int,False)}, + self.sectionNames[1]:{ + "ROSClientNameSub": (str,True), + "Dron550_ROStopicName_Flight_Information": (str,False), + "Dron380_ROStopicName_Flight_Information": (str,False), + "Dron380_ROStopicName_Flight_Information": (str,False), + "Dron650_ROStopicName_Flight_Information": (str,False), + "Dron888_ROStopicName_Flight_Information": (str,False), + "Dron555_ROStopicName_Flight_Information": (str,False), + "Dron_ROStopicName_Flight_Information": (str,False), + "ROStopicName_Fly_Formation": (str,False)}, + self.sectionNames[2]:{ + "logFileName":(str,False)}, + self.sectionNames[3]:{ + "uavlink_msg_format": (str,False), + "uav_id": (str,False), + "baudrate": (int,False), + "ttyport": (str,False)}} + self.setAttribute() + + def __str__(self): + return super().__str__() + +class Read_CMD_Config(Config): + def setAttribute(self): + super().setAttribute() + + def __init__(self, inFileName): + super().__init__(inFileName) + self.options = { + self.sectionNames[0]:{ + "msg_format": (str,True), + "MQTTClientNameCmd": (str,True), + "host": (str,True), + "port": (int,True), + "keepalive": (int,True), + "Cmd_Broadcast_topicToMqtt": (str,True), + "Cmd_Direct_topicToMqtt": (str,False), + "Cmd_Broadcast_topicToMqtt_QOS":(int,True), + "Cmd_Direct_topicToMqtt_QOS":(int,False)}, + self.sectionNames[1]:{ + "ROSClientNameCmd": (str,True), + "ROStopicName_Cmd_Broadcast_Receiver": (str,True), + "ROStopicName_Cmd_Direct_Receiver": (str,False)}, + self.sectionNames[2]:{ + "logFileName":(str,False)}} + self.setAttribute() + + def __str__(self): + return super().__str__() + +if __name__ == "__main__": + cfg=Read_CMD_Config("mqttConfig_CMD.yml") + print(cfg) + + diff --git a/Stream/RTL_test/uavlink_measrue_rtl/utils/uavlinkConfig_PUB.yml b/Stream/RTL_test/uavlink_measrue_rtl/utils/uavlinkConfig_PUB.yml new file mode 100644 index 0000000..e98b1dc --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/utils/uavlinkConfig_PUB.yml @@ -0,0 +1,12 @@ +UAVLINK: + uavlink_msg_format: Proto + uav_id: \x01\x01 + baudrate: 250000 + ttyport: /dev/ttyUSB3 +MQTT: "None" +#ROS +ROS: + ROSClientNamePub: Drone550UAVLINKPub + ROStopicName_Flight_Information: Flight_Information_reciver +LOG: + logFileName: ProtodelayA_PUB.log diff --git a/Stream/RTL_test/uavlink_measrue_rtl/utils/uavlinkConfig_SUB.yml b/Stream/RTL_test/uavlink_measrue_rtl/utils/uavlinkConfig_SUB.yml new file mode 100644 index 0000000..4b57446 --- /dev/null +++ b/Stream/RTL_test/uavlink_measrue_rtl/utils/uavlinkConfig_SUB.yml @@ -0,0 +1,11 @@ +UAVLINK: + uavlink_msg_format: Proto + uav_id: \x01\x01 + baudrate: 250000 + ttyport: /dev/ttyUSB3 +MQTT: "None" +ROS: + ROSClientNameSub: Drone550UAVLINKSub + Dron550_ROStopicName_Flight_Information: Flight_Information_reciver +LOG: + logFileName: ProtodelayA_SUB.log \ No newline at end of file diff --git a/Stream/uav_proto_msg/mqttSubMain.py b/Stream/uav_proto_msg/mqttSubMain.py index cd88833..d2d5d2c 100644 --- a/Stream/uav_proto_msg/mqttSubMain.py +++ b/Stream/uav_proto_msg/mqttSubMain.py @@ -20,20 +20,20 @@ def init_dataFormat(cfg:utils.Read_SUB_Config): utils.Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message() utils.Proto_msg_to_ros.fly_formation_msg = flyformatioln_pb2.fly_formation_message() - client.message_callback_add(cfg.Flight_Information_topicToMqtt, utils.Proto_msg_to_ros.on_message_Flight_Information) + client.message_callback_add(cfg.Drone550_Flight_Information_topicToMqtt, utils.Proto_msg_to_ros.on_message_Flight_Information) client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Proto_msg_to_ros.on_message_Fly_Formation) - utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt + utils.Proto_msg_to_ros.Drone550_Flight_Information_topicToMqtt = cfg.Drone550_Flight_Information_topicToMqtt utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt elif cfg.msg_format == "Json": # utils.Json_msg_to_ros.rate = rospy.Rate(10) # utils.Json_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10) # utils.Json_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10) - - client.message_callback_add(cfg.Flight_Information_topicToMqtt, utils.Json_msg_to_ros.on_message_Flight_Information) + + client.message_callback_add(cfg.Drone550_Flight_Information_topicToMqtt, utils.Json_msg_to_ros.on_message_Flight_Information) client.message_callback_add(cfg.Fly_Formation_topicToMqtt, utils.Json_msg_to_ros.on_message_Fly_Formation) - utils.Json_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt + utils.Json_msg_to_ros.Drone550_Flight_Information_topicToMqtt = cfg.Drone550_Flight_Information_topicToMqtt utils.Json_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt else: logging.debug("msg_format not found") @@ -41,7 +41,7 @@ def init_dataFormat(cfg:utils.Read_SUB_Config): def on_connect(self, userdata, flags, rc): logger.info("Connected with result code " + str(rc)) - client.subscribe(cfg.Flight_Information_topicToMqtt) + client.subscribe(cfg.Drone550_Flight_Information_topicToMqtt) client.subscribe(cfg.Fly_Formation_topicToMqtt) diff --git a/Stream/uav_proto_msg/uavlinkPubMain.py b/Stream/uav_proto_msg/uavlinkPubMain.py new file mode 100755 index 0000000..34bc9c0 --- /dev/null +++ b/Stream/uav_proto_msg/uavlinkPubMain.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 +#coding:utf-8 +import serial +import time +import sys +import os +import proto.flight_information_pb2 as flight_information_pb2 +import logging +from utils.readConfig import Read_PUB_Config +from utils.proto_uavlink_sub_data_from_ros import Proto_msg_from_ros +import random + +class fakeGps(): + def __init__(self): + self.latitude = 8.0 + self.longitude = 8.1 + self.altitude = 8.88 + +class fake_hdg(): + def __init__(self): + self.data = 40 + + +def init_dataFormat(cfg:Read_PUB_Config): + # f1data = b'\xf1drone550pub...........\r\n' + sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, timeout=0.3) + # sel.write(f1data) + # readTenByte = sel.readline() + # print(readTenByte) + Proto_msg_from_ros.sel = sel + Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message() + +if __name__ == '__main__': + FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_PUB.yml") + cfg = Read_PUB_Config(FilePath) + gps = fakeGps() + hdg = fake_hdg() + + # set log + log_format = "%(asctime)s - %(levelname)s - %(message)s" + formatter = logging.Formatter(log_format) + + stream_handler = logging.StreamHandler() + stream_handler.setFormatter(formatter) + stream_handler.setLevel(logging.DEBUG) + + file_handler = logging.FileHandler(cfg.logFileName) + file_handler.setFormatter(formatter) + file_handler.setLevel(logging.INFO) + + logger = logging.getLogger("__UAVLINKSUBPUB__") + logger.setLevel(logging.INFO) + logger.addHandler(file_handler) + logger.addHandler(stream_handler) + logger.info(cfg) + + + init_dataFormat(cfg) + while True: + + gps.latitude = random.uniform(10, 100) + gps.longitude = random.uniform(10, 100) + gps.altitude = random.uniform(10, 100) + + hdg.data = random.uniform(0, 10) + try: + # test json + Proto_msg_from_ros.callBack_gps(gps) + Proto_msg_from_ros.callBack_compass_hdg(hdg) + time.sleep(0.3) + + + except KeyboardInterrupt as e: + print("End of program") + sys.exit() + + + diff --git a/Stream/uav_proto_msg/uavlinkSubMain.py b/Stream/uav_proto_msg/uavlinkSubMain.py new file mode 100755 index 0000000..b7f7558 --- /dev/null +++ b/Stream/uav_proto_msg/uavlinkSubMain.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 +#coding:utf-8 +import serial +import time +import sys +import os +import proto.flight_information_pb2 as flight_information_pb2 +import logging +from utils.readConfig import Read_SUB_Config +from utils.proto_uavlink_pub_data_to_ros import Proto_msg_to_ros + + + +def init_dataFormat(cfg:Read_SUB_Config): + ros_namespace="/drone1" + deviceData = b'\xf12222222222222222222222\r\x1a' + + sel.write(deviceData) + time.sleep(1) + readTenByte = sel.read(size=5) + print(readTenByte) + + + Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message() + + +if __name__ == '__main__': + FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_SUB.yml") + cfg = Read_SUB_Config(FilePath) + + sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, timeout = 0.5) + # set log + log_format = "%(asctime)s - %(levelname)s - %(message)s" + formatter = logging.Formatter(log_format) + + stream_handler = logging.StreamHandler() + stream_handler.setFormatter(formatter) + stream_handler.setLevel(logging.DEBUG) + + file_handler = logging.FileHandler(cfg.logFileName) + file_handler.setFormatter(formatter) + file_handler.setLevel(logging.INFO) + + logger = logging.getLogger("__UAVLINKSUB__") + logger.setLevel(logging.INFO) + logger.addHandler(file_handler) + logger.addHandler(stream_handler) + logger.info(cfg) + + init_dataFormat(cfg) + + while True: + try: + # test json + if sel.in_waiting >= 25: + readTenByte = sel.read_until(expected= b'\x01\x01', size=25) + # print(readTenByte) + if b"drone550" in readTenByte: + print(readTenByte) + print("radar") + continue + + Proto_msg_to_ros.on_message_Flight_Information(readTenByte) + + except Exception as e: + print("End of program") + print(readTenByte) + pass + + + + diff --git a/Stream/uav_proto_msg/utils/mqttConfig_CMD.yml b/Stream/uav_proto_msg/utils/mqttConfig_CMD.yml index 4bc2e12..0fe930e 100644 --- a/Stream/uav_proto_msg/utils/mqttConfig_CMD.yml +++ b/Stream/uav_proto_msg/utils/mqttConfig_CMD.yml @@ -13,3 +13,4 @@ ROS: ROStopicName_Cmd_Broadcast_Receiver: cmd_receiver LOG: logFileName: cmd.log +UAVLINK: "None" \ No newline at end of file diff --git a/Stream/uav_proto_msg/utils/mqttConfig_PUB.yml b/Stream/uav_proto_msg/utils/mqttConfig_PUB.yml index 6a665e4..4e90bb7 100644 --- a/Stream/uav_proto_msg/utils/mqttConfig_PUB.yml +++ b/Stream/uav_proto_msg/utils/mqttConfig_PUB.yml @@ -20,3 +20,4 @@ ROS: ROStopicName_Fly_Formation: Fly_Formation_reciver LOG: logFileName: pub.log +UAVLINK: "None" \ No newline at end of file diff --git a/Stream/uav_proto_msg/utils/mqttConfig_SUB.yml b/Stream/uav_proto_msg/utils/mqttConfig_SUB.yml index 9e3feaa..759bdcd 100644 --- a/Stream/uav_proto_msg/utils/mqttConfig_SUB.yml +++ b/Stream/uav_proto_msg/utils/mqttConfig_SUB.yml @@ -9,7 +9,7 @@ MQTT: lwt: Dron550 Gone Offline willRetain: False # Mqtt topic - Flight_Information_topicToMqtt: Drone550/Flight_Information + Drone550_Flight_Information_topicToMqtt: Drone550/Flight_Information Fly_Formation_topicToMqtt: Drone550/Formation # Change formate qos Fly_Formation_topicToMqtt_QOS: 2 @@ -19,3 +19,4 @@ ROS: ROStopicName_Fly_Formation: Fly_Formation_reciver LOG: logFileName: sub.log +UAVLINK: "None" \ No newline at end of file diff --git a/Stream/uav_proto_msg/utils/proto_uavlink_pub_data_to_ros.py b/Stream/uav_proto_msg/utils/proto_uavlink_pub_data_to_ros.py new file mode 100644 index 0000000..ecd14a3 --- /dev/null +++ b/Stream/uav_proto_msg/utils/proto_uavlink_pub_data_to_ros.py @@ -0,0 +1,31 @@ +import time + +import proto.flight_information_pb2 as flight_information_pb2 +import google.protobuf.json_format as json_format +import logging + +# TODO: use native ros type instead of json or str +logger = logging.getLogger("__SUB__") + +class Proto_msg_to_ros: + #Protobuf + flight_information_msg = None + + #Ros publisher + rate = None + publisher_Flight_Information = None + + + #Proto + @classmethod + def on_message_Flight_Information(cls, msg): + proto = msg[1:-2] + + proto_msg = cls.flight_information_msg.FromString(proto) + protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True) + logger.info(protoTOJson_msg) + print(protoTOJson_msg) + cls.publisher_Flight_Information.publish(protoTOJson_msg) + cls.rate.sleep() + + \ No newline at end of file diff --git a/Stream/uav_proto_msg/utils/proto_uavlink_sub_data_from_ros.py b/Stream/uav_proto_msg/utils/proto_uavlink_sub_data_from_ros.py new file mode 100644 index 0000000..77f43bd --- /dev/null +++ b/Stream/uav_proto_msg/utils/proto_uavlink_sub_data_from_ros.py @@ -0,0 +1,27 @@ +import orjson +import time +import logging + +logger = logging.getLogger("__UAVLINKSUBPUB__") + +class Proto_msg_from_ros: + #Protobuf + flight_information_msg = None + sel = None + + + @classmethod + def callBack_gps(cls, GPS): + cls.flight_information_msg.gps.LAT = GPS.latitude + cls.flight_information_msg.gps.LON = GPS.longitude + cls.flight_information_msg.gps.ALT = GPS.altitude + + + @classmethod + def callBack_compass_hdg(cls, Compass): + cls.flight_information_msg.heading = Compass.data + flightInformationMsg = cls.flight_information_msg.SerializeToString() + cls.sel.write(b'\xf2' + flightInformationMsg + b'\r\n') + print(len(b'\xf2' + flightInformationMsg + b'\r\n')) + readTenByte = cls.sel.readline() + print(readTenByte) diff --git a/Stream/uav_proto_msg/utils/readConfig.py b/Stream/uav_proto_msg/utils/readConfig.py index 539b3a3..54386da 100644 --- a/Stream/uav_proto_msg/utils/readConfig.py +++ b/Stream/uav_proto_msg/utils/readConfig.py @@ -2,13 +2,14 @@ import yaml class Config: def __init__(self, inFileName): - self.sectionNames = ["MQTT","ROS", "LOG"] + self.sectionNames = ["MQTT","ROS", "LOG", "UAVLINK"] self.options = {} self.inFileName = inFileName def setAttribute(self): with open(self.inFileName,"r") as f: self.ymlcfg=yaml.safe_load(f) + ecfgs = [self.ymlcfg.get(name) for name in self.sectionNames] if None in ecfgs: nameIndex = ecfgs.index(None) @@ -20,13 +21,13 @@ class Config: optval=ecfg[opt] #verify parameter type if type(optval) != self.options[opts][opt][0]: - raise Exception("Parameter {} has wrong type".format(opt)) + raise Exception("Parameter {} has wrong type".format(self.opt)) #create attributes on the fly setattr(self,opt,optval) else: if self.options[opts][opt][1]: - raise Exception("Missing mandatory parameter {}".format(opt)) + raise Exception("Missing mandatory parameter {}".format(self.opt)) else: setattr(self,opt,None) @@ -42,24 +43,29 @@ class Read_PUB_Config(Config): super().__init__(inFileName) self.options = { self.sectionNames[0]:{ - "msg_format": (str,True), - "MQTTClientNamePub": (str,True), - "host": (str,True), - "port": (int,True), - "keepalive": (int,True), - "willTopic":(str,True), - "lwt":(str, True), + "msg_format": (str,False), + "MQTTClientNamePub": (str,False), + "host": (str,False), + "port": (int,False), + "keepalive": (int,False), + "willTopic":(str,False), + "lwt":(str, False), "willRetain":(bool,False), - "willTopicQOS":(int,True), + "willTopicQOS":(int,False), "Flight_Information_topicToMqtt": (str,False), "Fly_Formation_topicToMqtt": (str,False), "Fly_Formation_topicToMqtt_QOS":(int,False)}, self.sectionNames[1]:{ - "ROSClientNamePub": (str,False), + "ROSClientNamePub": (str,True), "ROStopicName_Flight_Information": (str,False), "ROStopicName_Fly_Formation": (str,False)}, self.sectionNames[2]:{ - "logFileName":(str,False)}} + "logFileName":(str,True)}, + self.sectionNames[3]:{ + "uavlink_msg_format": (str,False), + "uav_id": (str,False), + "baudrate": (int,False), + "ttyport": (str,False)}} self.setAttribute() def __str__(self): @@ -73,26 +79,44 @@ class Read_SUB_Config(Config): super().__init__(inFileName) self.options = { self.sectionNames[0]:{ - "msg_format": (str,True), - "MQTTClientNameSub": (str,True), - "host": (str,True), - "port": (int,True), - "keepalive": (int,True), - "willTopic":(str,True), - "lwt":(str, True), + "msg_format": (str,False), + "MQTTClientNameSub": (str,False), + "host": (str,False), + "port": (int,False), + "keepalive": (int,False), + "willTopic":(str,False), + "lwt":(str, False), "willRetain":(bool,False), - "willTopicQOS":(int,True), - "Flight_Information_topicToMqtt": (str,False), + "willTopicQOS":(int,False), + "Drone550_Flight_Information_topicToMqtt": (str,False), + "Drone380_Flight_Information_topicToMqtt":(str,False), + "Drone650_Flight_Information_topicToMqtt":(str,False), + "Drone888_Flight_Information_topicToMqtt":(str,False), + "Drone555_Flight_Information_topicToMqtt":(str,False), "Fly_Formation_topicToMqtt": (str,False), "Fly_Formation_topicToMqtt_QOS":(int,False)}, self.sectionNames[1]:{ - "ROSClientNameSub": (str,False), - "ROStopicName_Flight_Information": (str,False), + "ROSClientNameSub": (str,True), + "Dron550_ROStopicName_Flight_Information": (str,False), + "Dron380_ROStopicName_Flight_Information": (str,False), + "Dron380_ROStopicName_Flight_Information": (str,False), + "Dron650_ROStopicName_Flight_Information": (str,False), + "Dron888_ROStopicName_Flight_Information": (str,False), + "Dron555_ROStopicName_Flight_Information": (str,False), + "Dron_ROStopicName_Flight_Information": (str,False), "ROStopicName_Fly_Formation": (str,False)}, self.sectionNames[2]:{ - "logFileName":(str,False)}} + "logFileName":(str,False)}, + self.sectionNames[3]:{ + "uavlink_msg_format": (str,False), + "uav_id": (str,False), + "baudrate": (int,False), + "ttyport": (str,False)}} self.setAttribute() + def __str__(self): + return super().__str__() + class Read_CMD_Config(Config): def setAttribute(self): super().setAttribute() @@ -121,8 +145,6 @@ class Read_CMD_Config(Config): def __str__(self): return super().__str__() - - if __name__ == "__main__": cfg=Read_CMD_Config("mqttConfig_CMD.yml") print(cfg) diff --git a/Stream/uav_proto_msg/utils/uavlinkConfig_PUB.yml b/Stream/uav_proto_msg/utils/uavlinkConfig_PUB.yml new file mode 100644 index 0000000..4e5624e --- /dev/null +++ b/Stream/uav_proto_msg/utils/uavlinkConfig_PUB.yml @@ -0,0 +1,12 @@ +UAVLINK: + uavlink_msg_format: Proto + uav_id: \x01\x01 + baudrate: 250000 + ttyport: /dev/ttyUSB1 +MQTT: "None" +#ROS +ROS: + ROSClientNamePub: Drone550UAVLINKPub + ROStopicName_Flight_Information: Flight_Information_reciver +LOG: + logFileName: UAVLINKpub.log \ No newline at end of file diff --git a/Stream/uav_proto_msg/utils/uavlinkConfig_SUB.yml b/Stream/uav_proto_msg/utils/uavlinkConfig_SUB.yml new file mode 100644 index 0000000..cece1ea --- /dev/null +++ b/Stream/uav_proto_msg/utils/uavlinkConfig_SUB.yml @@ -0,0 +1,11 @@ +UAVLINK: + uavlink_msg_format: Proto + uav_id: \x01\x01 + baudrate: 250000 + ttyport: /dev/ttyUSB1 +MQTT: "None" +ROS: + ROSClientNameSub: Drone550UAVLINKSub + Dron550_ROStopicName_Flight_Information: Flight_Information_reciver +LOG: + logFileName: UAVLINKsub.log \ No newline at end of file