feat(nav2_rotation_shim_controller): add use_path_orientations (#5034)

* feat(nav2_rotation_shim_controller): add use_path_orientations

Signed-off-by: Kemal Bektas <[email protected]>

* Update nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Kemal Bektas <[email protected]>
Signed-off-by: Steve Macenski <[email protected]>
Co-authored-by: Kemal Bektas <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>

Author: 3 people

nav2_rotation_shim_controller/README.md

@@ -31,13 +31,14 @@ See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/
 
 | Parameter | Description | 
 |-----|----|
-| `angular_dist_threshold` | Maximum angular distance, in radians, away from the path heading to trigger rotation until within. | 
-| `forward_sampling_distance` | Forward distance, in meters, along path to select a sampling point to use to approximate path heading | 
-| `rotate_to_heading_angular_vel` | Angular rotational velocity, in rad/s, to rotate to the path heading | 
-| `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading | 
-| `max_angular_accel` | Maximum angular acceleration for rotation to heading | 
-| `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. | 
-| `rotate_to_goal_heading` | If true, the rotationShimController will take back control of the robot when in XY tolerance of the goal and start rotating to the goal heading | 
+| `angular_dist_threshold` | Maximum angular distance, in radians, away from the path heading to trigger rotation until within. |
+| `forward_sampling_distance` | Forward distance, in meters, along path to select a sampling point to use to approximate path heading |
+| `rotate_to_heading_angular_vel` | Angular rotational velocity, in rad/s, to rotate to the path heading |
+| `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading |
+| `max_angular_accel` | Maximum angular acceleration for rotation to heading |
+| `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. |
+| `rotate_to_goal_heading` | If true, the rotationShimController will take back control of the robot when in XY tolerance of the goal and start rotating to the goal heading |
+| `use_path_orientations` | If true, the controller will use the orientations of the path points to compute the heading of the path instead of computing the heading from the path points. If true, the controller will use the orientations of the path points to compute the heading of the path instead of computing the heading from the path points. Use for for feasible planners like the Smac Planner which generate feasible paths with orientations for forward and reverse motion. |
 
 Example fully-described XML with default parameter values:
 
@@ -70,6 +71,7 @@ controller_server:
       max_angular_accel: 3.2
       simulate_ahead_time: 1.0
       rotate_to_goal_heading: false
+      use_path_orientations: false
 
       # DWB parameters
       ...

nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp

@@ -190,6 +190,7 @@ class RotationShimController : public nav2_core::Controller
   double control_duration_, simulate_ahead_time_;
   bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_;
   bool closed_loop_;
+  bool use_path_orientations_;
   double last_angular_vel_ = std::numeric_limits<double>::max();
 
   // Dynamic parameters handler

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

@@ -70,6 +70,8 @@ void RotationShimController::configure(
     node, plugin_name_ + ".rotate_to_heading_once", rclcpp::ParameterValue(false));
   nav2_util::declare_parameter_if_not_declared(
     node, plugin_name_ + ".closed_loop", rclcpp::ParameterValue(true));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".use_path_orientations", rclcpp::ParameterValue(false));
 
   node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_);
   node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_);
@@ -87,6 +89,7 @@ void RotationShimController::configure(
   node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_);
   node->get_parameter(plugin_name_ + ".rotate_to_heading_once", rotate_to_heading_once_);
   node->get_parameter(plugin_name_ + ".closed_loop", closed_loop_);
+  node->get_parameter(plugin_name_ + ".use_path_orientations", use_path_orientations_);
 
   try {
     primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
@@ -202,10 +205,17 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands
 
     std::lock_guard<std::mutex> lock_reinit(mutex_);
     try {
-      geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(getSampledPathPt());
-
-      double angular_distance_to_heading =
-        std::atan2(sampled_pt_base.position.y, sampled_pt_base.position.x);
+      auto sampled_pt = getSampledPathPt();
+      double angular_distance_to_heading;
+      if (use_path_orientations_) {
+        angular_distance_to_heading = angles::shortest_angular_distance(
+          tf2::getYaw(pose.pose.orientation),
+          tf2::getYaw(sampled_pt.pose.orientation));
+      } else {
+        geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(sampled_pt);
+        angular_distance_to_heading = std::atan2(sampled_pt_base.position.y,
+          sampled_pt_base.position.x);
+      }
 
       double angular_thresh =
         in_rotation_ ? angular_disengage_threshold_ : angular_dist_threshold_;
@@ -418,6 +428,8 @@ RotationShimController::dynamicParametersCallback(std::vector<rclcpp::Parameter>
         rotate_to_heading_once_ = parameter.as_bool();
       } else if (name == plugin_name_ + ".closed_loop") {
         closed_loop_ = parameter.as_bool();
+      } else if (name == plugin_name_ + ".use_path_orientations") {
+        use_path_orientations_ = parameter.as_bool();
       }
     }
   }

nav2_rotation_shim_controller/test/test_shim_controller.cpp

@@ -626,7 +626,8 @@ TEST(RotationShimControllerTest, testDynamicParameter)
       rclcpp::Parameter("test.primary_controller", std::string("HI")),
       rclcpp::Parameter("test.rotate_to_goal_heading", true),
       rclcpp::Parameter("test.rotate_to_heading_once", true),
-      rclcpp::Parameter("test.closed_loop", false)});
+      rclcpp::Parameter("test.closed_loop", false),
+      rclcpp::Parameter("test.use_path_orientations", true)});
 
   rclcpp::spin_until_future_complete(
     node->get_node_base_interface(),
@@ -640,4 +641,5 @@ TEST(RotationShimControllerTest, testDynamicParameter)
   EXPECT_EQ(node->get_parameter("test.rotate_to_goal_heading").as_bool(), true);
   EXPECT_EQ(node->get_parameter("test.rotate_to_heading_once").as_bool(), true);
   EXPECT_EQ(node->get_parameter("test.closed_loop").as_bool(), false);
+  EXPECT_EQ(node->get_parameter("test.use_path_orientations").as_bool(), true);
 }