From c114197761712060eac2274e1020b760e27d161c Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Mon, 17 Feb 2020 16:41:07 +0000 Subject: [PATCH] Client: Rework position watchdog parameters --- Drone/config/spec/configspec_client.ini | 16 ++++++----- Drone/visual_pose_watchdog.py | 35 +++++++++++++------------ 2 files changed, 28 insertions(+), 23 deletions(-) diff --git a/Drone/config/spec/configspec_client.ini b/Drone/config/spec/configspec_client.ini index 0a6f063..8b595ab 100644 --- a/Drone/config/spec/configspec_client.ini +++ b/Drone/config/spec/configspec_client.ini @@ -1,5 +1,5 @@ -config_name = string(default='Copter config') -config_version = float(default=0.0) +config_name = string(default='client') +config_version = float(default=1.0) [SERVER] port = integer(default=25000, min=1) @@ -15,16 +15,20 @@ transmit = boolean(default=True) frequency = float(default=1.0, min=0) log_resources = boolean(default=True) -[VISUAL POSE WATCHDOG] -timeout = float(default=1.0, min=0) -pos_delta_max = float(default=3.0, min=0) +[POSITION WATCHDOG] +enabled = boolean(default=True) # Available options: emergency_land, land, disarm action = string(default=emergency_land) +# Timeout for the last vision pose in /mavros/vision_pose/pose +vision_pose_timeout = float(default=1.0, min=0) # Set 0 to disable this check +# Max delta between current position and setpoint +position_delta_max = float(default=3.0, min=0) # Set 0 to disable this check +# Time to disarm after action is triggered +disarm_timeout = float(default=10.0, min=0) [EMERGENCY LAND] thrust = float(default=0.45, min=0, max=1) decrease_thrust_after = float(default=5.0, min=0) -disarm_timeout = float(default=10.0, min=0) [COPTER] frame_id = string(default=map) diff --git a/Drone/visual_pose_watchdog.py b/Drone/visual_pose_watchdog.py index db206a0..ebb9336 100644 --- a/Drone/visual_pose_watchdog.py +++ b/Drone/visual_pose_watchdog.py @@ -23,12 +23,13 @@ from config import ConfigManager config = ConfigManager() config.load_config_and_spec("config/client.ini") -visual_pose_timeout = config.visual_pose_watchdog_timeout -pos_delta_max = config.visual_pose_watchdog_pos_delta_max -timeout_action = config.visual_pose_watchdog_action +watchdog_is_enabled = config.position_watchdog_enabled +visual_pose_timeout = config.position_watchdog_vision_pose_timeout +pos_delta_max = config.position_watchdog_position_delta_max +watchdog_action = config.position_watchdog_action +timeout_to_disarm = config.position_watchdog_disarm_timeout emergency_land_thrust = config.emergency_land_thrust emergency_land_decrease_thrust_after = config.emergency_land_decrease_thrust_after -timeout_to_disarm = config.emergency_land_disarm_timeout logging.basicConfig( # TODO all prints as logs level=logging.DEBUG, # INFO @@ -69,9 +70,9 @@ 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('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 timeout_action == 'emergency_land': +if watchdog_action == 'emergency_land': logger.info('emergency_land_thrust: {}'.format(emergency_land_thrust)) rate = rospy.Rate(10) @@ -174,21 +175,21 @@ def emergency_land_service(request): return responce def watchdog_callback(event): - global visual_pose_last_timestamp, armed, mode, timeout_action, laser_range, emergency, local_pose, setpoint_pose, offboard_start_time, emergency_land_called + global visual_pose_last_timestamp, armed, mode, watchdog_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': + logger.debug("armed: {} | mode: {} | viz_dt: {:.2f} | pos_delta: {:.2f} | pos_dt: {:.2f} | range: {:.2f} | watchdog_action: {}".format( + armed, mode, abs(time.time() - visual_pose_last_timestamp), pos_delta, pos_dt, laser_range, watchdog_action)) + if mode == 'OFFBOARD' and watchdog_is_enabled: if offboard_start_time is None: offboard_start_time = time.time() - if armed: + if armed and time.time() - offboard_start_time > visual_pose_timeout: visual_pose_dt = abs(time.time() - visual_pose_last_timestamp) - if visual_pose_dt > visual_pose_timeout or pos_delta > pos_delta_max: + 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() - if timeout_action in ['land', 'emergency_land', 'disarm']: + if watchdog_action in ['land', 'emergency_land', 'disarm']: emergency = True - if timeout_action == 'land': + if watchdog_action == 'land': logger.info('Visual pose data is too old, copter is armed, landing...') while mode != "AUTO.LAND": try: @@ -207,7 +208,7 @@ def watchdog_callback(event): except rospy.ServiceException as e: logger.info(e) rate.sleep() - elif timeout_action == 'disarm': + elif watchdog_action == 'disarm': logger.info('Visual pose data is too old, copter is armed, disarming...') while armed: try: @@ -215,7 +216,7 @@ def watchdog_callback(event): except rospy.ServiceException as e: logger.info(e) rate.sleep() - elif timeout_action == 'emergency_land': + 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: @@ -238,7 +239,7 @@ def watchdog_callback(event): logger.info(e) else: offboard_start_time = None - if abs(time.time() - visual_pose_last_timestamp) > visual_pose_timeout: + 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)