diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index d3807eeb952..abaf02c9393 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/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_; diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 5b2bf306df0..bb77879c7f7 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/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 } 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(); } } } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 1160a5a98af..1d63a77b47e 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/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(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto listener = std::make_shared(*tf, node, true); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + auto tf_broadcaster = std::make_shared(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(); + 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("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); }