From 631160a8cdbc9cbceb673b78071794a8f0434cb3 Mon Sep 17 00:00:00 2001 From: Daniil Khaninaev Date: Wed, 16 Oct 2024 19:57:54 +0300 Subject: [PATCH] Added parameter `rotate_to_heading_once` Signed-off-by: Daniil Khaninaev --- .../nav2_rotation_shim_controller.hpp | 9 ++++++++- .../src/nav2_rotation_shim_controller.cpp | 18 +++++++++++++++++- 2 files changed, 25 insertions(+), 2 deletions(-) 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 d3807eeb95..abaf02c939 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 5b2bf306df..bb77879c7f 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(); } } }