measureDelay
RangeOfGlitching
3 years ago
parent
fdfe1b1d85
commit
bea476f052
@ -0,0 +1,81 @@
|
||||
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"delayData/test{args.test}/ProtodelayA_PUB.log", f"delayData/test{args.test}/ProtodelayA_SUB.log", f"delayData/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, ax = plt.subplots()
|
||||
ax.boxplot(array_Proto)
|
||||
|
||||
ax.set_title("Round Trip Latency")
|
||||
ax.set_ylabel("Delay (s)")
|
||||
ax.set_xticklabels(["Latency"])
|
||||
|
||||
plt.savefig(f"delayData/test{args.test}/test{args.test}_boxplot.png", dpi=100)
|
||||
|
||||
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"delayData/test{args.test}/test{args.test}.png", dpi=100)
|
||||
@ -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,83 @@
|
||||
#!/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
|
||||
import google.protobuf.message
|
||||
|
||||
import rospy
|
||||
from std_msgs.msg import String
|
||||
from std_msgs.msg import Float64
|
||||
from std_msgs.msg import Header
|
||||
from mavros_msgs.srv import ParamGet
|
||||
from sensor_msgs.msg import NavSatFix
|
||||
from sensor_msgs.msg import Imu
|
||||
from sensor_msgs.msg import Range
|
||||
from geometry_msgs.msg import Vector3
|
||||
# costom msg
|
||||
from class_model.msg import FlightInformation
|
||||
|
||||
|
||||
def init_dataFormat(cfg:Read_SUB_Config):
|
||||
Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
|
||||
Proto_msg_to_ros.rate = rospy.Rate(10)
|
||||
Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,FlightInformation,queue_size=10)
|
||||
Proto_msg_to_ros.FlightInformationRosMsg = FlightInformation()
|
||||
Proto_msg_to_ros.sel = sel
|
||||
Proto_msg_to_ros.detection()
|
||||
|
||||
|
||||
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)
|
||||
sel.write()
|
||||
# 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)
|
||||
|
||||
rospy.init_node(cfg.ROSClientNameSub)
|
||||
init_dataFormat(cfg)
|
||||
|
||||
|
||||
|
||||
while True:
|
||||
try:
|
||||
logger.debug(sel.read_until(size=25))
|
||||
# if sel.in_waiting >= 25:
|
||||
# readTenByte = sel.read_until(expected= b'\x01\x17', size=25)
|
||||
# Proto_msg_to_ros.on_message_Flight_Information(readTenByte)
|
||||
|
||||
except google.protobuf.message.DecodeError as e:
|
||||
logger.info("DecodeError:{}".format(e))
|
||||
logger.info("readTenByte:{}".format(readTenByte))
|
||||
except KeyboardInterrupt as e:
|
||||
Proto_msg_to_ros.turnOffUavlink()
|
||||
print("End of program")
|
||||
sys.exit()
|
||||
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1,73 @@
|
||||
#!/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 rospy
|
||||
from std_msgs.msg import String
|
||||
from std_msgs.msg import Float64
|
||||
from std_msgs.msg import Header
|
||||
from mavros_msgs.srv import ParamGet
|
||||
from sensor_msgs.msg import NavSatFix
|
||||
from sensor_msgs.msg import Imu
|
||||
from sensor_msgs.msg import Range
|
||||
from geometry_msgs.msg import Vector3
|
||||
|
||||
|
||||
def init_dataFormat(cfg:Read_PUB_Config):
|
||||
Proto_msg_from_ros.sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE)
|
||||
Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
|
||||
Proto_msg_from_ros.rate = rospy.Rate(7)
|
||||
rospy.Subscriber('/mavros/global_position/global', NavSatFix, Proto_msg_from_ros.callBack_gps)
|
||||
rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, Proto_msg_from_ros.callBack_compass_hdg)
|
||||
|
||||
if __name__ == '__main__':
|
||||
FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_PUB.yml")
|
||||
cfg = Read_PUB_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.DEBUG)
|
||||
|
||||
logger = logging.getLogger("__UAVLINKSUBPUB__")
|
||||
logger.setLevel(logging.DEBUG)
|
||||
logger.addHandler(file_handler)
|
||||
logger.addHandler(stream_handler)
|
||||
logger.debug(cfg)
|
||||
|
||||
# Ros
|
||||
rosClient = cfg.ROSClientNamePub
|
||||
rospy.init_node(rosClient)
|
||||
|
||||
# init data format
|
||||
init_dataFormat(cfg)
|
||||
|
||||
try:
|
||||
rospy.spin()
|
||||
except KeyboardInterrupt as e:
|
||||
Proto_msg_from_ros.turnOffUavlink()
|
||||
logger.debug("End of program")
|
||||
sys.exit()
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1,7 @@
|
||||
from utils.protoJson_mqtt_pub_data_to_ros import Proto_msg_to_ros
|
||||
from utils.protoJson_mqtt_pub_data_to_ros import Json_msg_to_ros
|
||||
from utils.protoJson_mqtt_sub_data_from_ros import Proto_msg_from_ros
|
||||
from utils.protoJson_mqtt_sub_data_from_ros import Json_msg_from_ros
|
||||
from utils.readConfig import Read_PUB_Config
|
||||
from utils.readConfig import Read_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: Json
|
||||
MQTTClientNameCmd: Drone550Cmd
|
||||
host: 192.168.50.117
|
||||
port: 1883
|
||||
keepalive: 600
|
||||
# Mqtt topic
|
||||
Cmd_Broadcast_topicToMqtt: cmd/broadcast
|
||||
Cmd_Broadcast_topicToMqtt_QOS: 2
|
||||
#ROS
|
||||
ROS:
|
||||
ROSClientNameCmd: Drone550_Cmd
|
||||
ROStopicName_Cmd_Broadcast_Receiver: cmd_receiver
|
||||
LOG:
|
||||
logFileName: cmd.log
|
||||
@ -0,0 +1,22 @@
|
||||
MQTT:
|
||||
msg_format: Json
|
||||
MQTTClientNamePub: Drone550Pub
|
||||
host: 192.168.50.117
|
||||
port: 1883
|
||||
keepalive: 60
|
||||
willTopic: CheckDoneConnect
|
||||
willTopicQOS: 1
|
||||
lwt: Dron550 Gone Offline
|
||||
willRetain: False
|
||||
# Mqtt topic
|
||||
Flight_Information_topicToMqtt: Drone550/Flight_Information
|
||||
Fly_Formation_topicToMqtt: Drone550/Formation
|
||||
# Change formate qos
|
||||
Fly_Formation_topicToMqtt_QOS: 2
|
||||
#ROS
|
||||
ROS:
|
||||
ROSClientNamePub: Drone550Pub
|
||||
ROStopicName_Flight_Information: Flight_Information_reciver
|
||||
ROStopicName_Fly_Formation: Fly_Formation_reciver
|
||||
LOG:
|
||||
logFileName: pub.log
|
||||
@ -0,0 +1,21 @@
|
||||
MQTT:
|
||||
msg_format: Json
|
||||
MQTTClientNameSub: Drone550Sub
|
||||
host: 192.168.50.117
|
||||
port: 1883
|
||||
keepalive: 60
|
||||
willTopic: CheckDoneConnect
|
||||
willTopicQOS: 1
|
||||
lwt: Dron550 Gone Offline
|
||||
willRetain: False
|
||||
# Mqtt topic
|
||||
Flight_Information_topicToMqtt: leader/Flight_Information
|
||||
Fly_Formation_topicToMqtt: leader/Formation
|
||||
# Change formate qos
|
||||
Fly_Formation_topicToMqtt_QOS: 2
|
||||
ROS:
|
||||
ROSClientNameSub: Drone550Sub
|
||||
ROStopicName_Flight_Information: Flight_Information_reciver
|
||||
ROStopicName_Fly_Formation: Fly_Formation_reciver
|
||||
LOG:
|
||||
logFileName: sub.log
|
||||
@ -0,0 +1,32 @@
|
||||
import orjson
|
||||
import time
|
||||
|
||||
import logging
|
||||
|
||||
# TODO: use native ros type instead of json or str
|
||||
logger = logging.getLogger("__CMD__")
|
||||
|
||||
class Proto_msg_to_ros:
|
||||
pass
|
||||
|
||||
class Json_msg_to_ros:
|
||||
rate = None
|
||||
# Ros publisher
|
||||
publisher_Cmd_Broadcast = None
|
||||
|
||||
Cmd_Broadcast_topicToMqtt = None
|
||||
|
||||
|
||||
@classmethod
|
||||
def ros_pub(cls, dataJson):
|
||||
if dataJson.topic == cls.Cmd_Broadcast_topicToMqtt:
|
||||
logger.debug(dataJson.payload.decode("UTF-8"))
|
||||
cls.publisher_Cmd_Broadcast.publish(dataJson.payload.decode("UTF-8"))
|
||||
cls.rate.sleep()
|
||||
else:
|
||||
logger.info("topic not found")
|
||||
|
||||
|
||||
@staticmethod
|
||||
def on_message(client, userdata, msg):
|
||||
Json_msg_to_ros.ros_pub(msg)
|
||||
@ -0,0 +1,69 @@
|
||||
import orjson
|
||||
import time
|
||||
|
||||
import proto.flight_information_pb2 as flight_information_pb2
|
||||
import proto.flyformatioln_pb2 as flyformatioln_pb2
|
||||
import google.protobuf.json_format as json_format
|
||||
|
||||
class Proto_msg_to_ros:
|
||||
#Protobuf
|
||||
flight_information_msg = None
|
||||
fly_formation_msg = None # delcare in function
|
||||
|
||||
#Ros publisher
|
||||
rate = None
|
||||
publisher_Flight_Information = None
|
||||
publisher_Fly_Formation = None
|
||||
|
||||
#Mqtt topic: check data from which topic
|
||||
Flight_Information_topicToMqtt = None
|
||||
Fly_Formation_topicToMqtt = None
|
||||
|
||||
#Proto
|
||||
|
||||
@classmethod
|
||||
def ros_pub(cls, dataProto):
|
||||
if dataProto.topic == cls.Flight_Information_topicToMqtt:
|
||||
proto_msg = cls.flight_information_msg.FromString(dataProto.payload)
|
||||
protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True, use_integers_for_enums=True)
|
||||
cls.publisher_Flight_Information.publish(protoTOJson_msg)
|
||||
cls.rate.sleep()
|
||||
elif dataProto.topic == cls.Fly_Formation_topicToMqtt:
|
||||
proto_msg = cls.fly_formation_msg.FromString(dataProto.payload)
|
||||
# protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True)
|
||||
protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True, use_integers_for_enums=True)
|
||||
cls.publisher_Fly_Formation.publish(protoTOJson_msg)
|
||||
cls.rate.sleep()
|
||||
else:
|
||||
print("topic not found")
|
||||
|
||||
|
||||
@staticmethod
|
||||
def on_message(client, userdata, msg):
|
||||
Proto_msg_to_ros.ros_pub(msg)
|
||||
|
||||
|
||||
class Json_msg_to_ros:
|
||||
rate = None
|
||||
#Ros publisher
|
||||
publisher_Flight_Information = None
|
||||
publisher_Fly_Formation = None
|
||||
#Mqtt topic: check data from which topic
|
||||
Flight_Information_topicToMqtt = None
|
||||
Fly_Formation_topicToMqtt = None
|
||||
|
||||
@classmethod
|
||||
def ros_pub(cls, dataJson):
|
||||
if dataJson.topic == cls.Flight_Information_topicToMqtt:
|
||||
cls.publisher_Flight_Information.publish(dataJson.payload.decode("UTF-8"))
|
||||
cls.rate.sleep()
|
||||
elif dataJson.topic == cls.Fly_Formation_topicToMqtt:
|
||||
cls.publisher_Fly_Formation.publish(dataJson.payload.decode("UTF-8"))
|
||||
cls.rate.sleep()
|
||||
else:
|
||||
print("topic not found")
|
||||
|
||||
|
||||
@staticmethod
|
||||
def on_message(client, userdata, msg):
|
||||
Json_msg_to_ros.ros_pub(msg)
|
||||
@ -0,0 +1,106 @@
|
||||
import orjson
|
||||
import time
|
||||
|
||||
class Proto_msg_from_ros:
|
||||
#Protobuf
|
||||
flight_information_msg = None
|
||||
fly_formation_msg = None
|
||||
#Mqtt
|
||||
client = None
|
||||
Flight_Information_topicToMqtt = None
|
||||
Fly_Formation_topicToMqtt = None
|
||||
Fly_Formation_topicToMqtt_QOS = None
|
||||
|
||||
# TODO: gps decimal point fix
|
||||
@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.mqtt_Pub(message=flightInformationMsg, topics=cls.Flight_Information_topicToMqtt)
|
||||
|
||||
|
||||
# TODO: Formation.velocity check
|
||||
@classmethod
|
||||
def callBack_fly_formation(cls, Formation):
|
||||
cls.fly_formation_msg.velocity = Formation.velocity
|
||||
cls.fly_formation_msg.fly_formation = Formation.type
|
||||
flyFormationMsg = cls.fly_formation_msg.SerializeToString()
|
||||
cls.mqtt_Pub(message=flyFormationMsg, topics=cls.Fly_Formation_topicToMqtt, qos=cls.Fly_Formation_topicToMqtt_QOS)
|
||||
|
||||
|
||||
|
||||
@classmethod
|
||||
def mqtt_Pub(cls, message:bytes, topics:str, waitForAck:bool=False, qos:int=0)->None:
|
||||
mid = cls.client.publish(topics, message, qos)[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)
|
||||
|
||||
|
||||
|
||||
|
||||
class Json_msg_from_ros:
|
||||
GPS_Data = {}
|
||||
Formation_Data = {}
|
||||
client = None
|
||||
#Mqtt
|
||||
Flight_Information_topicToMqtt = None
|
||||
Fly_Formation_topicToMqtt = None
|
||||
Fly_Formation_topicToMqtt_QOS = None
|
||||
# TODO: remove str
|
||||
@classmethod
|
||||
def callBack_imu(cls, IMU):
|
||||
gyro_x=str(IMU.linear_acceleration.x)
|
||||
gyro_y=str(IMU.linear_acceleration.y)
|
||||
gyro_z=str(IMU.linear_acceleration.z)
|
||||
|
||||
accel_x=str(IMU.angular_velocity.x)
|
||||
accel_y=str(IMU.angular_velocity.y)
|
||||
accel_z=str(IMU.angular_velocity.z)
|
||||
|
||||
dataImuUpdate = {"gyro_x": gyro_x, "gyro_y": gyro_y,"gyro_z":gyro_z, "accel_x": accel_x, "accel_y": accel_y, "accel_z": accel_z}
|
||||
cls.data.update(dataImuUpdate)
|
||||
|
||||
|
||||
@classmethod
|
||||
def callBack_gps(cls, GPS):
|
||||
lat=int(GPS.latitude*10000000) #change the scale to centimeters
|
||||
lon=int(GPS.longitude*10000000)
|
||||
alt=int(GPS.altitude*100)
|
||||
dataGpsUpdate = {"lat": lat, "lon": lon, "alt": alt}
|
||||
cls.GPS_Data.update(dataGpsUpdate)
|
||||
dataJsonFormate = orjson.dumps(cls.GPS_Data)
|
||||
cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Flight_Information_topicToMqtt)
|
||||
|
||||
|
||||
# TODO: does decode need utf8
|
||||
@classmethod
|
||||
def callBack_compass_hdg(cls, Compass):
|
||||
heading = int(Compass.data*100)
|
||||
dataGpsUpdate = {"heading": heading}
|
||||
cls.GPS_Data.update(dataGpsUpdate)
|
||||
dataJsonFormate = orjson.dumps(cls.GPS_Data)
|
||||
cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Flight_Information_topicToMqtt)
|
||||
@classmethod
|
||||
def callBack_fly_formation(cls, Formation):
|
||||
Formation_data = {"velocity": Formation.velocity, "type": Formation.type}
|
||||
flyFormationMsg = orjson.dumps(Formation_data)
|
||||
cls.mqtt_Pub(message=flyFormationMsg, topics=cls.Fly_Formation_topicToMqtt, qos=cls.Fly_Formation_topicToMqtt_QOS)
|
||||
|
||||
@classmethod
|
||||
def mqtt_Pub(cls, message:str, topics:str, waitForAck:bool=False, qos:int=0):
|
||||
mid = cls.client.publish(topics, message, qos)[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,51 @@
|
||||
import time
|
||||
|
||||
import proto.flight_information_pb2 as flight_information_pb2
|
||||
import google.protobuf.json_format as json_format
|
||||
import logging
|
||||
# costom msg
|
||||
|
||||
# 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
|
||||
FlightInformationRosMsg = None
|
||||
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
|
||||
|
||||
#Proto
|
||||
@classmethod
|
||||
def on_message_Flight_Information(cls, msg):
|
||||
cls.publisher_Flight_Information.publish(cls.convert_proto_to_ros(msg[1:-2]))
|
||||
cls.rate.sleep()
|
||||
|
||||
@classmethod
|
||||
def convert_proto_to_ros(cls, proto):
|
||||
proto_msg = cls.flight_information_msg.FromString(proto)
|
||||
cls.FlightInformationRosMsg.LAT = proto_msg.gps.LAT
|
||||
cls.FlightInformationRosMsg.LON = proto_msg.gps.LON
|
||||
cls.FlightInformationRosMsg.ALT = proto_msg.gps.ALT
|
||||
cls.FlightInformationRosMsg.heading = proto_msg.heading
|
||||
return cls.FlightInformationRosMsg
|
||||
|
||||
@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,44 @@
|
||||
import orjson
|
||||
import time
|
||||
import logging
|
||||
|
||||
logger = logging.getLogger("__UAVLINKSUBPUB__")
|
||||
|
||||
class Proto_msg_from_ros:
|
||||
#Protobuf
|
||||
flight_information_msg = None
|
||||
#ros
|
||||
rate = 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
|
||||
|
||||
|
||||
@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(cls.f2_code + flightInformationMsg + cls.noEcho_code)
|
||||
cls.rate.sleep()
|
||||
|
||||
|
||||
@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,132 @@
|
||||
import yaml
|
||||
|
||||
class Config:
|
||||
def __init__(self, inFileName):
|
||||
self.sectionNames = ["MQTT","ROS", "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(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,True),
|
||||
"MQTTClientNamePub": (str,True),
|
||||
"host": (str,True),
|
||||
"port": (int,True),
|
||||
"keepalive": (int,True),
|
||||
"willTopic":(str,True),
|
||||
"lwt":(str, True),
|
||||
"willRetain":(bool,False),
|
||||
"willTopicQOS":(int,True),
|
||||
"Flight_Information_topicToMqtt": (str,False),
|
||||
"Fly_Formation_topicToMqtt": (str,False),
|
||||
"Fly_Formation_topicToMqtt_QOS":(int,False)},
|
||||
self.sectionNames[1]:{
|
||||
"ROSClientNamePub": (str,False),
|
||||
"ROStopicName_Flight_Information": (str,False),
|
||||
"ROStopicName_Fly_Formation": (str,False)},
|
||||
self.sectionNames[2]:{
|
||||
"logFileName":(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,True),
|
||||
"MQTTClientNameSub": (str,True),
|
||||
"host": (str,True),
|
||||
"port": (int,True),
|
||||
"keepalive": (int,True),
|
||||
"willTopic":(str,True),
|
||||
"lwt":(str, True),
|
||||
"willRetain":(bool,False),
|
||||
"willTopicQOS":(int,True),
|
||||
"Flight_Information_topicToMqtt": (str,False),
|
||||
"Fly_Formation_topicToMqtt": (str,False),
|
||||
"Fly_Formation_topicToMqtt_QOS":(int,False)},
|
||||
self.sectionNames[1]:{
|
||||
"ROSClientNameSub": (str,False),
|
||||
"ROStopicName_Flight_Information": (str,False),
|
||||
"ROStopicName_Fly_Formation": (str,False)},
|
||||
self.sectionNames[2]:{
|
||||
"logFileName":(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,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\x17
|
||||
baudrate: 250000
|
||||
ttyport: /dev/ttyTHS1
|
||||
MQTT: "None"
|
||||
#ROS
|
||||
ROS:
|
||||
ROSClientNamePub: Drone550UAVLINKPub
|
||||
ROStopicName_Flight_Information: Flight_Information_reciver
|
||||
LOG:
|
||||
logFileName: UAVLINKpub.log
|
||||
@ -0,0 +1,11 @@
|
||||
UAVLINK:
|
||||
uavlink_msg_format: Proto
|
||||
uav_id: \x01\x01
|
||||
baudrate: 250000
|
||||
ttyport: /dev/ttyTHS1
|
||||
MQTT: "None"
|
||||
ROS:
|
||||
ROSClientNameSub: Drone550UAVLINKSub
|
||||
Dron550_ROStopicName_Flight_Information: Flight_Information_reciver
|
||||
LOG:
|
||||
logFileName: UAVLINKsub.log
|
||||
Loading…
Reference in New Issue