From c91bb949a95e2f8215d3ea59a9a2c397ace27505 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Fri, 27 Dec 2019 21:54:11 +0000 Subject: [PATCH] Client: move landing when pos delta is big to visual_pose_watchdog --- Drone/visual_pose_watchdog.py | 221 ++++++++++++++++++++++++++-------- 1 file changed, 168 insertions(+), 53 deletions(-) diff --git a/Drone/visual_pose_watchdog.py b/Drone/visual_pose_watchdog.py index 663f028..1978071 100644 --- a/Drone/visual_pose_watchdog.py +++ b/Drone/visual_pose_watchdog.py @@ -1,23 +1,27 @@ import rospy import sys import time +import math import logging +import threading import ConfigParser from clever.srv import SetAttitude from sensor_msgs.msg import Range -from mavros_msgs.msg import State +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 config = ConfigParser.ConfigParser() config.read("client_config.ini") visual_pose_timeout = config.getfloat('VISUAL_POSE_WATCHDOG', 'timeout') +pos_delta_max = config.getfloat('VISUAL_POSE_WATCHDOG', 'pos_delta_max') 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') +timeout_to_disarm = config.getfloat('VISUAL_POSE_WATCHDOG', 'timeout_to_disarm') logging.basicConfig( # TODO all prints as logs level=logging.DEBUG, # INFO @@ -33,7 +37,7 @@ formatter = logging.Formatter("%(asctime)s [%(name)-7.7s] [%(threadName)-12.12s] handler.setFormatter(formatter) logger = logging.getLogger(__name__) -logger.setLevel(logging.INFO) +logger.setLevel(logging.DEBUG) logger.addHandler(handler) set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) @@ -46,19 +50,73 @@ mode = '' laser_range = 10 emergency = False +local_pose = None +setpoint_raw = None +setpoint_position = None +setpoint_pose = 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('timeout = {} | timeout_action = {}'.format(visual_pose_timeout, timeout_action)) -logger.info('timeout_to_disarm_after_watchdog_action = {}'.format(timeout_to_disarm_after_watchdog_action)) +logger.info('timeout_to_disarm = {}'.format(timeout_to_disarm)) if timeout_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 @@ -68,72 +126,130 @@ 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, 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: + global visual_pose_last_timestamp, armed, mode, timeout_action, laser_range, emergency, local_pose, setpoint_pose, offboard_start_time, emergency_land_called + pos_delta = get_pos_delta(local_pose, setpoint_pose) + pos_dt = get_time_delta(local_pose, setpoint_pose) + logger.debug("armed: {} | mode: {} | viz_dt: {:.2f} | pos_delta: {:.2f} | pos_dt: {:.2f} | action: {} | range: {:.2f}".format( + armed, mode, abs(time.time() - visual_pose_last_timestamp), pos_delta, pos_dt, timeout_action, laser_range)) + if mode == 'OFFBOARD': + if offboard_start_time is None: + offboard_start_time = time.time() 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') + visual_pose_dt = abs(time.time() - visual_pose_last_timestamp) + if visual_pose_dt > visual_pose_timeout or pos_delta > pos_delta_max: action_timestamp = time.time() - while armed: - if time.time() - action_timestamp > timeout_to_disarm_after_watchdog_action: + 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) + 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 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 == '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 + rate.sleep() + elif timeout_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 + elif 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: + 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: 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(): @@ -141,4 +257,3 @@ while not rospy.is_shutdown(): emergency_msg.data = emergency emergency_pub.publish(emergency_msg) rate.sleep() -