From bed317a05b5923e3b001cd5cba595216f3d7e2d2 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 19 Mar 2018 23:06:40 +0300 Subject: [PATCH] simple_offboard: warn no local position in /navigate --- clever/src/simple_offboard.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/clever/src/simple_offboard.py b/clever/src/simple_offboard.py index f49532b5..f926aa01 100755 --- a/clever/src/simple_offboard.py +++ b/clever/src/simple_offboard.py @@ -81,6 +81,7 @@ AUTO_OFFBOARD = rospy.get_param('~auto_offboard', True) AUTO_ARM = AUTO_OFFBOARD and rospy.get_param('~auto_arm', True) OFFBOARD_TIMEOUT = rospy.Duration(rospy.get_param('~offboard_timeout', 3)) ARM_TIMEOUT = rospy.Duration(rospy.get_param('~arm_timeout', 5)) +LOCAL_POSITION_TIMEOUT = rospy.Duration(rospy.get_param('~local_position_timeout', 0.5)) NAVIGATE_AFTER_ARMED = rospy.Duration(rospy.get_param('~navigate_after_armed', False)) TRANSFORM_TIMEOUT = rospy.Duration(rospy.get_param('~transform_timeout', 3)) SETPOINT_RATE = rospy.get_param('~setpoint_rate', 30) @@ -263,6 +264,11 @@ def handle(req): rospy.logwarn('Navigate speed must be greater than zero, %s passed') return {'message': 'Navigate speed must be greater than zero, %s passed' % req.speed} + if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)) and \ + (pose is None or rospy.get_rostime() - pose.header.stamp > LOCAL_POSITION_TIMEOUT): + rospy.logwarn('No local position') + return {'message': 'No local position'} + if getattr(req, 'yaw_rate', 0) != 0 and not math.isnan(getattr(req, 'yaw')): rospy.logwarn('Yaw value should be NaN for setting yaw rate') return {'message': 'Yaw value should be NaN for setting yaw rate'}