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 d129a6f..08a5da0 100755 --- a/class_model/src/uavlinkSubMain.py +++ b/class_model/src/uavlinkSubMain.py @@ -22,12 +22,21 @@ from geometry_msgs.msg import Vector3 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) @@ -36,8 +45,8 @@ def init_dataFormat(cfg:Read_SUB_Config): 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,7 +59,7 @@ 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) @@ -59,6 +68,10 @@ if __name__ == '__main__': 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 @@ -70,13 +83,30 @@ if __name__ == '__main__': 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) - - except KeyboardInterrupt as e: +======= + 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) +>>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5 + + except Exception as e: 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 626dbc3..ecd14a3 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,7 +19,8 @@ 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) 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 5cb21d7..146ad9b 100644 --- a/class_model/src/utils/uavlinkConfig_SUB.yml +++ b/class_model/src/utils/uavlinkConfig_SUB.yml @@ -2,10 +2,14 @@ UAVLINK: uavlink_msg_format: Proto uav_id: \x01\x01 baudrate: 250000 +<<<<<<< HEAD ttyport: /dev/ttyUSB0 +======= + ttyport: /dev/ttyTHS1 +>>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5 MQTT: "None" ROS: ROSClientNameSub: Drone550UAVLINKSub Dron550_ROStopicName_Flight_Information: Flight_Information_reciver LOG: - logFileName: UAVLINKsub.log + logFileName: UAVLINKsub.log \ No newline at end of file