organize files

main
RangeOfGlitching 3 years ago
parent 690da410e5
commit fdfe1b1d85

@ -52,7 +52,7 @@ if __name__ == '__main__':
if args.format != None:
cfg.msg_format = args.format
init_dataFormat(cfg)
# set log
log_format = "%(asctime)s - %(levelname)s - %(message)s"
file_foramt = "%(message)s"
@ -70,12 +70,14 @@ if __name__ == '__main__':
file_handler.setLevel(logging.INFO)
logger = logging.getLogger("__delayA_SUB__")
logger.setLevel(logging.INFO)
logger.setLevel(logging.DEBUG)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.debug(cfg)
init_dataFormat(cfg)
# Mqtt
client.on_connect = on_connect
client.on_publish = on_publish

@ -70,10 +70,7 @@ if __name__ == '__main__':
client.on_publish = on_publish
client.on_disconnect = on_disconnect
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
# initialize
init_dataFormat(cfg)
# set log
log_format = "%(asctime)s - %(levelname)s - %(message)s"
file_foramt = "%(message)s"
@ -91,12 +88,13 @@ if __name__ == '__main__':
file_handler.setLevel(logging.INFO)
logger = logging.getLogger("__SUB__")
logger.setLevel(logging.INFO)
logger.setLevel(logging.DEBUG)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.debug(cfg)
# initialize
init_dataFormat(cfg)
try:
client.loop_forever()
# rospy.spin()

@ -67,7 +67,7 @@ if __name__ == '__main__':
# test json
Proto_msg_from_ros.callBack_gps(gps)
Proto_msg_from_ros.callBack_compass_hdg(hdg)
time.sleep(0.5)
time.sleep(1)
hdg.data += 1
except KeyboardInterrupt as e:
sel.write(b'\xf2' + b'......................' + b'\x0d\x2a')

@ -22,7 +22,7 @@ class Proto_msg_to_ros:
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')
cls.sel.write(b'\xf2' + proto + b'\x0d\x0a')
timePub = time.perf_counter_ns()
# readTenByte = cls.sel.read_until(size=5)
timeDiff = timePub - timeSub

@ -2,7 +2,7 @@ UAVLINK:
uavlink_msg_format: Proto
uav_id: \x01\x01
baudrate: 250000
ttyport: /dev/ttyUSB3
ttyport: /dev/ttyUSB0
MQTT: "None"
#ROS
ROS:

@ -0,0 +1,11 @@
UAVLINK:
uavlink_msg_format: Proto
uav_id: \x01\x01
baudrate: 250000
ttyport: /dev/ttyUSB0
MQTT: "None"
ROS:
ROSClientNameSub: Drone550UAVLINKSub
Dron550_ROStopicName_Flight_Information: Flight_Information_reciver
LOG:
logFileName: ProtodelayA_SUB.log

@ -0,0 +1,3 @@
python delayClientA_PUB.py -q 0 -p 1884 -f Json

@ -0,0 +1,11 @@
frequency,packet_loss,percentage_loss
1hz,0,0.0
2hz,0,0.0
3hz,0,0.0
4hz,0,0.0
5hz,0,0.0
6hz,0,0.0
7hz,0,0.0
8hz,0,0.0
9hz,0,0.0
10hz,0,0.0
1 frequency packet_loss percentage_loss
2 1hz 0 0.0
3 2hz 0 0.0
4 3hz 0 0.0
5 4hz 0 0.0
6 5hz 0 0.0
7 6hz 0 0.0
8 7hz 0 0.0
9 8hz 0 0.0
10 9hz 0 0.0
11 10hz 0 0.0

@ -0,0 +1,11 @@
frequency,packet_loss,percentage_loss
1hz,0,0.0
2hz,1,1.0
3hz,6,6.0
4hz,8,8.0
5hz,17,17.0
6hz,32,32.0
7hz,48,48.0
8hz,52,52.0
9hz,52,52.0
10hz,55,55.00000000000001
1 frequency packet_loss percentage_loss
2 1hz 0 0.0
3 2hz 1 1.0
4 3hz 6 6.0
5 4hz 8 8.0
6 5hz 17 17.0
7 6hz 32 32.0
8 7hz 48 48.0
9 8hz 52 52.0
10 9hz 52 52.0
11 10hz 55 55.00000000000001

@ -0,0 +1,107 @@
#!/usr/bin/env python3
#coding:utf-8
import paho.mqtt.client as mqtt
import os
import sys
import time
import utils
import argparse
import proto.flyformatioln_pb2 as flyformatioln_pb2
import logging
from utils.protoJson_delayClientA_PUB import Json_msg
from utils.protoJson_delayClientA_PUB import Proto_msg
def init_dataFormat(cfg:utils.Read_delayA_PUB_Config):
global pubFun
if cfg.msg_format == "Proto":
Proto_msg.client = client
Proto_msg.qos = cfg.qos
Proto_msg.Delay_topicToMqtt_PUB = cfg.Delay_topicToMqtt_PUB
cfg.logFileName = "Proto" + cfg.logFileName
pubFun = Proto_msg.callBack_gps
elif cfg.msg_format == "Json":
Json_msg.client = client
Json_msg.qos = cfg.qos
Json_msg.Delay_topicToMqtt_PUB = cfg.Delay_topicToMqtt_PUB
cfg.logFileName = "Json" + cfg.logFileName
pubFun = Json_msg.callBack_gps
else:
logging.debug("msg_format not found")
def on_connect(self, userdata, flags, rc):
logger.debug("Connected with result code " + str(rc))
def on_publish(self, userdata, mid):
# logger.debug("pub success")
pass
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument("-q", "--qos", type=int, help="qos")
parser.add_argument("-p", "--port", type=int, help="port")
parser.add_argument("-d", "--data_format", type=str, help="data_format")
parser.add_argument("-f", "--fre", type=str, help="format")
parser.add_argument("-t", "--delaytime", type=float, default="1.0", help="number of packet", required=True)
args = parser.parse_args()
# Read Config
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_delayA_PUB.yml")
cfg = utils.Read_delayA_PUB_Config(FilePath)
if args.qos != None:
cfg.qos = args.qos
if args.port != None:
cfg.port = args.port
if args.data_format != None:
cfg.msg_format = args.data_format
# Mqtt
client = utils.MQTTClient(cfg.MQTTClientNamePub)
pubFun = None
init_dataFormat(cfg)
# set log
log_format = "%(asctime)s - %(levelname)s - %(message)s"
file_format = "%(message)s"
file_formatter = logging.Formatter(file_format)
formatter = logging.Formatter(log_format)
stream_handler = logging.StreamHandler()
stream_handler.setFormatter(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("__PUB__")
logger.setLevel(logging.DEBUG)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.debug(cfg)
# Mqtt
client.on_connect = on_connect
client.on_publish = on_publish
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
client.loop_start()
for i in range(cfg.msgAmount):
try:
pubFun()
time.sleep(args.delaytime)
except KeyboardInterrupt as e:
client.disconnect()
logger.debug("Not elegeant of program")
sys.exit()
client.disconnect()
logger.debug(cfg)
logger.debug("End of program")
sys.exit()

@ -0,0 +1,114 @@
#!/usr/bin/env python3
#coding:utf-8
import paho.mqtt.client as mqtt
import os
import sys
import argparse
import utils
import proto.flight_information_pb2 as flight_information_pb2
import proto.flyformatioln_pb2 as flyformatioln_pb2
import logging
from utils.protoJson_delayClientB_SUB import Json_msg
from utils.protoJson_delayClientB_SUB import Proto_msg
def init_dataFormat(cfg:utils.Read_delayB_SUB_Config):
if cfg.msg_format == "Proto":
Proto_msg.qos = cfg.qos
Proto_msg.flight_information_msg = flight_information_pb2.flight_information_message()
Proto_msg.fly_formation_msg = flyformatioln_pb2.fly_formation_message()
Proto_msg.client = client
client.on_message = Proto_msg.on_message
cfg.logFileName = "Proto" + cfg.logFileName
Proto_msg.Delay_topicToMqtt_PUB = cfg.Delay_topicToMqtt_PUB
elif cfg.msg_format == "Json":
client.on_message = Json_msg.on_message
Json_msg.qos = cfg.qos
Json_msg.client = client
Json_msg.Delay_topicToMqtt_PUB = cfg.Delay_topicToMqtt_PUB
cfg.logFileName = "Json" + cfg.logFileName
else:
logger.debug("msg_format not found")
def on_connect(self, userdata, flags, rc):
logger.debug("Connected with result code " + str(rc))
client.subscribe(cfg.Delay_topicToMqtt_SUB, qos=cfg.qos)
def on_disconnect(client, userdata, rc):
# logger.info("disconnecting reason " +str(rc))
client.connected_flag=False
client.disconnect_flag=True
def on_publish(self, userdata, mid):
pass
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument("-q", "--qos", type=int, help="qos")
parser.add_argument("-p", "--port", type=int, help="port")
parser.add_argument("-d", "--data_format", type=str, help="format")
parser.add_argument("-f", "--frequency", type=str, default="1", help="transmission frequency", required=True)
args = parser.parse_args()
# Read Config
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_delayB_SUB.yml")
cfg = utils.Read_delayB_SUB_Config(FilePath)
if args.qos != None:
cfg.qos = args.qos
if args.port != None:
cfg.port = args.port
if args.data_format != None:
cfg.msg_format = args.data_format
client = utils.MQTTClient(cfg.MQTTClientNameSub)
# Mqtt
client = utils.MQTTClient(cfg.MQTTClientNameSub)
client.on_connect = on_connect
client.on_publish = on_publish
client.on_disconnect = on_disconnect
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
# initialize
init_dataFormat(cfg)
# set log
log_format = "%(asctime)s - %(levelname)s - %(message)s"
file_foramt = "%(message)s"
formatter = logging.Formatter(log_format)
file_formatter = logging.Formatter(file_foramt)
stream_handler = logging.StreamHandler()
stream_handler.setFormatter(formatter)
stream_handler.setLevel(logging.DEBUG)
file_handler = logging.FileHandler(args.frequency +"hz.log", mode='w')
file_handler.setFormatter(file_formatter)
file_handler.setLevel(logging.INFO)
logger = logging.getLogger("__SUB__")
logger.setLevel(logging.DEBUG)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.debug(cfg)
try:
client.loop_forever()
# rospy.spin()
except KeyboardInterrupt as e:
client.loop_stop()
client.disconnect()
logger.debug("Not elegeant end of program")
sys.exit()
client.loop_stop()
client.disconnect()
logger.debug("End of program")
sys.exit()

@ -0,0 +1,85 @@
import pandas as pd
import matplotlib.pyplot as plt
import argparse
import os
import fnmatch
import re
def check_number_of_lines(df, log_name):
if df.shape[0] != args.number:
print(f"Warning: Number of lines in the log file {log_name} is not correct")
return list(check_continuous_count(df, log_name))
else:
return []
def check_continuous_count(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}")
return missing
else:
return 0
def extract_number(filename):
return int(re.search(r'\d+', filename).group())
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_path = os.listdir(f"loseData/test{args.test}")
log_files = [file_name for file_name in log_files_path if fnmatch.fnmatch(file_name, '*.log')]
log_files.sort(key=extract_number)
frequence = [file_name.replace(".log", "") for file_name in log_files]
dataframes = []
missing_counts = []
for log_file in log_files:
df = pd.read_csv(f"loseData/test{args.test}/{log_file}", delim_whitespace=True, header=None, names=["count", "time"])
dataframes.append(df)
missing_counts.append(check_number_of_lines(df, log_file))
total_lengths = [len(sublist) for sublist in missing_counts]
print(total_lengths)
plt.rcParams["figure.figsize"] = [13, 6]
plt.rcParams["figure.autolayout"] = True
bar_plot = plt.bar(frequence, total_lengths, edgecolor='black')
plt.title("Packet drop")
plt.xlabel("Frequency")
plt.ylabel("packet loss rate((%))")
total_packets = args.number
for i, bar in enumerate(bar_plot):
loss_percentage = total_lengths[i] / total_packets * 100
plt.text(bar.get_x() + bar.get_width() / 2, bar.get_height() + 0.1,
f"{loss_percentage:.2f}%", ha="center", va="bottom")
plt.savefig(f"loseData/test{args.test}/test{args.test}.png", dpi=300)
plt.show()
# Create a DataFrame with frequency and packet loss data
results_df = pd.DataFrame({'frequency': frequence, 'packet_loss': total_lengths})
# Calculate percentage loss for each row
results_df['percentage_loss'] = results_df['packet_loss'] / total_packets * 100
# Save the results as a CSV file
results_df.to_csv(f"loseData/test{args.test}/results.csv", index=False)

@ -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)

@ -0,0 +1,14 @@
syntax = 'proto3';
package flight_information_proto;
// 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;
}

@ -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\x12\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\"j\n\x1a\x66light_information_message\x12*\n\x03gps\x18\x01 \x01(\x0b\x32\x1d.flight_information_proto.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=54
_GPS._serialized_end=137
_FLIGHT_INFORMATION_MESSAGE._serialized_start=139
_FLIGHT_INFORMATION_MESSAGE._serialized_end=245
# @@protoc_insertion_point(module_scope)

@ -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;
}

@ -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)

@ -0,0 +1,6 @@
from utils.readConfig import Read_delayA_PUB_Config
from utils.readConfig import Read_delayB_SUB_Config
from utils.readConfig import Read_delayA_SUB_Config
from utils.basicMqtt import MQTTClient

@ -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=[]

@ -0,0 +1,15 @@
MQTT:
# msg_format: Proto
msg_format: Json
qos: 0
MQTTClientNamePub: delayA_PUB
host: 192.168.50.117
port: 1883
keepalive: 60
msgAmount: 100
msgInterval: 0.01
# Mqtt topic
Delay_topicToMqtt_PUB: delayA_PUB/delay
#ROS
LOG:
logFileName: delayA_PUB.log

@ -0,0 +1,12 @@
MQTT:
# msg_format: Proto
msg_format: Json
qos: 0
MQTTClientNameSub: delayA_SUB
host: 192.168.50.117
port: 1883
keepalive: 60
# Mqtt topic
Respond_topicToMqtt_SUB: delayB/respond
LOG:
logFileName: delayA_SUB.log

@ -0,0 +1,13 @@
MQTT:
# msg_format: Proto
msg_format: Json
qos: 0
MQTTClientNameSub: delayB_SUB
host: 192.168.50.117
port: 1883
keepalive: 60
# Mqtt topic
Delay_topicToMqtt_SUB: delayA_PUB/delay
Delay_topicToMqtt_PUB: delayB/respond
LOG:
logFileName: delayB_SUB.log

@ -0,0 +1,77 @@
import orjson
import time
import logging
import proto.flight_information_pb2 as flight_information_pb2
logger = logging.getLogger("__PUB__")
class Proto_msg:
#Protobuf
flight_information_msg = flight_information_pb2.flight_information_message()
flight_information_msg.gps.LAT = 100000000
flight_information_msg.gps.LON = 200000000
flight_information_msg.gps.ALT = 300000000
timePub = 0
#Mqtt
client = None
Delay_topicToMqtt_PUB = None
count = 1
qos = None
@classmethod
def callBack_gps(cls):
cls.flight_information_msg.heading = cls.count
flightInformationMsg = cls.flight_information_msg.SerializeToString()
cls.mqtt_Pub(message=flightInformationMsg, topics=cls.Delay_topicToMqtt_PUB)
@classmethod
# publish a message
def mqtt_Pub(cls, message:bytes, topics:str, waitForAck:bool=False)->None:
cls.timePub = time.perf_counter_ns()
mid = cls.client.publish(topics, message, cls.qos)[1]
logger.info("{} {}".format(cls.count,cls.timePub))
cls.count += 1
# print(f"just published {message} to topic")
if waitForAck:
while mid not in cls.client.topic_ack:
# TODO: logging
print("wait for ack")
time.sleep(0.25)# You can download security extensions from the HiveMQ Marke
cls.client.topic_ack.remove(mid)
class Json_msg:
GPS_Data = {"lat": 100000000, "lon": 200000000, "alt": 300000000, "count":0}
count = 0
#Mqtt
client = None
Delay_topicToMqtt_PUB = None
timePub = 0
qos = None
@classmethod
def callBack_gps(cls):
cls.GPS_Data["count"] = cls.count
dataJsonFormate = orjson.dumps(cls.GPS_Data)
cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Delay_topicToMqtt_PUB)
# print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
@classmethod
def mqtt_Pub(cls, message:str, topics:str, waitForAck:bool=False):
cls.timePub = time.perf_counter_ns()
mid = cls.client.publish(topics, message, cls.qos)[1]
logger.info("{} {}".format(cls.count,cls.timePub))
cls.count += 1
# if waitForAck:
# while mid not in cls.client.topic_ack:
# print("wait for ack")
# time.sleep(0.25)
# cls.client.topic_ack.remove(mid)

@ -0,0 +1,36 @@
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
import logging
logger = logging.getLogger("__delayA_SUB__")
class Proto_msg:
#Protobuf
flight_information_msg = None
timeRcv = 0
msgCounter = None
@classmethod
def on_message(cls, client, userdata, msg):
cls.timeRcv = time.perf_counter_ns()
cls.msgCounter = int(cls.flight_information_msg.FromString(msg.payload).heading)
logger.info("{} {}".format(cls.msgCounter, cls.timeRcv))
class Json_msg:
timeRcv = 0
msgCounter = None
@classmethod
def on_message(cls, client, userdata, msg):
cls.timeRcv = time.perf_counter_ns()
cls.msgCounter = orjson.loads(msg.payload)["count"]
logger.info("{} {}".format(cls.msgCounter, cls.timeRcv))

@ -0,0 +1,53 @@
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
import logging
# TODO: use native ros type instead of json or str
logger = logging.getLogger("__SUB__")
class Proto_msg:
#Protobuf
flight_information_msg = None
fly_formation_msg = None # delcare in function
#Mqtt topic: check data from which topic
Delay_topicToMqtt_PUB = None
qos = None
client = None
timeRcv = 0
timeRsp = 0
msgCounter = None
@classmethod
def on_message(cls, client, userdata, msg):
cls.timeRcv = time.perf_counter_ns()
cls.msgCounter = int(cls.flight_information_msg.FromString(msg.payload).heading)
logger.info("{} {}".format(cls.msgCounter, cls.timeRcv))
class Json_msg:
#Mqtt topic: check data from which topic
Delay_topicToMqtt_PUB = None
qos = None
client = None
timeRcv = 0
timeRsp = 0
msgCounter = None
@classmethod
def on_message(cls, client, userdata, msg):
cls.timeRcv = time.perf_counter_ns()
cls.msgCounter = orjson.loads(msg.payload)["count"]
logger.info("{} {}".format(cls.msgCounter, cls.timeRcv))

@ -0,0 +1,109 @@
import yaml
class Config:
def __init__(self, inFileName):
self.sectionNames = ["MQTT", "LOG"]
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(opt))
#create attributes on the fly
setattr(self,opt,optval)
else:
if self.options[opts][opt][1]:
raise Exception("Missing mandatory parameter {}".format(opt))
else:
setattr(self,opt,None)
def __str__(self):
return str(yaml.dump(self.ymlcfg, default_flow_style=False))
class Read_delayA_PUB_Config(Config):
def setAttribute(self):
super().setAttribute()
def __init__(self, inFileName):
super().__init__(inFileName)
self.options = {
self.sectionNames[0]:{
"msg_format": (str,True),
"MQTTClientNamePub": (str,True),
"host": (str,True),
"port": (int,True),
"keepalive": (int,True),
"Delay_topicToMqtt_PUB": (str,False),
"msgInterval": (float,True),
"msgAmount": (int,True),
"qos": (int,True)},
self.sectionNames[1]:{
"logFileName":(str,False)}}
self.setAttribute()
def __str__(self):
return super().__str__()
class Read_delayB_SUB_Config(Config):
def setAttribute(self):
super().setAttribute()
def __init__(self, inFileName):
super().__init__(inFileName)
self.options = {
self.sectionNames[0]:{
"msg_format": (str,True),
"MQTTClientNameSub": (str,True),
"host": (str,True),
"port": (int,True),
"keepalive": (int,True),
"Delay_topicToMqtt_SUB":(str,True),
"Delay_topicToMqtt_PUB":(str,True),
"qos": (int,True)},
self.sectionNames[1]:{
"logFileName":(str,False)}}
self.setAttribute()
class Read_delayA_SUB_Config(Config):
def setAttribute(self):
super().setAttribute()
def __init__(self, inFileName):
super().__init__(inFileName)
self.options = {
self.sectionNames[0]:{
"msg_format": (str,True),
"MQTTClientNameSub": (str,True),
"host": (str,True),
"port": (int,True),
"keepalive": (int,True),
"Respond_topicToMqtt_SUB":(str,True),
"qos": (int,True)},
self.sectionNames[1]:{
"logFileName":(str,False)}}
self.setAttribute()
def __str__(self):
return super().__str__()
if __name__ == "__main__":
cfg=Read_delayA_SUB_Config("mqttConfig_CMD.yml")
print(cfg)

@ -0,0 +1,4 @@
frequency,packet_loss,percentage_loss
1hz,0,0.0
2hz,0,0.0
5hz,15,15.0
1 frequency packet_loss percentage_loss
2 1hz 0 0.0
3 2hz 0 0.0
4 5hz 15 15.0

@ -0,0 +1,11 @@
frequency,packet_loss,percentage_loss
1hz,0,0.0
2hz,1,1.0
3hz,6,6.0
4hz,8,8.0
5hz,17,17.0
6hz,32,32.0
7hz,48,48.0
8hz,52,52.0
9hz,52,52.0
10hz,55,55.00000000000001
1 frequency packet_loss percentage_loss
2 1hz 0 0.0
3 2hz 1 1.0
4 3hz 6 6.0
5 4hz 8 8.0
6 5hz 17 17.0
7 6hz 32 32.0
8 7hz 48 48.0
9 8hz 52 52.0
10 9hz 52 52.0
11 10hz 55 55.00000000000001

@ -0,0 +1,82 @@
#!/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
import argparse
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()
parser = argparse.ArgumentParser()
parser.add_argument("-t", "--delaytime", type=float, default="1.0", help="1/frequence", required=True)
args = parser.parse_args()
# 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(args.delaytime)
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()

@ -0,0 +1,69 @@
#!/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
import argparse
def init_dataFormat(cfg:Read_SUB_Config):
Proto_msg_to_ros.sel = sel
Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument("-f", "--frequency", type=str, default="1", help="transmission frequency", required=True)
args = parser.parse_args()
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(args.frequency +"hz.log", 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)
except KeyboardInterrupt as e:
Proto_msg_to_ros.turnOffUavlink()
logger.debug("End of program")
sys.exit()

@ -0,0 +1,85 @@
import pandas as pd
import matplotlib.pyplot as plt
import argparse
import os
import fnmatch
import re
def check_number_of_lines(df, log_name):
if df.shape[0] != args.number:
print(f"Warning: Number of lines in the log file {log_name} is not correct")
return list(check_continuous_count(df, log_name))
else:
return []
def check_continuous_count(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}")
return missing
else:
return 0
def extract_number(filename):
return int(re.search(r'\d+', filename).group())
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_path = os.listdir(f"loseData/test{args.test}")
log_files = [file_name for file_name in log_files_path if fnmatch.fnmatch(file_name, '*.log')]
log_files.sort(key=extract_number)
frequence = [file_name.replace(".log", "") for file_name in log_files]
dataframes = []
missing_counts = []
for log_file in log_files:
df = pd.read_csv(f"loseData/test{args.test}/{log_file}", delim_whitespace=True, header=None, names=["count", "time"])
dataframes.append(df)
missing_counts.append(check_number_of_lines(df, log_file))
total_lengths = [len(sublist) for sublist in missing_counts]
print(total_lengths)
plt.rcParams["figure.figsize"] = [13, 6]
plt.rcParams["figure.autolayout"] = True
bar_plot = plt.bar(frequence, total_lengths, edgecolor='black')
plt.title("Packet drop")
plt.xlabel("Frequency")
plt.ylabel("packet loss rate((%))")
total_packets = args.number
for i, bar in enumerate(bar_plot):
loss_percentage = total_lengths[i] / total_packets * 100
plt.text(bar.get_x() + bar.get_width() / 2, bar.get_height() + 0.1,
f"{loss_percentage:.2f}%", ha="center", va="bottom")
plt.savefig(f"loseData/test{args.test}/test{args.test}.png", dpi=300)
plt.show()
# Create a DataFrame with frequency and packet loss data
results_df = pd.DataFrame({'frequency': frequence, 'packet_loss': total_lengths})
# Calculate percentage loss for each row
results_df['percentage_loss'] = results_df['packet_loss'] / total_packets * 100
# Save the results as a CSV file
results_df.to_csv(f"loseData/test{args.test}/results.csv", index=False)

@ -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)

@ -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;
}

@ -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)

@ -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=[]

@ -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))

@ -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))

@ -0,0 +1,47 @@
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
#uavlink
sel = None
payload = b"......................"
noEcho_code = b"\x0d\x0a"
echo_code = b"\x0d\x1a"
close_code = b"\x0d\x2a"
f1_code = b"\xf1"
f2_code = b"\xf2"
f1_close_code = f1_code + payload + close_code
f2_close_code = f2_code + payload + close_code
# 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))
@classmethod
def turnOffUavlink(cls):
time.sleep(0.5)
cls.sel.write(cls.f1_close_code)
time.sleep(0.5)
cls.sel.write(cls.f2_close_code)

@ -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)

@ -0,0 +1,12 @@
UAVLINK:
uavlink_msg_format: Proto
uav_id: \x01\x01
baudrate: 250000
ttyport: /dev/ttyUSB0
MQTT: "None"
#ROS
ROS:
ROSClientNamePub: Drone550UAVLINKPub
ROStopicName_Flight_Information: Flight_Information_reciver
LOG:
logFileName: ProtodelayA_PUB.log

@ -8,4 +8,4 @@ ROS:
ROSClientNameSub: Drone550UAVLINKSub
Dron550_ROStopicName_Flight_Information: Flight_Information_reciver
LOG:
logFileName: ProtodelayA_SUB.log
logFileName: ProtoloseA_SUB.log

@ -0,0 +1 @@
{"serialize":{"json":0.34479037301207427,"proto":1.0475460140005453},"deserializ":{"json":0.42335569999704603,"proto":0.5118989990005502}}

@ -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;
}

@ -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)

Some files were not shown because too many files have changed in this diff Show More

Loading…
Cancel
Save