failsafe.py

import rospy
import os
import sys
import time
import math
import logging
import threading

# for backward compatibility with clever
try:
    from clever import srv
except ImportError:
    from clover import srv

from sensor_msgs.msg import Range
from mavros_msgs.msg import State, PositionTarget
from mavros_msgs.srv import SetMode, CommandBool
from std_msgs.msg import Bool
from std_srvs.srv import Trigger, TriggerResponse
from geometry_msgs.msg import PoseStamped

# Add parent dir to PATH to import messaging_lib and config_lib
current_dir = (os.path.dirname(os.path.realpath(__file__)))
lib_dir = os.path.realpath(os.path.join(current_dir, '../lib'))
sys.path.insert(0, lib_dir)

from config import ConfigManager

config = ConfigManager()
config.load_config_and_spec("config/client.ini")

watchdog_is_enabled = config.failsafe_enabled
log_state = config.failsafe_log_state
vision_pose_delay_after_arm = config.failsafe_vision_pose_delay_after_arm
visual_pose_timeout = config.failsafe_vision_pose_timeout
pos_delta_max = config.failsafe_position_delta_max
watchdog_action = config.failsafe_action
timeout_to_disarm = config.failsafe_disarm_timeout
emergency_land_thrust = config.emergency_land_thrust
emergency_land_decrease_thrust_after = config.emergency_land_decrease_thrust_after

logging.basicConfig(  # TODO all prints as logs
   level=logging.DEBUG, # INFO
   stream=sys.stdout,
   format="%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s",
   handlers=[
       logging.StreamHandler(sys.stdout),
   ])

handler = logging.StreamHandler(sys.stdout)
handler.setLevel(logging.DEBUG)
formatter = logging.Formatter("%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] [%(levelname)-5.5s]  %(message)s")
handler.setFormatter(formatter)

logger = logging.getLogger(__name__)
logger.setLevel(logging.DEBUG)
logger.addHandler(handler)

set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
set_attitude = rospy.ServiceProxy('/set_attitude', srv.SetAttitude)

visual_pose_last_timestamp = 0
armed = False
mode = ''
laser_range = 10
emergency = False

local_pose = None
setpoint_raw = None
setpoint_position = None
setpoint_pose = None

arm_start_time = None
offboard_start_time = None
offboard_disarmed_timeout = 3.

emergency_land_called = False

rospy.init_node('visual_pose_watchdog')
logger.info('visual_pose_watchdog inited')
logger.info('visual_pose_timeout = {} | position_delta_max = {} | watchdog_action = {}'.format(visual_pose_timeout, pos_delta_max, watchdog_action))
logger.info('timeout_to_disarm = {}'.format(timeout_to_disarm))
if watchdog_action == 'emergency_land':
    logger.info('emergency_land_thrust: {}'.format(emergency_land_thrust))

rate = rospy.Rate(10)

def get_distance(x1, y1, z1, x2, y2, z2):
    return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2)

def get_pos_delta(PoseStamped1, PoseStamped2):
    if PoseStamped1 is None or PoseStamped2 is None:
        return float('nan')
    pos1 = PoseStamped1.pose.position
    pos2 = PoseStamped2.pose.position
    return get_distance(pos1.x, pos1.y, pos1.z, pos2.x, pos2.y, pos2.z)

def get_time_delta(PoseStamped1, PoseStamped2):
    if PoseStamped1 is None or PoseStamped2 is None:
        return float('nan')
    time1 = PoseStamped1.header.stamp.to_sec()
    time2 = PoseStamped2.header.stamp.to_sec()
    return time1 - time2

def visual_pose_callback(data):
    global visual_pose_last_timestamp
    visual_pose_last_timestamp = data.header.stamp.to_sec()

def local_pose_callback(data):
    global local_pose
    local_pose = data

def setpoint_raw_callback(data):
    global setpoint_raw, setpoint_position, setpoint_pose
    setpoint_raw_pose = PoseStamped()
    setpoint_raw_pose.header = data.header
    setpoint_raw_pose.pose.position = data.position
    setpoint_raw = setpoint_raw_pose
    setpoint_pose = get_current_setpoint_pose(setpoint_raw, setpoint_position)

def setpoint_position_callback(data):
    global setpoint_raw, setpoint_position, setpoint_pose
    setpoint_position = data
    setpoint_pose = get_current_setpoint_pose(setpoint_raw, setpoint_position)

def get_current_setpoint_pose(_setpoint_raw, _setpoint_position):
    if _setpoint_position is None and _setpoint_raw is None:
        return None
    elif _setpoint_position is not None and _setpoint_raw is None:
        return _setpoint_position
    elif _setpoint_raw is not None and _setpoint_position is None:
        return _setpoint_raw
    else:
        return _setpoint_raw if _setpoint_raw.header.stamp > _setpoint_position.header.stamp else _setpoint_position

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 emergency_land(disarm_if_timeout = True):
    global emergency_land_thrust, laser_range
    current_thrust = emergency_land_thrust
    action_timestamp = time.time()
    while armed:
        logger.debug("Emergency land | range: {:.2f} | thrust: {:.2f}".format(laser_range, current_thrust))
        if current_thrust >= 0.03:
            try:
                set_attitude(thrust = current_thrust, yaw = 0, frame_id = 'body', auto_arm = True)
            except rospy.ServiceException as e:
                logger.info(e)
        delta = time.time() - action_timestamp
        if delta > timeout_to_disarm and disarm_if_timeout:
            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.03:
                current_thrust = 0
                try:
                    arming(False)
                except rospy.ServiceException as e:
                    logger.info(e)
        rate.sleep()

def emergency_land_service(request):
    global emergency_land_called, armed
    responce = TriggerResponse()
    if armed:
        responce.success = True
        responce.message = "Start emergency landing"
        emergency_land_called = True
    else:
        responce.success = False
        responce.message = "Copter is disarmed, no need for emergency landing!"
        emergency_land_called = False
    return responce

def watchdog_callback(event):
    global visual_pose_last_timestamp, armed, mode, watchdog_action, laser_range
    global emergency, local_pose, setpoint_pose, emergency_land_called, log_state
    global offboard_start_time, arm_start_time, vision_pose_delay_after_arm
    pos_delta = get_pos_delta(local_pose, setpoint_pose)
    pos_dt = get_time_delta(local_pose, setpoint_pose)
    visual_pose_dt = abs(time.time() - visual_pose_last_timestamp)
    if log_state:
        logger.info("armed: {} | mode: {} | vis_dt: {:.2f} | pos_delta: {:.2f} | pos_dt: {:.2f} | range: {:.2f} | watchdog_action: {}".format(
                    armed, mode, visual_pose_dt, pos_delta, pos_dt, laser_range, watchdog_action))
    if mode == 'OFFBOARD':
        if offboard_start_time is None:
            offboard_start_time = time.time()
        if armed:
            if arm_start_time is None:
                arm_start_time = time.time()
            arm_time = time.time() - arm_start_time
            logger.debug('arm time: {}'.format(arm_time))
            if arm_time > vision_pose_delay_after_arm and watchdog_is_enabled:
                if (visual_pose_dt > visual_pose_timeout and visual_pose_timeout != 0.) or (pos_delta > pos_delta_max and pos_delta_max != 0.):
                    action_timestamp = time.time()
                    emergency = True
                    if watchdog_action not in ['land', 'emergency_land', 'disarm']:
                        watchdog_action = 'land'
                    if watchdog_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)
                            if time.time() - action_timestamp > timeout_to_disarm:
                                break
                            rate.sleep()
                        else:
                            logger.info('Land mode is set')
                        while armed:
                            if time.time() - action_timestamp > timeout_to_disarm:
                                try:
                                    arming(False)
                                except rospy.ServiceException as e:
                                    logger.info(e)
                            rate.sleep()
                    elif watchdog_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 watchdog_action == 'emergency_land':
                        if visual_pose_dt > visual_pose_timeout:
                            logger.info('Visual pose data is too old, copter is armed, emergency landing...')
                        if pos_delta > pos_delta_max:
                            logger.info('Position delta is {} m, copter is armed, emergency landing...'.format(pos_delta))
                        emergency_land()
                    logger.info('Disarmed')
                    emergency = False
            if emergency_land_called:
                emergency = True
                logger.info('/emergency_land service was called, start emergency landing...')
                emergency_land()
                logger.info('Disarmed')
                emergency = False
                emergency_land_called = False
        else:
            arm_start_time = None
            if time.time() - offboard_start_time > offboard_disarmed_timeout:
                try:
                    set_mode(custom_mode='AUTO.LAND')
                except rospy.ServiceException as e:
                    logger.info(e)
    else:
        offboard_start_time = None
        if (abs(time.time() - visual_pose_last_timestamp) > visual_pose_timeout and visual_pose_timeout != 0.0):
            logger.info('Visual pose data is too old')

rospy.Subscriber('/mavros/vision_pose/pose', PoseStamped, visual_pose_callback)

rospy.Subscriber('/mavros/local_position/pose', PoseStamped, local_pose_callback)

rospy.Subscriber('/mavros/setpoint_position/local', PoseStamped, setpoint_position_callback)

rospy.Subscriber('/mavros/setpoint_raw/local', PositionTarget, setpoint_raw_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.Service('emergency_land', Trigger, emergency_land_service)

rospy.Timer(rospy.Duration(0.5), watchdog_callback)

while not rospy.is_shutdown():
    emergency_msg = Bool()
    emergency_msg.data = emergency
    emergency_pub.publish(emergency_msg)
    rate.sleep()