From 434d04854779c672c0ce3eeb2a1e355c5a3139e8 Mon Sep 17 00:00:00 2001 From: RangeOfGlitching Date: Thu, 9 Feb 2023 02:07:11 +0800 Subject: [PATCH] update code logging and readconfig --- class_model/src/local_mqtt_pub_data_to_ros.py | 70 -------- .../src/local_mqtt_sub_data_from_ros.py | 136 ---------------- class_model/src/mqttPubMain.py | 96 +++++------ class_model/src/mqttSubMain.py | 56 +++++-- .../src/proto/flight_information.proto | 2 +- .../src/proto/flight_information_pb2.py | 10 +- class_model/src/proto/flyformatioln.proto | 2 +- class_model/src/proto/flyformatioln_pb2.py | 10 +- class_model/src/tower.py | 5 - .../{mqttConfig.yml => mqttConfig_PUB.yml} | 19 +-- class_model/src/utils/mqttConfig_SUB.yml | 21 +++ .../utils/protoJson_mqtt_pub_data_to_ros.py | 10 +- .../utils/protoJson_mqtt_sub_data_from_ros.py | 43 +++-- class_model/src/utils/readConfig.py | 152 +++++++++++------- 14 files changed, 246 insertions(+), 386 deletions(-) delete mode 100755 class_model/src/local_mqtt_pub_data_to_ros.py delete mode 100755 class_model/src/local_mqtt_sub_data_from_ros.py rename class_model/src/utils/{mqttConfig.yml => mqttConfig_PUB.yml} (50%) create mode 100644 class_model/src/utils/mqttConfig_SUB.yml diff --git a/class_model/src/local_mqtt_pub_data_to_ros.py b/class_model/src/local_mqtt_pub_data_to_ros.py deleted file mode 100755 index f60328b..0000000 --- a/class_model/src/local_mqtt_pub_data_to_ros.py +++ /dev/null @@ -1,70 +0,0 @@ -#!/usr/bin/env python3 -#coding:utf-8 -# license removed for brevity -import ssl -import rospy -from std_msgs.msg import String -import paho.mqtt.client as mqtt - - - -# Ros -def ros_pub(dataJson): - # global publisher, rate - # data = json.loads(dataJson) - publisher.publish(dataJson) #將date字串發布到topic - rate.sleep() - # print(f"publish data {data}") - -# MQTT -def initialise_clients(clientname): - # callback assignment - initialise_client = mqtt.Client(clientname, False) - initialise_client.topic_ack = [] - return initialise_client - -def on_connect(client, userdata, flags, rc): - print("Connected with result code " + str(rc)) - # client.subscribe(mqtt_config["topic"]) - client.subscribe("data/sensor") - - -def on_message(client, userdata, msg): - # print(f"got {msg.payload.decode('utf-8')}") - # print(f"msg.topic {msg.payload.decode('utf-8')}") - ros_pub(msg.payload.decode('utf-8')) - - - -if __name__ == '__main__': - # Mqtt - mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor"} - - client = initialise_clients("123456") - client.on_connect = on_connect - client.on_message = on_message - client.connect(mqtt_config["host"], mqtt_config["port"], 60) - - - - # Ros - Mqtt_Node = 'publisher_py' - rospy.init_node(Mqtt_Node) - # initialize Ros node - topicName = 'uav_message' - publisher = rospy.Publisher(topicName,String,queue_size=10) - - rate = rospy.Rate(10) - - - - client.loop_forever() - -# mqtt connect code list -# 0: Connection successful -# 1: Connection refused – incorrect protocol version -# 2: Connection refused – invalid client identifier -# 3: Connection refused – server unavailable -# 4: Connection refused – bad username or password -# 5: Connection refused – not authorised -# 6-255: Currently unused. diff --git a/class_model/src/local_mqtt_sub_data_from_ros.py b/class_model/src/local_mqtt_sub_data_from_ros.py deleted file mode 100755 index dc1b8a7..0000000 --- a/class_model/src/local_mqtt_sub_data_from_ros.py +++ /dev/null @@ -1,136 +0,0 @@ -#!/usr/bin/env python3 -#coding:utf-8 -import sys -import orjson -import paho.mqtt.client as mqtt - - -from traceback import print_tb -import rospy -from std_msgs.msg import String -from std_msgs.msg import Float64 -from std_msgs.msg import Header -from mavros_msgs.srv import ParamGet -from sensor_msgs.msg import NavSatFix -from sensor_msgs.msg import Imu -from sensor_msgs.msg import Range -from geometry_msgs.msg import Vector3 -import time - -mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor"} -data ={} -# Ros -def callBack_imu(IMU): - gyro_x=str(IMU.linear_acceleration.x) - gyro_y=str(IMU.linear_acceleration.y) - gyro_z=str(IMU.linear_acceleration.z) - - accel_x=str(IMU.angular_velocity.x) - accel_y=str(IMU.angular_velocity.y) - accel_z=str(IMU.angular_velocity.z) - - dataImuUpdate = {"gyro_x": gyro_x, "gyro_y": gyro_y,"gyro_z":gyro_z, "accel_x": accel_x, "accel_y": accel_y, "accel_z": accel_z} - data.update(dataImuUpdate) - # print ('gyro_x:'+gyro_x+'\n'+'gyro_y:'+gyro_y+'\n'+'gyro_z:'+gyro_z +'\n') - print ('accel_x:'+accel_x+'\n'+'accel_y:'+accel_y+'\n'+'accel_z:'+accel_z +'\n') - - -def callBack_gps(GPS): - 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} - data.update(dataGpsUpdate) - # dataJsonFormate = orjson.dumps(data) - # mqtt_Pub(message=dataJsonFormate) - # print ('lat:'+lat+'\n'+'lon:'+lon+'\n') - - -def callBack_compass_hdg(Compass): - heading = int(Compass.data*100) - dataGpsUpdate = {"heading": heading} - data.update(dataGpsUpdate) - dataJsonFormate = orjson.dumps(data) - mqtt_Pub(message=dataJsonFormate) - -def callBack_state(state): - mode = state.mode - dataGpsUpdate = {"mode": mode} - data.update(dataGpsUpdate) - - - -# get parameter -def GetParam(self,param_name): - param = self.get_param_srv(param_name) - if param.success: - if param.value.integer != 0: - value = param.value.integer - - else: - value = param.value.real - - else: - rospy.logwarn("Parameter "+param_name+" not read") - value = 0 - return value - - -# MQTT -def initialise_clients(clientname): - # callback assignment - initialise_client = mqtt.Client(clientname, False) - initialise_client.topic_ack = [] - return initialise_client - - -# publish a message -def mqtt_Pub(message, topics = mqtt_config["topic"], waitForAck=False): - mid = client.publish(topics, message, 0)[1] - # print(f"just published {message} to topic") - if waitForAck: - while mid not in client.topic_ack: - print("wait for ack") - time.sleep(0.25) - client.topic_ack.remove(mid) - -def on_publish(self, userdata, mid): - client.topic_ack.append(mid) - -def on_connect(self, userdata, flags, rc): - print("Connected with result code " + str(rc)) - - -if __name__ == '__main__': - # Mqtt - client = initialise_clients("client1") - client.on_publish = on_publish - client.on_connect = on_connect - client.connect(mqtt_config["host"], mqtt_config["port"], 60) - client.loop_start() - - # Ros - nodeName = 'save_data_py' - rospy.init_node(nodeName) - - ros_namespace = "" - if not rospy.has_param("namespace"): - print("using default namespace") - else: - rospy.get_param("namespace", ros_namespace) - print("using namespace "+ros_namespace) - - ros_namespace="/drone1" - # topicName = 'data_topic' - # subscriber = rospy.Subscriber(ros_namespace+'/mavros/imu/data_raw',Imu,callBack_imu) #從topic獲取string再呼叫callback - subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/global',NavSatFix,callBack_gps) #從topic獲取string再呼叫callback - subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg',Float64,callBack_compass_hdg) #從topic獲取string再呼叫callback - # subscriber = rospy.Subscriber(ros_namespace+"/mavros/next_command",String,callBack_message) - #subscriber = rospy.Subscriber(ros_namespace+'/mavros/mavros_msgs/State',Header,callBack_state) - # ID = rospy.get_param(ros_namespace+'/leader/droneID') - # subscriber = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng) #從topic獲取string再呼叫callback - - # ID = rospy.GetParam("droneID") - # print(ID) - - rospy.spin() \ No newline at end of file diff --git a/class_model/src/mqttPubMain.py b/class_model/src/mqttPubMain.py index eaeafd4..fbc4465 100755 --- a/class_model/src/mqttPubMain.py +++ b/class_model/src/mqttPubMain.py @@ -7,6 +7,7 @@ import time import utils import proto.flight_information_pb2 as flight_information_pb2 import proto.flyformatioln_pb2 as flyformatioln_pb2 +import logging import rospy @@ -18,29 +19,11 @@ from sensor_msgs.msg import NavSatFix from sensor_msgs.msg import Imu from sensor_msgs.msg import Range from geometry_msgs.msg import Vector3 -''' -# test proto -import random -class fakeGps(): - def __init__(self): - self.latitude = 0.0 - self.longitude = 20 - self.altitude = 30 -class fake_hdg(): - def __init__(self): - self.data = 40 -# test json -class state(): - def __init__(self): - self.mode = "test" -''' - - -def init_dataFormat(cfg): +def init_dataFormat(cfg:utils.Read_PUB_Config): ros_namespace="/drone1" if cfg.msg_format == "Proto": utils.Proto_msg_from_ros.client = client @@ -59,26 +42,44 @@ def init_dataFormat(cfg): rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps) rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg) else: - print("msg_format not found") + logging.debug("msg_format not found") -def on_message(client, userdata, msg): - pass def on_connect(self, userdata, flags, rc): - print("Connected with result code " + str(rc)) + logger.info("Connected with result code " + str(rc)) def on_publish(self, userdata, mid): - print("pub ok") + logger.debug("pub success") + +def on_disconnect(client, userdata, rc): + logger.debug("disconnecting reason " +str(rc)) + client.connected_flag=False + client.disconnect_flag=True if __name__ == '__main__': # Read Config - FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig.yml") - cfg = utils.MQTT_ROS_Config(FilePath) + FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB.yml") + cfg = utils.Read_PUB_Config(FilePath) client = utils.MQTTClient(cfg.MQTTClientNamePub) - print(cfg) - # Setting Config - + # 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(__name__) + logger.setLevel(logging.INFO) + logger.addHandler(file_handler) + logger.addHandler(stream_handler) + logger.info(cfg) + # Mqtt client.on_connect = on_connect client.on_message = on_message @@ -90,31 +91,14 @@ if __name__ == '__main__': # Ros rosClient = cfg.ROSClientNamePub rospy.init_node(rosClient) - init_dataFormat(cfg) - - - rospy.spin() - -''' - #test proto - gps = fakeGps() - hdg = fake_hdg() - state = state() - while True: - #test proto - - gps.latitude = random.uniform(0, 10) - gps.longitude = random.uniform(0, 10) - gps.altitude = random.uniform(0, 10) - hdg.data = random.uniform(0, 10) - - utils.Proto_msg_from_ros.callBack_gps(gps) - utils.Proto_msg_from_ros.callBack_compass_hdg(hdg) - time.sleep(0.25) - #test json - # utils.Json_msg_from_ros.callBack_gps(gps) - # utils.Json_msg_from_ros.callBack_compass_hdg(hdg) - # utils.Json_msg_from_ros.callBack_state(state) - # time.sleep(1) -''' + # init data format + init_dataFormat(cfg) + + try: + rospy.spin() + except KeyboardInterrupt as e: + client.disconnect() + logger.info("End of program") + sys.exit() + \ No newline at end of file diff --git a/class_model/src/mqttSubMain.py b/class_model/src/mqttSubMain.py index ede39a2..9d5448e 100755 --- a/class_model/src/mqttSubMain.py +++ b/class_model/src/mqttSubMain.py @@ -9,8 +9,10 @@ import proto.flight_information_pb2 as flight_information_pb2 import proto.flyformatioln_pb2 as flyformatioln_pb2 import rospy from std_msgs.msg import String +import logging -def init_dataFormat(cfg): + +def init_dataFormat(cfg:utils.Read_SUB_Config): if cfg.msg_format == "Proto": utils.Proto_msg_to_ros.rate = rospy.Rate(10) utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10) @@ -32,15 +34,18 @@ def init_dataFormat(cfg): utils.Json_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt utils.Json_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt else: - print("msg_format not found") + logging.debug("msg_format not found") -# def on_message(client, userdata, msg): -# utils.Json_msg_to_ros.ros_pub(msg) def on_connect(self, userdata, flags, rc): - print("Connected with result code " + str(rc)) + logging.info("Connected with result code " + str(rc)) client.subscribe(cfg.Flight_Information_topicToMqtt) + client.subscribe(cfg.Fly_Formation_topicToMqtt) +def on_disconnect(client, userdata, rc): + # logging.info("disconnecting reason " +str(rc)) + client.connected_flag=False + client.disconnect_flag=True def on_publish(self, userdata, mid): pass @@ -48,16 +53,35 @@ def on_publish(self, userdata, mid): if __name__ == '__main__': # Read Config - FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig.yml") - # Read Config - cfg = utils.MQTT_ROS_Config(FilePath) - print(cfg) - + FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_SUB.yml") + cfg = utils.Read_SUB_Config(FilePath) + client = utils.MQTTClient(cfg.MQTTClientNameSub) + + # 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(__name__) + logger.setLevel(logging.DEBUG) + logger.addHandler(file_handler) + logger.addHandler(stream_handler) + + logger.info(cfg) + # Mqtt client = utils.MQTTClient(cfg.MQTTClientNameSub) client.on_connect = on_connect - client.on_message = utils.Json_msg_to_ros.on_message client.on_publish = on_publish + client.on_disconnect = on_disconnect client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) @@ -66,5 +90,11 @@ if __name__ == '__main__': # initialize init_dataFormat(cfg) - client.loop_start() - rospy.spin() \ No newline at end of file + try: + client.loop_start() + rospy.spin() + except KeyboardInterrupt: + client.loop_stop() + client.disconnect() + logger.info("End of program") + sys.exit(0) \ No newline at end of file diff --git a/class_model/src/proto/flight_information.proto b/class_model/src/proto/flight_information.proto index a53b4f3..620a8c8 100644 --- a/class_model/src/proto/flight_information.proto +++ b/class_model/src/proto/flight_information.proto @@ -1,5 +1,5 @@ syntax = 'proto3'; - +package flight_information_proto; // GPS + compass message GPS { diff --git a/class_model/src/proto/flight_information_pb2.py b/class_model/src/proto/flight_information_pb2.py index 63c28d4..d35aec8 100644 --- a/class_model/src/proto/flight_information_pb2.py +++ b/class_model/src/proto/flight_information_pb2.py @@ -13,15 +13,15 @@ _sym_db = _symbol_database.Default() -DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x18\x66light_information.proto\"S\n\x03GPS\x12\x10\n\x03LAT\x18\x01 \x01(\x02H\x00\x88\x01\x01\x12\x10\n\x03LON\x18\x02 \x01(\x02H\x01\x88\x01\x01\x12\x10\n\x03\x41LT\x18\x03 \x01(\x02H\x02\x88\x01\x01\x42\x06\n\x04_LATB\x06\n\x04_LONB\x06\n\x04_ALT\"Q\n\x1a\x66light_information_message\x12\x11\n\x03gps\x18\x01 \x01(\x0b\x32\x04.GPS\x12\x14\n\x07heading\x18\x02 \x01(\x02H\x00\x88\x01\x01\x42\n\n\x08_headingb\x06proto3') +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=28 - _GPS._serialized_end=111 - _FLIGHT_INFORMATION_MESSAGE._serialized_start=113 - _FLIGHT_INFORMATION_MESSAGE._serialized_end=194 + _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) diff --git a/class_model/src/proto/flyformatioln.proto b/class_model/src/proto/flyformatioln.proto index 8825dcb..56a6d58 100644 --- a/class_model/src/proto/flyformatioln.proto +++ b/class_model/src/proto/flyformatioln.proto @@ -1,5 +1,5 @@ syntax = "proto3"; - +package fly_formation_proto; // leader only enum FLY_FORMATION{ diff --git a/class_model/src/proto/flyformatioln_pb2.py b/class_model/src/proto/flyformatioln_pb2.py index ef2a0e0..34014f4 100644 --- a/class_model/src/proto/flyformatioln_pb2.py +++ b/class_model/src/proto/flyformatioln_pb2.py @@ -13,15 +13,15 @@ _sym_db = _symbol_database.Default() -DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x13\x66lyformatioln.proto\"b\n\x15\x66ly_formation_message\x12\x15\n\x08velocity\x18\x01 \x01(\x02H\x00\x88\x01\x01\x12%\n\rfly_formation\x18\x02 \x01(\x0e\x32\x0e.FLY_FORMATIONB\x0b\n\t_velocity*\xb3\x01\n\rFLY_FORMATION\x12\x1d\n\x19\x46LY_FORMATION_UNSPECIFIED\x10\x00\x12\x13\n\x0f\x46LY_FORMATION_v\x10\x01\x12\x13\n\x0f\x46LY_FORMATION_X\x10\x02\x12\x13\n\x0f\x46LY_FORMATION_O\x10\x03\x12\x16\n\x12\x46LY_FORMATION_LINE\x10\x04\x12\x15\n\x11\x46LY_FORMATION_ROW\x10\x05\x12\x15\n\x11\x46LY_FORMATION_HEX\x10\x06\x62\x06proto3') +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=124 - _FLY_FORMATION._serialized_end=303 - _FLY_FORMATION_MESSAGE._serialized_start=23 - _FLY_FORMATION_MESSAGE._serialized_end=121 + _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/class_model/src/tower.py b/class_model/src/tower.py index ef27e1f..c755852 100755 --- a/class_model/src/tower.py +++ b/class_model/src/tower.py @@ -11,11 +11,6 @@ def on_connect(self, userdata, flags, rc): connect_flag = True -# def on_message(self, userdata, msg): -# print(f"Receive UAV_Z550 {msg.payload.decode('utf-8')}") -# print(f"Receive UAV_H380 {msg.payload.decode('utf-8')}") -# print("command: ", end="") - def initialise_clients(clientName): # callback assignment diff --git a/class_model/src/utils/mqttConfig.yml b/class_model/src/utils/mqttConfig_PUB.yml similarity index 50% rename from class_model/src/utils/mqttConfig.yml rename to class_model/src/utils/mqttConfig_PUB.yml index 86cc5d3..f4b79f7 100644 --- a/class_model/src/utils/mqttConfig.yml +++ b/class_model/src/utils/mqttConfig_PUB.yml @@ -1,21 +1,22 @@ -MQTT_ROS: +MQTT: msg_format: Json - MQTTClientNamePub: Drone650Pub - MQTTClientNameSub: Drone650Sub - host: 192.168.50.27 + MQTTClientNamePub: Drone550Pub + host: 192.168.50.118 port: 1883 keepalive: 60 willTopic: CheckDoneConnect willTopicQOS: 1 - lwt: Drone650 Gone Offline + lwt: Dron550 Gone Offline willRetain: False # Mqtt topic - Flight_Information_topicToMqtt: drone/leader/Flight_Information - Fly_Formation_topicToMqtt: drone/leader/Formation + Flight_Information_topicToMqtt: Drone550/Flight_Information + Fly_Formation_topicToMqtt: Drone550/Formation # Change formate qos Fly_Formation_topicToMqtt_QOS: 2 #ROS - ROSClientNamePub: Drone650Pub - ROSClientNameSub: Drone650Sub +ROS: + ROSClientNamePub: Drone550Pub ROStopicName_Flight_Information: Flight_Information_reciver ROStopicName_Fly_Formation: Fly_Formation_reciver +LOG: + logFileName: pub.log diff --git a/class_model/src/utils/mqttConfig_SUB.yml b/class_model/src/utils/mqttConfig_SUB.yml new file mode 100644 index 0000000..ec37b3d --- /dev/null +++ b/class_model/src/utils/mqttConfig_SUB.yml @@ -0,0 +1,21 @@ +MQTT: + msg_format: Json + MQTTClientNameSub: Drone550Sub + host: 192.168.50.118 + 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: + ROSClientNameSub: Drone550Sub + ROStopicName_Flight_Information: Flight_Information_reciver + ROStopicName_Fly_Formation: Fly_Formation_reciver +LOG: + logFileName: sub.log diff --git a/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py b/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py index 8b6c210..a3c517d 100644 --- a/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py +++ b/class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py @@ -25,13 +25,13 @@ class Proto_msg_to_ros: def ros_pub(cls, dataProto): if dataProto.topic == cls.Flight_Information_topicToMqtt: proto_msg = cls.flight_information_msg.FromString(dataProto.payload) - protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True) - print(protoTOJson_msg) + 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) + 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: @@ -55,10 +55,10 @@ class Json_msg_to_ros: @classmethod def ros_pub(cls, dataJson): if dataJson.topic == cls.Flight_Information_topicToMqtt: - cls.publisher_Flight_Information.publish(dataJson.payload.decode('utf-8')) + 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.publisher_Fly_Formation.publish(dataJson.payload.decode("UTF-8")) cls.rate.sleep() else: print("topic not found") diff --git a/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py b/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py index bde5878..a81ceb4 100644 --- a/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py +++ b/class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py @@ -4,21 +4,19 @@ import time class Proto_msg_from_ros: #Protobuf flight_information_msg = None - fly_formation_msg = None # delcare in function + 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): - # flight_information_msg = flight_information_pb2.flight_information_message() cls.flight_information_msg.gps.LAT = GPS.latitude cls.flight_information_msg.gps.LON = GPS.longitude cls.flight_information_msg.gps.ALT = GPS.altitude - # print ('lat:'+lat+'\n'+'lon:'+lon+'\n') - # print(cls.flight_information_msg) @classmethod def callBack_compass_hdg(cls, Compass): @@ -32,18 +30,15 @@ class Proto_msg_from_ros: @classmethod def callBack_fly_formation(cls, Formation): cls.fly_formation_msg.velocity = Formation.velocity - # cls.fly_formation_msg.fly_formation = flyformatioln_pb2.FLY_FORMATION_v - cls.fly_formation_msg.fly_formation = 2 + 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 - # publish a message def mqtt_Pub(cls, message:bytes, topics:str, waitForAck:bool=False, qos:int=0)->None: mid = cls.client.publish(topics, message, qos)[1] - # print(f"just published {message} to topic") if waitForAck: while mid not in cls.client.topic_ack: print("wait for ack") @@ -54,13 +49,14 @@ class Proto_msg_from_ros: class Json_msg_from_ros: - data = {} + 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) @@ -73,8 +69,7 @@ class Json_msg_from_ros: dataImuUpdate = {"gyro_x": gyro_x, "gyro_y": gyro_y,"gyro_z":gyro_z, "accel_x": accel_x, "accel_y": accel_y, "accel_z": accel_z} cls.data.update(dataImuUpdate) - # print ('gyro_x:'+gyro_x+'\n'+'gyro_y:'+gyro_y+'\n'+'gyro_z:'+gyro_z +'\n') - print ('accel_x:'+accel_x+'\n'+'accel_y:'+accel_y+'\n'+'accel_z:'+accel_z +'\n') + @classmethod def callBack_gps(cls, GPS): @@ -82,30 +77,28 @@ class Json_msg_from_ros: lon=int(GPS.longitude*10000000) alt=int(GPS.altitude*100) dataGpsUpdate = {"lat": lat, "lon": lon, "alt": alt} - cls.data.update(dataGpsUpdate) - # dataJsonFormate = orjson.dumps(data) - # mqtt_Pub(message=dataJsonFormate) - # print ('lat:'+lat+'\n'+'lon:'+lon+'\n') + cls.GPS_Data.update(dataGpsUpdate) + dataJsonFormate = orjson.dumps(cls.GPS_Data) + cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Flight_Information_topicToMqtt) - @classmethod - def callBack_state(cls, state): - mode = state.mode - dataGpsUpdate = {"mode": mode} - cls.data.update(dataGpsUpdate) +# TODO: does decode need utf8 @classmethod def callBack_compass_hdg(cls, Compass): heading = int(Compass.data*100) dataGpsUpdate = {"heading": heading} cls.data.update(dataGpsUpdate) - dataJsonFormate = orjson.dumps(cls.data).decode("UTF-8") + dataJsonFormate = orjson.dumps(cls.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] - # print(f"just published {message} to topic") if waitForAck: while mid not in cls.client.topic_ack: print("wait for ack") diff --git a/class_model/src/utils/readConfig.py b/class_model/src/utils/readConfig.py index c94a9aa..683b5e3 100644 --- a/class_model/src/utils/readConfig.py +++ b/class_model/src/utils/readConfig.py @@ -1,65 +1,107 @@ import yaml - -class MQTT_ROS_Config: - """ - MQTT_ROS_Config configuration class - """ - # class variables - sectionName='MQTT_ROS' - options={ - 'msg_format': (str,True), - 'MQTTClientNamePub': (str,True), - 'MQTTClientNameSub': (str,True), - 'host': (str,True), - 'port': (int,True), - 'keepalive': (int,True), - 'willTopic':(str,True), - 'lwt':(str, True), - 'willRetain':(bool,False), - 'willTopicQOS':(int,True), - 'Flight_Information_topicToMqtt': (str,False), - 'Fly_Formation_topicToMqtt': (str,False), - 'Fly_Formation_topicToMqtt_QOS':(int,False), - 'ROSClientNamePub': (str,False), - 'ROSClientNameSub': (str,False), - 'ROStopicName_Flight_Information': (str,False), - 'ROStopicName_Fly_Formation': (str,False)} - - #constructor +class Config: def __init__(self, inFileName): - #read YAML config and get EV3 section - infile=open(inFileName,'r') - ymlcfg=yaml.safe_load(infile) - infile.close() - eccfg=ymlcfg.get(self.sectionName,None) - if eccfg is None: raise Exception('Missing {} section in cfg file'.format(self.sectionName)) - + 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 opt in self.options: - if opt in eccfg: - optval=eccfg[opt] - - #verify parameter type - if type(optval) != self.options[opt][0]: - raise Exception('Parameter "{}" has wrong type'.format(opt)) - - #create attributes on the fly - setattr(self,opt,optval) - else: - if self.options[opt][1]: - raise Exception('Missing mandatory parameter "{}"'.format(opt)) + 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: - setattr(self,opt,None) - - #string representation for class data + 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 str(yaml.dump(self.__dict__,default_flow_style=False)) + 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__() + - -if __name__ == '__main__': - cfg=MQTT_ROS_Config("mqttConfig.yml") - print(cfg) +if __name__ == "__main__": + cfg=SUB_Config("mqttConfig_PUB.yml") + cfg.setAttribute() + print(cfg.msg_format)