fix bug in use of v_angular_min_in_place (#4813)

Signed-off-by: Michael Ferguson <[email protected]>
ros-navigation · Dec 20, 2024 · 17e80d6 · 17e80d6
1 parent 7b73a38
commit 17e80d6
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 1 deletion.
diff --git 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
@@ -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) {