From 17e80d655c2ec4f73a6ee949e69dfa04f09c9a36 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Fri, 20 Dec 2024 15:49:24 -0500 Subject: [PATCH] fix bug in use of v_angular_min_in_place (#4813) Signed-off-by: Michael Ferguson --- .../src/graceful_controller.cpp | 2 +- .../test/test_graceful_controller.cpp | 13 +++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/nav2_graceful_controller/src/graceful_controller.cpp b/nav2_graceful_controller/src/graceful_controller.cpp index fea723c009..d6d74cd568 100644 --- a/nav2_graceful_controller/src/graceful_controller.cpp +++ b/nav2_graceful_controller/src/graceful_controller.cpp @@ -367,7 +367,7 @@ geometry_msgs::msg::Twist GracefulController::rotateToTarget(double angle_to_tar geometry_msgs::msg::Twist vel; vel.linear.x = 0.0; vel.angular.z = params_->rotation_scaling_factor * angle_to_target * params_->v_angular_max; - vel.angular.z = std::copysign(1.0, vel.angular.z) * std::min(abs(vel.angular.z), + vel.angular.z = std::copysign(1.0, vel.angular.z) * std::max(abs(vel.angular.z), params_->v_angular_min_in_place); return vel; } diff --git a/nav2_graceful_controller/test/test_graceful_controller.cpp b/nav2_graceful_controller/test/test_graceful_controller.cpp index 2fd98675b8..f00fda6ab5 100644 --- a/nav2_graceful_controller/test/test_graceful_controller.cpp +++ b/nav2_graceful_controller/test/test_graceful_controller.cpp @@ -461,6 +461,19 @@ TEST(GracefulControllerTest, rotateToTarget) { // Check results: it must be a negative rotation EXPECT_EQ(cmd_vel.linear.x, 0.0); EXPECT_EQ(cmd_vel.angular.z, -0.25); + + // Set very high v_angular_min_in_place velocity + results = params->set_parameters_atomically( + {rclcpp::Parameter("test.v_angular_min_in_place", 1.0)}); + rclcpp::spin_until_future_complete(node->get_node_base_interface(), results); + + // Set a new angle to target + angle_to_target = 0.5; + cmd_vel = controller->rotateToTarget(angle_to_target); + + // Check results: positive velocity, at least as high as min_in_place + EXPECT_EQ(cmd_vel.linear.x, 0.0); + EXPECT_EQ(cmd_vel.angular.z, 1.0); } TEST(GracefulControllerTest, setSpeedLimit) {