Client: Add config parser and 3 variants of emergency actions to visual pose watchdog

Drone/visual_pose_watchdog.py

@@ -2,9 +2,23 @@ import rospy
 import sys
 import time
 import logging
-from std_srvs.srv import Trigger
+import ConfigParser
+from clever.srv import SetAttitude
+from sensor_msgs.msg import Range
+from mavros_msgs.msg import State
+from mavros_msgs.srv import SetMode, CommandBool
+from std_msgs.msg import Bool
 from geometry_msgs.msg import PoseStamped
 
+config = ConfigParser.ConfigParser()
+config.read("client_config.ini")
+
+visual_pose_timeout = config.getfloat('VISUAL_POSE_WATCHDOG', 'timeout')
+timeout_action = config.get('VISUAL_POSE_WATCHDOG', 'action')
+emergency_land_thrust = config.getfloat('VISUAL_POSE_WATCHDOG', 'emergency_land_thrust')
+emergency_land_decrease_thrust_after = config.getfloat('VISUAL_POSE_WATCHDOG', 'emergency_land_decrease_thrust_after')
+timeout_to_disarm_after_watchdog_action = config.getfloat('VISUAL_POSE_WATCHDOG', 'timeout_to_disarm_after_watchdog_action')
+
 logging.basicConfig(  # TODO all prints as logs
    level=logging.DEBUG, # INFO
    stream=sys.stdout,
@@ -19,30 +33,112 @@ formatter = logging.Formatter("%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s]
 handler.setFormatter(formatter)
 
 logger = logging.getLogger(__name__)
-logger.setLevel(logging.DEBUG)
+logger.setLevel(logging.INFO)
 logger.addHandler(handler)
 
-land = rospy.ServiceProxy('/land', Trigger)
+set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
+arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
+set_attitude = rospy.ServiceProxy('/set_attitude', SetAttitude)
 
 visual_pose_last_timestamp = 0
-visual_pose_timeout = 2.
+armed = False
+mode = ''
+laser_range = 10
+emergency = False
 
 rospy.init_node('visual_pose_watchdog')
 logger.info('visual_pose_watchdog inited')
+logger.info('timeout = {} | timeout_action = {}'.format(visual_pose_timeout, timeout_action))
+logger.info('timeout_to_disarm_after_watchdog_action = {}'.format(timeout_to_disarm_after_watchdog_action))
+if timeout_action == 'emergency_land':
+    logger.info('emergency_land_thrust: {}'.format(emergency_land_thrust))
+
+rate = rospy.Rate(10)
 
 def visual_pose_callback(data):
     global visual_pose_last_timestamp
     visual_pose_last_timestamp = data.header.stamp.to_sec()
 
+def state_callback(data):
+    global armed, mode
+    armed = data.armed
+    mode = data.mode
+
+def laser_callback(data):
+    global laser_range
+    laser_range = data.range
+
 def watchdog_callback(event):
-    global visual_pose_last_timestamp
-    if (time.time() - visual_pose_last_timestamp) > visual_pose_timeout and visual_pose_last_timestamp != 0:
-        logger.info('Visual pose data is too old, landing...')
-        land()
+    global visual_pose_last_timestamp, armed, mode, timeout_action, laser_range, emergency
+    logger.debug("armed: {} | mode: {} | delta: {} | action: {} | range: {}".format(armed, mode, abs(time.time() - visual_pose_last_timestamp), timeout_action, laser_range))
+    if abs(time.time() - visual_pose_last_timestamp) > visual_pose_timeout:
+        if armed:
+            if timeout_action in ['land', 'emergency_land', 'disarm']:
+                emergency = True
+            if timeout_action == 'land':
+                logger.info('Visual pose data is too old, copter is armed, landing...')
+                while mode != "AUTO.LAND":
+                    try:
+                        set_mode(custom_mode='AUTO.LAND')
+                    except rospy.ServiceException as e:
+                        logger.info(e)
+                    rate.sleep()
+                logger.info('Land mode is set')
+                action_timestamp = time.time()
+                while armed:
+                    if time.time() - action_timestamp > timeout_to_disarm_after_watchdog_action:
+                        try:
+                            arming(False)
+                        except rospy.ServiceException as e:
+                            logger.info(e)   
+                    rate.sleep()
+            elif timeout_action == 'disarm':
+                logger.info('Visual pose data is too old, copter is armed, disarming...')
+                while armed:
+                    try:
+                        arming(False)
+                    except rospy.ServiceException as e:
+                        logger.info(e)   
+                    rate.sleep() 
+            elif timeout_action == 'emergency_land':
+                logger.info('Visual pose data is too old, copter is armed, emergency landing...')
+                action_timestamp = time.time()
+                current_thrust = emergency_land_thrust
+                while armed:
+                    logger.debug("Emergency land | range: {} | thrust: {}".format(laser_range, current_thrust))
+                    try:
+                        set_attitude(thrust = current_thrust, yaw = 0, frame_id = 'body')
+                    except rospy.ServiceException as e:
+                        logger.info(e) 
+                    delta = time.time() - action_timestamp
+                    if delta > timeout_to_disarm_after_watchdog_action:
+                        try:
+                            arming(False)
+                        except rospy.ServiceException as e:
+                            logger.info(e)  
+                    if (laser_range < 0.1 or delta > emergency_land_decrease_thrust_after) and current_thrust > 0.:
+                        current_thrust -= 0.02
+                        if current_thrust < 0:
+                            current_thrust = 0  
+                    rate.sleep()
+            logger.info('Disarmed') 
+            emergency = False                
+        else:
+            logger.info('Visual pose data is too old')
 
 rospy.Subscriber('/mavros/vision_pose/pose', PoseStamped, visual_pose_callback)
 
+rospy.Subscriber('/mavros/state', State, state_callback)
+
+rospy.Subscriber('/mavros/distance_sensor/rangefinder', Range, laser_callback)
+
+emergency_pub = rospy.Publisher('/emergency', Bool, queue_size=10)
+
 rospy.Timer(rospy.Duration(0.5), watchdog_callback)
 
-rospy.spin()
+while not rospy.is_shutdown():
+    emergency_msg = Bool()
+    emergency_msg.data = emergency
+    emergency_pub.publish(emergency_msg)
+    rate.sleep()