feat(nav2_rotation_shim_controller): add use_path_orientations

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

Author: Kemal Bektas

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. |
 
 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();
+      auto angular_distance_to_heading = [&](){
+          if (use_path_orientations_) {
+            return 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);
+            return 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)