more review comments implemented

nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp

@@ -153,11 +153,9 @@ class GracefulController : public nav2_core::Controller
 
   /**
    * @brief Control law requires proper orientations, not all planners provide them
-   * @param path Path to add orientations into
-   * @returns Path with orientations.
+   * @param path Path to add orientations into, if required
    */
-  std::vector<geometry_msgs::msg::PoseStamped> addOrientations(
-    const std::vector<geometry_msgs::msg::PoseStamped> & path);
+  void validateOrientations(std::vector<geometry_msgs::msg::PoseStamped> & path);
 
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::string plugin_name_;

nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp

@@ -52,7 +52,7 @@ struct Parameters
   bool prefer_final_rotation;
   double rotation_scaling_factor;
   bool allow_backward;
-  bool add_orientations;
+  double in_place_collision_tolerance;
 };
 
 /**

nav2_graceful_controller/src/graceful_controller.cpp

@@ -136,10 +136,8 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
   auto transformed_plan = path_handler_->transformGlobalPlan(
     pose, params_->max_robot_pose_search_dist);
 
-  // Add proper orientations to plan, if desired
-  if (params_->add_orientations) {
-    transformed_plan.poses = addOrientations(transformed_plan.poses);
-  }
+  // Add proper orientations to plan, if needed
+  validateOrientations(transformed_plan.poses);
 
   // Publish plan for visualization
   transformed_plan_pub_->publish(transformed_plan);
@@ -166,10 +164,9 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
     goal_reached_ = true;
     double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation);
     // Check for collisions between our current pose and goal pose
-    size_t num_steps = fabs(angle_to_goal) / 0.1;
+    size_t num_steps = fabs(angle_to_goal) / params_->in_place_collision_tolerance;
     // Need to check at least the end pose
     num_steps = std::max(static_cast<size_t>(1), num_steps);
-    // If we fail to generate an in-place rotation, maybe we need to move along path a bit more
     bool collision_free = true;
     for (size_t i = 1; i <= num_steps; ++i) {
       double step = static_cast<double>(i) / static_cast<double>(num_steps);
@@ -426,32 +423,28 @@ void GracefulController::computeDistanceAlongPath(
   }
 }
 
-std::vector<geometry_msgs::msg::PoseStamped> GracefulController::addOrientations(
-  const std::vector<geometry_msgs::msg::PoseStamped> & path)
+void GracefulController::validateOrientations(
+  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()) {
-    // This really shouldn't happen
-    return oriented_path;
+  // This really shouldn't happen
+  if (path.empty()) {return;}
+
+  // Check if we actually need to add orientations
+  for (size_t i = 0; i < path.size() - 1; ++i) {
+    if (tf2::getYaw(path[i].pose.orientation) != 0.0) {return;}
   }
 
-  // The last pose will already be oriented since it is our goal
-  oriented_path.back() = path.back();
+  RCLCPP_WARN(logger_, "Adding orientations");
 
   // For each pose, point at the next one
-  for (size_t i = 0; i < oriented_path.size() - 1; ++i) {
-    // XY will be unchanged
-    oriented_path[i] = path[i];
+  // NOTE: control loop will handle reversing logic
+  for (size_t i = 0; i < path.size() - 1; ++i) {
     // Get relative yaw angle
     double dx = path[i + 1].pose.position.x - path[i].pose.position.x;
     double dy = path[i + 1].pose.position.y - path[i].pose.position.y;
     double yaw = std::atan2(dy, dx);
-    oriented_path[i].pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
+    path[i].pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
   }
-
-  return oriented_path;
 }
 
 }  // namespace nav2_graceful_controller

nav2_graceful_controller/src/parameter_handler.cpp

@@ -69,7 +69,7 @@ ParameterHandler::ParameterHandler(
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".allow_backward", rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
-    node, plugin_name_ + ".add_orientations", rclcpp::ParameterValue(false));
+    node, plugin_name_ + ".in_place_collision_tolerance", rclcpp::ParameterValue(0.1));
 
   node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance);
   node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead);
@@ -101,7 +101,8 @@ ParameterHandler::ParameterHandler(
   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);
+  node->get_parameter(
+    plugin_name_ + ".in_place_collision_tolerance", params_.in_place_collision_tolerance);
 
   if (params_.initial_rotation && params_.allow_backward) {
     RCLCPP_WARN(
@@ -164,6 +165,8 @@ ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
         params_.initial_rotation_tolerance = parameter.as_double();
       } else if (name == plugin_name_ + ".rotation_scaling_factor") {
         params_.rotation_scaling_factor = parameter.as_double();
+      } else if (name == plugin_name_ + ".in_place_collision_tolerance") {
+        params_.in_place_collision_tolerance = parameter.as_double();
       }
     } else if (type == ParameterType::PARAMETER_BOOL) {
       if (name == plugin_name_ + ".initial_rotation") {
@@ -184,8 +187,6 @@ ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> param
           continue;
         }
         params_.allow_backward = parameter.as_bool();
-      } else if (name == plugin_name_ + ".add_orientations") {
-        params_.add_orientations = parameter.as_bool();
       }
     }
   }

nav2_graceful_controller/test/test_graceful_controller.cpp

@@ -288,7 +288,7 @@ TEST(GracefulControllerTest, dynamicParameters) {
       rclcpp::Parameter("test.prefer_final_rotation", false),
       rclcpp::Parameter("test.rotation_scaling_factor", 13.0),
       rclcpp::Parameter("test.allow_backward", true),
-      rclcpp::Parameter("test.add_orientations", true)});
+      rclcpp::Parameter("test.in_place_collision_tolerance", 15.0)});
 
   // Spin
   rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
@@ -311,7 +311,7 @@ TEST(GracefulControllerTest, dynamicParameters) {
   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.allow_backward").as_bool(), true);
-  EXPECT_EQ(node->get_parameter("test.add_orientations").as_bool(), true);
+  EXPECT_EQ(node->get_parameter("test.in_place_collision_tolerance").as_double(), 15.0);
 
   // Set initial rotation to true
   results = params->set_parameters_atomically(
@@ -830,8 +830,6 @@ TEST(GracefulControllerTest, computeVelocityCommandRotate) {
     node, "test.v_angular_max", rclcpp::ParameterValue(1.0));
   nav2_util::declare_parameter_if_not_declared(
     node, "test.rotation_scaling_factor", rclcpp::ParameterValue(0.5));
-  nav2_util::declare_parameter_if_not_declared(
-    node, "test.add_orientations", rclcpp::ParameterValue(true));
 
   // Create a costmap of 10x10 meters
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
@@ -1058,7 +1056,9 @@ TEST(GracefulControllerTest, computeVelocityCommandRegularBackwards) {
   // Check results: the robot should go straight to the target.
   // So, both linear velocity should be some negative values.
   EXPECT_LT(cmd_vel.twist.linear.x, 0.0);
-  EXPECT_LT(cmd_vel.twist.angular.z, 0.0);
+  // There might be a small bit of noise on angular velocity
+  EXPECT_LT(cmd_vel.twist.angular.z, 0.01);
+  EXPECT_GT(cmd_vel.twist.angular.z, -0.01);
 }
 
 TEST(GracefulControllerTest, computeVelocityCommandFinal) {