first part of addressing review comments

Signed-off-by: Michael Ferguson <[email protected]>

nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp

@@ -169,7 +169,9 @@ class GracefulController : public nav2_core::Controller
   Parameters * params_;
   double goal_dist_tolerance_;
   bool goal_reached_;
-  bool has_new_path_;
+
+  // True from the time a new path arrives until we have completed an initial rotation
+  bool do_initial_rotation_;
 
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> transformed_plan_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> local_plan_pub_;

nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp

@@ -47,6 +47,7 @@ struct Parameters
   double v_angular_max_initial;
   double v_angular_min_in_place;
   double slowdown_radius;
+  bool initial_rotation;
   double initial_rotation_tolerance;
   bool prefer_final_rotation;
   double rotation_scaling_factor;

nav2_graceful_controller/src/graceful_controller.cpp

@@ -155,7 +155,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
       logger_, "Could not transform %s to %s: %s",
       costmap_ros_->getBaseFrameID().c_str(), costmap_ros_->getGlobalFrameID().c_str(),
       ex.what());
-    throw nav2_core::NoValidControl("Could not transform");
+    throw ex;
   }
 
   // Compute distance to goal as the path's integrated distance to account for path curvatures
@@ -257,7 +257,7 @@ void GracefulController::setPlan(const nav_msgs::msg::Path & path)
 {
   path_handler_->setPlan(path);
   goal_reached_ = false;
-  has_new_path_ = true;
+  do_initial_rotation_ = true;
 }
 
 void GracefulController::setSpeedLimit(
@@ -299,12 +299,12 @@ bool GracefulController::simulateTrajectory(
   next_pose.pose.orientation.w = 1.0;
 
   // Should we simulate rotation initially?
-  bool sim_initial_rotation = has_new_path_ && (params_->initial_rotation_tolerance > 0.0);
+  bool sim_initial_rotation = do_initial_rotation_ && params_->initial_rotation;
   double angle_to_target =
     std::atan2(motion_target.pose.position.y, motion_target.pose.position.x);
   if (fabs(angle_to_target) < params_->initial_rotation_tolerance) {
     sim_initial_rotation = false;
-    has_new_path_ = false;
+    do_initial_rotation_ = false;
   }
 
   double distance = std::numeric_limits<double>::max();
@@ -370,11 +370,8 @@ 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;
-  if (vel.angular.z < 0.0) {
-    vel.angular.z = std::min(vel.angular.z, -params_->v_angular_min_in_place);
-  } else {
-    vel.angular.z = std::max(vel.angular.z, params_->v_angular_min_in_place);
-  }
+  vel.angular.z = std::copysign(1.0, vel.angular.z) * std::min(abs(vel.angular.z),
+      params_->v_angular_min_in_place);
   return vel;
 }
 
@@ -432,6 +429,7 @@ void GracefulController::computeDistanceAlongPath(
 std::vector<geometry_msgs::msg::PoseStamped> GracefulController::addOrientations(
   const std::vector<geometry_msgs::msg::PoseStamped> & path)
 {
+  // NOTE: control loop will handle reversing logic
   std::vector<geometry_msgs::msg::PoseStamped> oriented_path;
   oriented_path.resize(path.size());
   if (path.empty()) {

nav2_graceful_controller/src/parameter_handler.cpp

@@ -58,6 +58,8 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".v_angular_min_in_place", rclcpp::ParameterValue(0.25));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".slowdown_radius", rclcpp::ParameterValue(1.5));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".initial_rotation", rclcpp::ParameterValue(true));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".initial_rotation_tolerance", rclcpp::ParameterValue(0.25));
   declare_parameter_if_not_declared(
@@ -93,14 +95,15 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(
     plugin_name_ + ".v_angular_min_in_place", params_.v_angular_min_in_place);
   node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius);
+  node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation);
   node->get_parameter(
     plugin_name_ + ".initial_rotation_tolerance", params_.initial_rotation_tolerance);
   node->get_parameter(plugin_name_ + ".prefer_final_rotation", params_.prefer_final_rotation);
   node->get_parameter(plugin_name_ + ".rotation_scaling_factor", params_.rotation_scaling_factor);
   node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward);
   node->get_parameter(plugin_name_ + ".add_orientations", params_.add_orientations);
 
-  if ((params_.initial_rotation_tolerance > 0.0) && params_.allow_backward) {
+  if (params_.initial_rotation && params_.allow_backward) {
     RCLCPP_WARN(
       logger_, "Initial rotation and allow backward parameters are both true, "
       "setting allow backward to false.");
@@ -158,23 +161,25 @@ ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
       } else if (name == plugin_name_ + ".slowdown_radius") {
         params_.slowdown_radius = parameter.as_double();
       } else if (name == plugin_name_ + ".initial_rotation_tolerance") {
-        if ((parameter.as_double() > 0.0) && params_.allow_backward) {
-          RCLCPP_WARN(
-            logger_, "Initial rotation and allow backward cannot be combined, "
-            "rejecting parameter change.");
-          continue;
-        }
         params_.initial_rotation_tolerance = parameter.as_double();
       } else if (name == plugin_name_ + ".rotation_scaling_factor") {
         params_.rotation_scaling_factor = parameter.as_double();
       }
     } else if (type == ParameterType::PARAMETER_BOOL) {
-      if (name == plugin_name_ + ".prefer_final_rotation") {
+      if (name == plugin_name_ + ".initial_rotation") {
+        if (parameter.as_bool() && params_.allow_backward) {
+          RCLCPP_WARN(
+            logger_, "Initial rotation and allow backward parameters are both true, "
+            "rejecting parameter change.");
+          continue;
+        }
+        params_.initial_rotation = parameter.as_bool();
+      } else if (name == plugin_name_ + ".prefer_final_rotation") {
         params_.prefer_final_rotation = parameter.as_bool();
       } else if (name == plugin_name_ + ".allow_backward") {
-        if ((params_.initial_rotation_tolerance > 0.0) && parameter.as_bool()) {
+        if (params_.initial_rotation && parameter.as_bool()) {
           RCLCPP_WARN(
-            logger_, "Initial rotation and allow backward cannot be combined, "
+            logger_, "Initial rotation and allow backward parameters are both true, "
             "rejecting parameter change.");
           continue;
         }

nav2_graceful_controller/test/test_graceful_controller.cpp

@@ -53,7 +53,7 @@ class GMControllerFixture : public nav2_graceful_controller::GracefulController
   GMControllerFixture()
   : nav2_graceful_controller::GracefulController() {}
 
-  double getInitialRotationTolerance() {return params_->initial_rotation_tolerance;}
+  bool getInitialRotation() {return params_->initial_rotation;}
 
   bool getAllowBackward() {return params_->allow_backward;}
 
@@ -251,7 +251,7 @@ TEST(GracefulControllerTest, dynamicParameters) {
 
   // Set initial rotation and allow backward to true so it warns and allow backward is false
   nav2_util::declare_parameter_if_not_declared(
-    node, "test.initial_rotation_tolerance", rclcpp::ParameterValue(0.25));
+    node, "test.initial_rotation", rclcpp::ParameterValue(true));
   nav2_util::declare_parameter_if_not_declared(
     node, "test.allow_backward", rclcpp::ParameterValue(true));
 
@@ -281,11 +281,12 @@ TEST(GracefulControllerTest, dynamicParameters) {
       rclcpp::Parameter("test.v_linear_min", 8.0),
       rclcpp::Parameter("test.v_linear_max", 9.0),
       rclcpp::Parameter("test.v_angular_max", 10.0),
+      rclcpp::Parameter("test.v_angular_min_in_place", 14.0),
       rclcpp::Parameter("test.slowdown_radius", 11.0),
-      rclcpp::Parameter("test.initial_rotation_tolerance", 0.0),
+      rclcpp::Parameter("test.initial_rotation", false),
+      rclcpp::Parameter("test.initial_rotation_tolerance", 12.0),
       rclcpp::Parameter("test.prefer_final_rotation", false),
       rclcpp::Parameter("test.rotation_scaling_factor", 13.0),
-      rclcpp::Parameter("test.v_angular_min_in_place", 14.0),
       rclcpp::Parameter("test.allow_backward", true),
       rclcpp::Parameter("test.add_orientations", true)});
 
@@ -303,43 +304,44 @@ TEST(GracefulControllerTest, dynamicParameters) {
   EXPECT_EQ(node->get_parameter("test.v_linear_min").as_double(), 8.0);
   EXPECT_EQ(node->get_parameter("test.v_linear_max").as_double(), 9.0);
   EXPECT_EQ(node->get_parameter("test.v_angular_max").as_double(), 10.0);
+  EXPECT_EQ(node->get_parameter("test.v_angular_min_in_place").as_double(), 14.0);
   EXPECT_EQ(node->get_parameter("test.slowdown_radius").as_double(), 11.0);
-  EXPECT_EQ(node->get_parameter("test.initial_rotation_tolerance").as_double(), 0.0);
+  EXPECT_EQ(node->get_parameter("test.initial_rotation").as_bool(), false);
+  EXPECT_EQ(node->get_parameter("test.initial_rotation_tolerance").as_double(), 12.0);
   EXPECT_EQ(node->get_parameter("test.prefer_final_rotation").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.rotation_scaling_factor").as_double(), 13.0);
-  EXPECT_EQ(node->get_parameter("test.v_angular_min_in_place").as_double(), 14.0);
   EXPECT_EQ(node->get_parameter("test.allow_backward").as_bool(), true);
   EXPECT_EQ(node->get_parameter("test.add_orientations").as_bool(), true);
 
   // Set initial rotation to true
   results = params->set_parameters_atomically(
-    {rclcpp::Parameter("test.initial_rotation_tolerance", 1.0)});
+    {rclcpp::Parameter("test.initial_rotation", true)});
   rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
-  // Check parameters. Initial rotation should not be set as both cannot be true at the same time
-  EXPECT_EQ(controller->getInitialRotationTolerance(), 0.0);
+  // Check parameters. Initial rotation should be false as both cannot be true at the same time
+  EXPECT_EQ(controller->getInitialRotation(), false);
   EXPECT_EQ(controller->getAllowBackward(), true);
 
   // Set both initial rotation and allow backward to false
   results = params->set_parameters_atomically(
-    {rclcpp::Parameter("test.initial_rotation_tolerance", 0.0),
+    {rclcpp::Parameter("test.initial_rotation", false),
       rclcpp::Parameter("test.allow_backward", false)});
   rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
   // Check parameters.
-  EXPECT_EQ(node->get_parameter("test.initial_rotation_tolerance").as_double(), 0.0);
+  EXPECT_EQ(node->get_parameter("test.initial_rotation").as_bool(), false);
   EXPECT_EQ(node->get_parameter("test.allow_backward").as_bool(), false);
 
   // Set initial rotation to true
   results = params->set_parameters_atomically(
-    {rclcpp::Parameter("test.initial_rotation_tolerance", 1.0)});
+    {rclcpp::Parameter("test.initial_rotation", true)});
   rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
-  EXPECT_EQ(controller->getInitialRotationTolerance(), 1.0);
+  EXPECT_EQ(controller->getInitialRotation(), true);
 
   // Set allow backward to true
   results = params->set_parameters_atomically(
     {rclcpp::Parameter("test.allow_backward", true)});
   rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
   // Check parameters. Now allow backward should be false as both cannot be true at the same time
-  EXPECT_EQ(controller->getInitialRotationTolerance(), 1.0);
+  EXPECT_EQ(controller->getInitialRotation(), true);
   EXPECT_EQ(controller->getAllowBackward(), false);
 }
 
@@ -984,7 +986,7 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) {
 
   // Set initial rotation false and allow backward to true
   nav2_util::declare_parameter_if_not_declared(
-    node, "test.initial_rotation_tolerance", rclcpp::ParameterValue(0.0));
+    node, "test.initial_rotation", rclcpp::ParameterValue(false));
   nav2_util::declare_parameter_if_not_declared(
     node, "test.allow_backward", rclcpp::ParameterValue(true));
 
@@ -1139,7 +1141,7 @@ TEST(GracefulControllerTest, computeVelocityCommandFinal) {
   // Check results: the robot should do a final rotation near the target.
   // So, linear velocity should be zero and angular velocity should be a positive value below 0.5.
   EXPECT_EQ(cmd_vel.twist.linear.x, 0.0);
-  EXPECT_GT(cmd_vel.twist.angular.z, 0.0);
+  EXPECT_GE(cmd_vel.twist.angular.z, 0.0);
   EXPECT_LE(cmd_vel.twist.angular.z, 0.5);
 }