From aed0080249b60ffbc6547cd0c46e4d9815cf7490 Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Wed, 1 Dec 2021 18:19:43 +0100 Subject: [PATCH 1/2] Fix backwards cancel bug and add test for it --- src/path_tracking_pid_local_planner.cpp | 6 +- test/test_path_tracking_pid.test | 1 + test/test_path_tracking_pid_bw_turn_cancel.py | 105 ++++++++++++++++++ 3 files changed, 110 insertions(+), 2 deletions(-) create mode 100755 test/test_path_tracking_pid_bw_turn_cancel.py diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 00ee308e..f01d1806 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -252,8 +252,10 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_ if (cancel_requested_) { path_tracking_pid::PidConfig config = pid_controller_.getConfig(); - config.target_x_vel = 0.0; - config.target_end_x_vel = 0.0; + // Copysign here, such that when cancelling while driving backwards, we decelerate to -0.0 and hence + // the sign propagates correctly + config.target_x_vel = std::copysign(0.0, config.target_x_vel); + config.target_end_x_vel = std::copysign(0.0, config.target_x_vel); boost::recursive_mutex::scoped_lock lock(config_mutex_); // When updating from own server no callback is called. Thus controller is updated first and then server is notified pid_controller_.configure(config); diff --git a/test/test_path_tracking_pid.test b/test/test_path_tracking_pid.test index bfbc5410..b2a0e370 100644 --- a/test/test_path_tracking_pid.test +++ b/test/test_path_tracking_pid.test @@ -63,5 +63,6 @@ + diff --git a/test/test_path_tracking_pid_bw_turn_cancel.py b/test/test_path_tracking_pid_bw_turn_cancel.py new file mode 100755 index 00000000..1cdd5136 --- /dev/null +++ b/test/test_path_tracking_pid_bw_turn_cancel.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python +import unittest +from math import cos, hypot, pi, sin + +import rospy +import rostest +from actionlib import GoalStatus as GS +from actionlib import SimpleActionClient +from dynamic_reconfigure.client import Client as ReconfigureClient +from geometry_msgs.msg import Point, Pose +from math import atan2 +from mbf_msgs.msg import ExePathAction, ExePathGoal +from nav_msgs.msg import Odometry +from numpy import linspace +from visualization_msgs.msg import Marker + +from paths import create_path + + +def points_on_circle(angles, radius=1, center=(0, 0)): + x = [] + y = [] + yaw = [] + for angle in angles: + x.append(center[0] + (cos(angle) * radius)) + y.append(center[1] + (sin(angle) * radius)) + yaw.append(angle + pi*0.5) + return (x, y, yaw) + + +class TestPathTrackingPID(unittest.TestCase): + def setUp(self): + self.cur_odom = Odometry() + + def vis_cb(self, msg): + if msg.ns == 'control point': + self.carrot = msg.points[0] + elif msg.ns == 'plan point': + self.pos_on_plan = msg.points[0] + + # Only start checking when both markers are received + if self.carrot is None or self.pos_on_plan is None: + return + + angle = atan2(self.carrot.y - self.pos_on_plan.y, + self.carrot.x - self.pos_on_plan.x) + # Check if marker direction doesn't flip, if it did, hold this boolean + if not self.carrot_dir_flipped and self.marker_angle is not None: + angle_diff = angle - self.marker_angle + # 'Wrap' + if angle_diff > pi: + angle_diff -= 2*pi + elif angle_diff < -pi: + angle_diff += 2*pi + # Check if angle flipped + self.carrot_dir_flipped = abs(angle_diff) > 0.8*pi + self.marker_angle = angle + + def test_exepath_action(self): + + # Setup action client to ask question + client = SimpleActionClient("move_base_flex/exe_path", ExePathAction) + self.assertTrue( + client.wait_for_server(timeout=rospy.Duration(10)), + msg="No actionclient for recipe_control found", + ) + + self.carrot = None + self.pos_on_plan = None + self.marker_angle = None + self.carrot_dir_flipped = False + # Subscribe to visualization-markers to track control direction + self.vis_sub = rospy.Subscriber("move_base_flex/PathTrackingPID/visualization_marker", Marker, self.vis_cb) + + (x_circ, y_circ, yaw_circ) = points_on_circle(linspace(-0.5*pi, -0.75*pi), radius=3.0, center=(0.0, 3.0)) + path = create_path(x_circ, y_circ, yaw_circ) + + speed = -0.6 + endspeed = 0 + + # Start goal and cancel in between + reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5) + reconfigure.update_configuration({"target_x_vel": speed}) + reconfigure.update_configuration({"target_end_x_vel": endspeed}) + reconfigure.update_configuration({"target_x_acc": 0.5}) + reconfigure.update_configuration({"target_x_decc": 0.5}) + # We require debug enabled here to retrieve the markers! + reconfigure.update_configuration({"controller_debug_enabled": True}) + rospy.logwarn("Starting path!") + client.send_goal(ExePathGoal(path=path)) + + rospy.sleep(2.0) + rospy.logwarn("Preempting path!") + client.cancel_goal() + rospy.logwarn("Wait for result") + preempt_in_time = client.wait_for_result(timeout=rospy.Duration(10)) + rospy.logwarn("Got result") + self.assertTrue(preempt_in_time, msg="Action call didn't preempt in time") + + self.assertFalse(self.carrot_dir_flipped, msg="Guiding direction flipped while stopping!") + + +if __name__ == "__main__": + rospy.init_node("rostest_path_tracking_pid_bw_turn_cancel", anonymous=False) + rostest.rosrun("forth", "rostest_path_tracking_pid_bw_turn_cancel", TestPathTrackingPID) From ada695d21efdc309c923929ad9d9ae4b0cff463d Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Wed, 1 Dec 2021 19:17:49 +0100 Subject: [PATCH 2/2] Fix test!? --- test/test_path_tracking_pid_accel.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/test_path_tracking_pid_accel.py b/test/test_path_tracking_pid_accel.py index 8f3e4067..cb454f72 100755 --- a/test/test_path_tracking_pid_accel.py +++ b/test/test_path_tracking_pid_accel.py @@ -56,7 +56,7 @@ def test_exepath_action(self): self.assertTrue(abs(self.cur_odom.twist.twist.linear.x) < 0.1, msg="Violated deceleration on preempt") preempt_in_time = client.wait_for_result(timeout=rospy.Duration(10)) self.assertTrue(preempt_in_time, msg="Action call didn't preempt in time") - self.assertEqual(client.get_state(), GS.ABORTED, msg="Action didn't preempt on request") + self.assertEqual(client.get_state(), GS.PREEMPTED, msg="Action didn't preempt on request: {}".format(client.get_state())) # Resume action self.reconfigure()