From b09489cc80f46d26159d874dd736cf25fc1c4423 Mon Sep 17 00:00:00 2001 From: RangeOfGlitching Date: Sat, 15 Apr 2023 15:31:53 +0800 Subject: [PATCH] uavlink update --- class_model/src/uavlinkPubMain.py | 35 ++++---- class_model/src/uavlinkSubMain.py | 79 ++++++------------- .../utils/proto_uavlink_pub_data_to_ros.py | 38 ++++++--- .../utils/proto_uavlink_sub_data_from_ros.py | 30 +++++-- class_model/src/utils/uavlinkConfig_PUB.yml | 2 +- class_model/src/utils/uavlinkConfig_SUB.yml | 4 - 6 files changed, 91 insertions(+), 97 deletions(-) diff --git a/class_model/src/uavlinkPubMain.py b/class_model/src/uavlinkPubMain.py index 42fe593..9460353 100755 --- a/class_model/src/uavlinkPubMain.py +++ b/class_model/src/uavlinkPubMain.py @@ -21,52 +21,49 @@ from geometry_msgs.msg import Vector3 def init_dataFormat(cfg:Read_PUB_Config): - f1data = b'\xf1drone550pub...........\r\n' - sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, timeout=0.3) - sel.write(f1data) - readTenByte = sel.readline() - print(readTenByte) - Proto_msg_from_ros.sel = sel + Proto_msg_from_ros.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(10) 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 - log_format = "%(asctime)s - %(levelname)s - %(message)s" - formatter = logging.Formatter(log_format) + 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(formatter) + stream_handler.setFormatter(stream_formatter) stream_handler.setLevel(logging.DEBUG) - file_handler = logging.FileHandler(cfg.logFileName) - file_handler.setFormatter(formatter) + 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.INFO) + logger.setLevel(logging.DEBUG) logger.addHandler(file_handler) logger.addHandler(stream_handler) - logger.info(cfg) - - + logger.debug(cfg) # Ros rosClient = cfg.ROSClientNamePub rospy.init_node(rosClient) - + # init data format init_dataFormat(cfg) try: rospy.spin() - except BaseException as error: - logger.info("End of program") + except KeyboardInterrupt as e: + Proto_msg_from_ros.turnOffUavlink() + print("End of program") sys.exit() diff --git a/class_model/src/uavlinkSubMain.py b/class_model/src/uavlinkSubMain.py index 08a5da0..910dd53 100755 --- a/class_model/src/uavlinkSubMain.py +++ b/class_model/src/uavlinkSubMain.py @@ -8,6 +8,7 @@ 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 @@ -18,95 +19,59 @@ 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 mqtt.msg import FlightInformation def init_dataFormat(cfg:Read_SUB_Config): - ros_namespace="/drone1" -<<<<<<< HEAD - f1data = b'\xf1drone550..............\r\x1a' - - sel.write(f1data) - readTenByte = sel.read_until(size=5) -======= - deviceData = b'\xf12222222222222222222222\r\x1a' - - sel.write(deviceData) - time.sleep(1) - readTenByte = sel.read(size=5) ->>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5 - print(readTenByte) - - 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,String,queue_size=10) - + Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.topicName_Flight_Information,FlightInformation,queue_size=10) + Proto_msg_to_ros.FlightInformationRosMsg = FlightInformation() if __name__ == '__main__': FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_SUB.yml") cfg = Read_SUB_Config(FilePath) - sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, timeout = 0.5) + sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE) # set log - log_format = "%(asctime)s - %(levelname)s - %(message)s" - formatter = logging.Formatter(log_format) + 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(formatter) + stream_handler.setFormatter(stream_formatter) stream_handler.setLevel(logging.DEBUG) - file_handler = logging.FileHandler(cfg.logFileName) - file_handler.setFormatter(formatter) + 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.INFO) + logger.setLevel(logging.DEBUG) logger.addHandler(file_handler) logger.addHandler(stream_handler) - logger.info(cfg) - + logger.debug(cfg) rospy.init_node(cfg.ROSClientNameSub) init_dataFormat(cfg) -<<<<<<< HEAD -======= ->>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5 - flight_information_msg = flight_information_pb2.flight_information_message() - flight_information_msg.gps.LAT = 34123.1231515 - flight_information_msg.gps.LON = 23423.1231515 - flight_information_msg.gps.ALT = 12123.1231515 - flight_information_msg.heading = 155.12215 - proto = flight_information_msg.SerializeToString() - readTenByte = b'\xf1' + proto + b'\r\x1a' + while True: try: - # test json -<<<<<<< HEAD - # readTenByte = sel.read_until(expected= b'\x01\x01', size=25) - - Proto_msg_to_ros.on_message_Flight_Information(readTenByte) -======= if sel.in_waiting >= 25: - readTenByte = sel.read_until(expected= b'\x01\x01', size=25) - # print(readTenByte) - if b"drone550" in readTenByte: - print(readTenByte) - print("radar") - continue - + readTenByte = sel.read_until(expected= b'\x01\x17', size=25) Proto_msg_to_ros.on_message_Flight_Information(readTenByte) ->>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5 - except Exception as e: + 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") -<<<<<<< HEAD sys.exit() -======= - print(readTenByte) - pass ->>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5 diff --git a/class_model/src/utils/proto_uavlink_pub_data_to_ros.py b/class_model/src/utils/proto_uavlink_pub_data_to_ros.py index ecd14a3..88b5dc3 100644 --- a/class_model/src/utils/proto_uavlink_pub_data_to_ros.py +++ b/class_model/src/utils/proto_uavlink_pub_data_to_ros.py @@ -3,7 +3,8 @@ import time import proto.flight_information_pb2 as flight_information_pb2 import google.protobuf.json_format as json_format import logging - +# costom msg +from mqtt.msg import FlightInformation # TODO: use native ros type instead of json or str logger = logging.getLogger("__SUB__") @@ -12,20 +13,39 @@ class Proto_msg_to_ros: 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): - proto = msg[1:-2] - - proto_msg = cls.flight_information_msg.FromString(proto) - protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True) - logger.info(protoTOJson_msg) - print(protoTOJson_msg) - cls.publisher_Flight_Information.publish(protoTOJson_msg) + cls.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 - \ No newline at end of file + @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) diff --git a/class_model/src/utils/proto_uavlink_sub_data_from_ros.py b/class_model/src/utils/proto_uavlink_sub_data_from_ros.py index 3529534..f8afdb6 100644 --- a/class_model/src/utils/proto_uavlink_sub_data_from_ros.py +++ b/class_model/src/utils/proto_uavlink_sub_data_from_ros.py @@ -2,12 +2,23 @@ import orjson import time import logging -# logger = logging.getLogger("__PUB__") +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 @@ -15,14 +26,19 @@ class Proto_msg_from_ros: 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): cls.flight_information_msg.heading = Compass.data flightInformationMsg = cls.flight_information_msg.SerializeToString() - cls.sel.write(b'\xf2' + flightInformationMsg + b'\r\n') - print(len(b'\xf2' + flightInformationMsg + b'\r\n')) - readTenByte = cls.sel.readline() - print(readTenByte) + 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) diff --git a/class_model/src/utils/uavlinkConfig_PUB.yml b/class_model/src/utils/uavlinkConfig_PUB.yml index c74c016..5e2f19d 100644 --- a/class_model/src/utils/uavlinkConfig_PUB.yml +++ b/class_model/src/utils/uavlinkConfig_PUB.yml @@ -1,6 +1,6 @@ UAVLINK: uavlink_msg_format: Proto - uav_id: \x01\x01 + uav_id: \x01\x17 baudrate: 250000 ttyport: /dev/ttyTHS1 MQTT: "None" diff --git a/class_model/src/utils/uavlinkConfig_SUB.yml b/class_model/src/utils/uavlinkConfig_SUB.yml index 146ad9b..d6dedb7 100644 --- a/class_model/src/utils/uavlinkConfig_SUB.yml +++ b/class_model/src/utils/uavlinkConfig_SUB.yml @@ -2,11 +2,7 @@ UAVLINK: uavlink_msg_format: Proto uav_id: \x01\x01 baudrate: 250000 -<<<<<<< HEAD - ttyport: /dev/ttyUSB0 -======= ttyport: /dev/ttyTHS1 ->>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5 MQTT: "None" ROS: ROSClientNameSub: Drone550UAVLINKSub