From 0256deef66b70940e627d634f3b42dc55aa3b07c Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 5 Mar 2018 21:42:48 +0300 Subject: [PATCH] Experimental simple_offboard/navigate_after_armed parameter If this parameter is on, /navigate service waits until copter is armed, before starts to move the position setpoint --- clever/src/simple_offboard.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/clever/src/simple_offboard.py b/clever/src/simple_offboard.py index a84b9367..edbf815b 100755 --- a/clever/src/simple_offboard.py +++ b/clever/src/simple_offboard.py @@ -79,6 +79,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)) +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) @@ -162,6 +163,9 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True): current_nav_start = pose.pose.position current_nav_start_stamp = stamp + if NAVIGATE_AFTER_ARMED and not state.armed: + current_nav_start_stamp = stamp + setpoint = get_navigate_setpoint(stamp, current_nav_start, current_nav_finish.pose.position, current_nav_start_stamp, req.speed)