feat(nav2_rotation_shim_controller): add use_path_orientations

nav2_rotation_shim_controller/README.md

@@ -38,6 +38,7 @@ See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/
 | `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

@@ -186,6 +186,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

@@ -75,6 +75,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_);
@@ -92,6 +94,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);
@@ -207,10 +210,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_;
@@ -423,6 +433,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

@@ -621,7 +621,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(),
@@ -635,6 +636,7 @@ 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);
 }
 
 int main(int argc, char **argv)