From 8c914b819bf1e908a923a85d37f0af02a44da7d5 Mon Sep 17 00:00:00 2001 From: RangeOfGlitching Date: Sun, 26 Mar 2023 20:13:54 +0800 Subject: [PATCH] uavlink update --- class_model/src/uavlinkPubMain.py | 14 ++++--- class_model/src/uavlinkSubMain.py | 40 +++++++++++++------ .../utils/proto_uavlink_pub_data_to_ros.py | 8 ++-- .../utils/proto_uavlink_sub_data_from_ros.py | 3 +- class_model/src/utils/uavlinkConfig_PUB.yml | 4 +- class_model/src/utils/uavlinkConfig_SUB.yml | 7 ++-- 6 files changed, 46 insertions(+), 30 deletions(-) diff --git a/class_model/src/uavlinkPubMain.py b/class_model/src/uavlinkPubMain.py index 442aa12..42fe593 100755 --- a/class_model/src/uavlinkPubMain.py +++ b/class_model/src/uavlinkPubMain.py @@ -21,15 +21,16 @@ from geometry_msgs.msg import Vector3 def init_dataFormat(cfg:Read_PUB_Config): - ros_namespace="/drone1" - f1data = b'\xf1drone550..............\r\n' - sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE) + f1data = b'\xf1drone550pub...........\r\n' + sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, timeout=0.3) sel.write(f1data) + readTenByte = sel.readline() + print(readTenByte) Proto_msg_from_ros.sel = sel Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message() - rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, Proto_msg_from_ros.callBack_gps) - rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, Proto_msg_from_ros.callBack_compass_hdg) + 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") @@ -47,7 +48,7 @@ if __name__ == '__main__': file_handler.setFormatter(formatter) file_handler.setLevel(logging.INFO) - logger = logging.getLogger(__name__) + logger = logging.getLogger("__UAVLINKSUBPUB__") logger.setLevel(logging.INFO) logger.addHandler(file_handler) logger.addHandler(stream_handler) @@ -67,6 +68,7 @@ if __name__ == '__main__': except BaseException as error: logger.info("End of program") sys.exit() + diff --git a/class_model/src/uavlinkSubMain.py b/class_model/src/uavlinkSubMain.py index fb42535..57e7a37 100644 --- a/class_model/src/uavlinkSubMain.py +++ b/class_model/src/uavlinkSubMain.py @@ -22,22 +22,24 @@ from geometry_msgs.msg import Vector3 def init_dataFormat(cfg:Read_SUB_Config): ros_namespace="/drone1" - 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) 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.ROStopicName_Flight_Information,String,queue_size=10) + Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,String,queue_size=10) if __name__ == '__main__': FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_SUB.yml") cfg = Read_SUB_Config(FilePath) - print(cfg) - sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE) + + sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, timeout = 0.5) # set log log_format = "%(asctime)s - %(levelname)s - %(message)s" formatter = logging.Formatter(log_format) @@ -50,20 +52,34 @@ if __name__ == '__main__': file_handler.setFormatter(formatter) file_handler.setLevel(logging.INFO) - logger = logging.getLogger(__name__) + logger = logging.getLogger("__UAVLINKSUB__") logger.setLevel(logging.INFO) logger.addHandler(file_handler) logger.addHandler(stream_handler) - logger.info(cfg) + logger.info(cfg) + + + rospy.init_node(cfg.ROSClientNameSub) + init_dataFormat(cfg) + while True: try: # test json - readTenByte = sel.read_until(expected= b'\x01\x01', size=25) - + if sel.in_waiting >= 25: + readTenByte = sel.read_until(expected= b'\x01\x01', size=25) + # print(readTenByte) + if b"drone550" in readTenByte: + print(readTenByte) + print("radar") + continue + + Proto_msg_to_ros.on_message_Flight_Information(readTenByte) - except KeyboardInterrupt as e: + except Exception as e: print("End of program") + print(readTenByte) + pass 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 d943acd..a7f8662 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 @@ -10,7 +10,6 @@ logger = logging.getLogger("__SUB__") class Proto_msg_to_ros: #Protobuf flight_information_msg = None - sel = None #Ros publisher rate = None @@ -20,13 +19,12 @@ class Proto_msg_to_ros: #Proto @classmethod def on_message_Flight_Information(cls, msg): - proto = msg[1:23] + 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) cls.publisher_Flight_Information.publish(protoTOJson_msg) cls.rate.sleep() - - - + \ No newline at end of file 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 bd38d76..3529534 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 @@ -23,5 +23,6 @@ class Proto_msg_from_ros: cls.flight_information_msg.heading = Compass.data flightInformationMsg = cls.flight_information_msg.SerializeToString() cls.sel.write(b'\xf2' + flightInformationMsg + b'\r\n') - readTenByte = cls.sel.read_all() + print(len(b'\xf2' + flightInformationMsg + b'\r\n')) + readTenByte = cls.sel.readline() print(readTenByte) diff --git a/class_model/src/utils/uavlinkConfig_PUB.yml b/class_model/src/utils/uavlinkConfig_PUB.yml index 334ec53..c74c016 100644 --- a/class_model/src/utils/uavlinkConfig_PUB.yml +++ b/class_model/src/utils/uavlinkConfig_PUB.yml @@ -2,11 +2,11 @@ UAVLINK: uavlink_msg_format: Proto uav_id: \x01\x01 baudrate: 250000 - ttyport: /dev/ttyUSB0 + ttyport: /dev/ttyTHS1 MQTT: "None" #ROS ROS: ROSClientNamePub: Drone550UAVLINKPub ROStopicName_Flight_Information: Flight_Information_reciver LOG: - logFileName: UAVLINKpub.log + logFileName: UAVLINKpub.log \ No newline at end of file diff --git a/class_model/src/utils/uavlinkConfig_SUB.yml b/class_model/src/utils/uavlinkConfig_SUB.yml index e5bbf0f..d6dedb7 100644 --- a/class_model/src/utils/uavlinkConfig_SUB.yml +++ b/class_model/src/utils/uavlinkConfig_SUB.yml @@ -2,11 +2,10 @@ UAVLINK: uavlink_msg_format: Proto uav_id: \x01\x01 baudrate: 250000 - ttyport: /dev/ttyUSB2 + ttyport: /dev/ttyTHS1 MQTT: "None" ROS: ROSClientNameSub: Drone550UAVLINKSub - ROStopicName_Flight_Information: Flight_Information_reciver - ROStopicName_Fly_Formation: Fly_Formation_reciver + Dron550_ROStopicName_Flight_Information: Flight_Information_reciver LOG: - logFileName: UAVLINKsub.log + logFileName: UAVLINKsub.log \ No newline at end of file