Shim Controller: Added parameter rotate_to_heading_once to align a robot only at the very beginning

nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp

@@ -148,6 +148,13 @@ class RotationShimController : public nav2_core::Controller
     const double & angular_distance_to_heading,
     const geometry_msgs::msg::PoseStamped & pose);
 
+  /**
+   * @brief Checks if the goal has changed based on the given path.
+   * @param path The path to compare with the current goal.
+   * @return True if the goal has changed, false otherwise.
+   */
+  bool isGoalChanged(const nav_msgs::msg::Path & path);
+
   /**
    * @brief Callback executed when a parameter change is detected
    * @param event ParameterEvent message
@@ -171,7 +178,7 @@ class RotationShimController : public nav2_core::Controller
   double forward_sampling_distance_, angular_dist_threshold_, angular_disengage_threshold_;
   double rotate_to_heading_angular_vel_, max_angular_accel_;
   double control_duration_, simulate_ahead_time_;
-  bool rotate_to_goal_heading_, in_rotation_;
+  bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_;
 
   // Dynamic parameters handler
   std::mutex mutex_;

nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

@@ -71,6 +71,8 @@ void RotationShimController::configure(
     node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING);
   nav2_util::declare_parameter_if_not_declared(
     node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false));
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".rotate_to_heading_once", 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_);
@@ -86,6 +88,7 @@ void RotationShimController::configure(
   control_duration_ = 1.0 / control_frequency;
 
   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_);
 
   try {
     primary_controller_ = lp_loader_.createUniqueInstance(primary_controller);
@@ -340,9 +343,20 @@ void RotationShimController::isCollisionFree(
   }
 }
 
+bool RotationShimController::isGoalChanged(const nav_msgs::msg::Path & path)
+{
+  // Return true if rotating or if the current path is empty
+  if (in_rotation_ || current_path_.poses.empty()) {
+    return true;
+  }
+
+  // Check if the last pose of the current and new paths differ
+  return current_path_.poses.back().pose != path.poses.back().pose;
+}
+
 void RotationShimController::setPlan(const nav_msgs::msg::Path & path)
 {
-  path_updated_ = true;
+  path_updated_ = rotate_to_heading_once_ ? isGoalChanged(path) : true;
   current_path_ = path;
   primary_controller_->setPlan(path);
 }
@@ -377,6 +391,8 @@ RotationShimController::dynamicParametersCallback(std::vector<rclcpp::Parameter>
     } else if (type == ParameterType::PARAMETER_BOOL) {
       if (name == plugin_name_ + ".rotate_to_goal_heading") {
         rotate_to_goal_heading_ = parameter.as_bool();
+      } else if (name == plugin_name_ + ".rotate_to_heading_once") {
+        rotate_to_heading_once_ = parameter.as_bool();
       }
     }
   }

nav2_rotation_shim_controller/test/test_shim_controller.cpp

@@ -62,6 +62,11 @@ class RotationShimShim : public nav2_rotation_shim_controller::RotationShimContr
     return getSampledPathPt();
   }
 
+  bool isGoalChangedWrapper(const nav_msgs::msg::Path & path)
+  {
+    return isGoalChanged(path);
+  }
+
   geometry_msgs::msg::Pose transformPoseToBaseFrameWrapper(geometry_msgs::msg::PoseStamped pt)
   {
     return transformPoseToBaseFrame(pt);
@@ -382,6 +387,57 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) {
   EXPECT_EQ(cmd_vel.twist.angular.z, 1.8);
 }
 
+TEST(RotationShimControllerTest, isGoalChangedTest)
+{
+  auto ctrl = std::make_shared<RotationShimShim>();
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
+  std::string name = "PathFollower";
+  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+  auto listener = std::make_shared<tf2_ros::TransformListener>(*tf, node, true);
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
+  rclcpp_lifecycle::State state;
+  costmap->on_configure(state);
+  auto tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(node);
+
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.frame_id = "base_link";
+  transform.child_frame_id = "odom";
+  transform.transform.rotation.x = 0.0;
+  transform.transform.rotation.y = 0.0;
+  transform.transform.rotation.z = 0.0;
+  transform.transform.rotation.w = 1.0;
+  tf_broadcaster->sendTransform(transform);
+
+  // set a valid primary controller so we can do lifecycle
+  node->declare_parameter(
+    "PathFollower.primary_controller",
+    std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"));
+  node->declare_parameter(
+    "PathFollower.rotate_to_heading_once",
+    true);
+
+  auto controller = std::make_shared<RotationShimShim>();
+  controller->configure(node, name, tf, costmap);
+  controller->activate();
+
+  nav_msgs::msg::Path path;
+  path.header.frame_id = "base_link";
+  path.poses.resize(2);
+  path.poses.back().pose.position.x = 2.0;
+  path.poses.back().pose.position.y = 2.0;
+
+  // Test: Current path is empty, should return true
+  EXPECT_EQ(controller->isGoalChangedWrapper(path), true);
+
+  // Test: Last pose of the current path is the same, should return false
+  controller->setPlan(path);
+  EXPECT_EQ(controller->isGoalChangedWrapper(path), false);
+
+  // Test: Last pose of the current path differs, should return true
+  path.poses.back().pose.position.x = 3.0;
+  EXPECT_EQ(controller->isGoalChangedWrapper(path), true);
+}
+
 TEST(RotationShimControllerTest, testDynamicParameter)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
@@ -412,7 +468,8 @@ TEST(RotationShimControllerTest, testDynamicParameter)
       rclcpp::Parameter("test.max_angular_accel", 7.0),
       rclcpp::Parameter("test.simulate_ahead_time", 7.0),
       rclcpp::Parameter("test.primary_controller", std::string("HI")),
-      rclcpp::Parameter("test.rotate_to_goal_heading", true)});
+      rclcpp::Parameter("test.rotate_to_goal_heading", true),
+      rclcpp::Parameter("test.rotate_to_heading_once", true)});
 
   rclcpp::spin_until_future_complete(
     node->get_node_base_interface(),
@@ -424,4 +481,5 @@ TEST(RotationShimControllerTest, testDynamicParameter)
   EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 7.0);
   EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0);
   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);
 }