1.uavlink update topic 2.add shutdown uavlink(turoff module) 3.add detection

class_model/src/uavlinkPubMain.py

@@ -23,10 +23,13 @@ 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(10)
+    Proto_msg_from_ros.rate = rospy.Rate(5)
     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)
 
+def shutdown_callback():
+    Proto_msg_from_ros.turnOffUavlink()
+    
 if __name__ == '__main__':
     FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_PUB.yml")
     cfg = Read_PUB_Config(FilePath)
@@ -44,7 +47,7 @@ if __name__ == '__main__':
 
     file_handler = logging.FileHandler(cfg.logFileName, mode='w')
     file_handler.setFormatter(file_formatter)
-    file_handler.setLevel(logging.INFO)
+    file_handler.setLevel(logging.DEBUG)
 
     logger = logging.getLogger("__UAVLINKSUBPUB__")
     logger.setLevel(logging.DEBUG)
@@ -55,15 +58,16 @@ if __name__ == '__main__':
     # Ros
     rosClient = cfg.ROSClientNamePub
     rospy.init_node(rosClient)
+    rospy.on_shutdown(shutdown_callback)
 
     # init data format
     init_dataFormat(cfg)
 
     try:
         rospy.spin()
-    except KeyboardInterrupt as e:
+    except rospy.ROSInterruptException as e:
         Proto_msg_from_ros.turnOffUavlink()
-        print("End of program")
+        logger.debug("End of program")
         sys.exit()
 
      

class_model/src/uavlinkSubMain.py

@@ -20,15 +20,19 @@ from sensor_msgs.msg import Imu
 from sensor_msgs.msg import Range
 from geometry_msgs.msg import Vector3
 # costom msg
-from class_mode.msg import FlightInformation
+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.topicName_Flight_Information,FlightInformation,queue_size=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
+
+def shutdown_callback():
+    Proto_msg_to_ros.turnOffUavlink()
+
 if __name__ == '__main__':
     FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_SUB.yml")
     cfg = Read_SUB_Config(FilePath)
@@ -55,9 +59,8 @@ if __name__ == '__main__':
     logger.debug(cfg)
 
     rospy.init_node(cfg.ROSClientNameSub)  
+    rospy.on_shutdown(shutdown_callback)
     init_dataFormat(cfg)
- 
-
 
     while True:
         try:
@@ -68,7 +71,7 @@ if __name__ == '__main__':
         except google.protobuf.message.DecodeError as e:
             logger.info("DecodeError:{}".format(e))
             logger.info("readTenByte:{}".format(readTenByte))
-        except KeyboardInterrupt as e:
+        except rospy.ROSInterruptException as e:
             Proto_msg_to_ros.turnOffUavlink()
             print("End of program")
             sys.exit()

class_model/src/utils/proto_uavlink_pub_data_to_ros.py

@@ -4,7 +4,8 @@ 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__")
 

class_model/src/utils/uavlinkConfig_SUB.yml

@@ -6,6 +6,6 @@ UAVLINK:
 MQTT: "None"
 ROS:
   ROSClientNameSub: Drone550UAVLINKSub
-  Dron550_ROStopicName_Flight_Information: Flight_Information_reciver
+  Dron550_ROStopicName_Flight_Information: Flight_Information_Uavlink_reciver
 LOG:
   logFileName: UAVLINKsub.log