From 43c331d4d826ef5190cfb509933ce61459926e30 Mon Sep 17 00:00:00 2001 From: PrabhavSaxena Date: Thu, 15 May 2025 01:05:36 +0530 Subject: [PATCH 1/5] added position goal checker plugin Signed-off-by: PrabhavSaxena --- .../plugins/position_goal_checker.hpp | 78 +++++++++ nav2_controller/plugins.xml | 5 + .../plugins/position_goal_checker.cpp | 155 ++++++++++++++++++ 3 files changed, 238 insertions(+) create mode 100644 nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp create mode 100644 nav2_controller/plugins/position_goal_checker.cpp diff --git a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp new file mode 100644 index 00000000000..be856e02b82 --- /dev/null +++ b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp @@ -0,0 +1,78 @@ +// Copyright (c) 2025 Prabhav Saxena +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_ +#define NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_ + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_core/goal_checker.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +namespace nav2_controller +{ + + /** + * @class PositionGoalChecker + * @brief Goal Checker plugin that only checks XY position, ignoring orientation + */ + class PositionGoalChecker : public nav2_core::GoalChecker + { + public: + PositionGoalChecker(); + ~PositionGoalChecker() override = default; + + void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, + const std::string &plugin_name, + const std::shared_ptr costmap_ros) override; + + void reset() override; + + bool isGoalReached( + const geometry_msgs::msg::Pose &query_pose, const geometry_msgs::msg::Pose &goal_pose, + const geometry_msgs::msg::Twist &velocity) override; + + bool getTolerances( + geometry_msgs::msg::Pose &pose_tolerance, + geometry_msgs::msg::Twist &vel_tolerance) override; + + /** + * @brief Set the XY goal tolerance + * @param tolerance New tolerance value + */ + void setXYGoalTolerance(double tolerance); + + protected: + double xy_goal_tolerance_; + double xy_goal_tolerance_sq_; + bool stateful_; + bool position_reached_; + std::string plugin_name_; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + + /** + * @brief Callback executed when a parameter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + }; + +} // namespace nav2_controller + +#endif // NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_ \ No newline at end of file diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml index ffbd0e5f1cb..e3e9c7cf63b 100644 --- a/nav2_controller/plugins.xml +++ b/nav2_controller/plugins.xml @@ -19,4 +19,9 @@ Checks linear and angular velocity after stopping + + + Goal checker that only checks XY position and ignores orientation + + diff --git a/nav2_controller/plugins/position_goal_checker.cpp b/nav2_controller/plugins/position_goal_checker.cpp new file mode 100644 index 00000000000..d14efc28cdc --- /dev/null +++ b/nav2_controller/plugins/position_goal_checker.cpp @@ -0,0 +1,155 @@ +// Copyright (c) 2025 Prabhav Saxena +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include "nav2_controller/plugins/position_goal_checker.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "nav2_util/node_utils.hpp" + +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; + +namespace nav2_controller +{ + + PositionGoalChecker::PositionGoalChecker() + : xy_goal_tolerance_(0.25), + xy_goal_tolerance_sq_(0.0625), + stateful_(true), + position_reached_(false) + { + } + + void PositionGoalChecker::initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, + const std::string &plugin_name, + const std::shared_ptr /*costmap_ros*/) + { + plugin_name_ = plugin_name; + auto node = parent.lock(); + + nav2_util::declare_parameter_if_not_declared( + node, + plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25)); + nav2_util::declare_parameter_if_not_declared( + node, + plugin_name + ".stateful", rclcpp::ParameterValue(true)); + + node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); + node->get_parameter(plugin_name + ".stateful", stateful_); + + xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&PositionGoalChecker::dynamicParametersCallback, this, _1)); + } + + void PositionGoalChecker::reset() + { + position_reached_ = false; + } + + bool PositionGoalChecker::isGoalReached( + const geometry_msgs::msg::Pose &query_pose, const geometry_msgs::msg::Pose &goal_pose, + const geometry_msgs::msg::Twist &) + { + // If stateful and position was already reached, maintain state + if (stateful_ && position_reached_) + { + return true; + } + + // Check if position is within tolerance + double dx = query_pose.position.x - goal_pose.position.x; + double dy = query_pose.position.y - goal_pose.position.y; + + bool position_reached = (dx * dx + dy * dy <= xy_goal_tolerance_sq_); + + // If stateful, remember that we reached the position + if (stateful_ && position_reached) + { + position_reached_ = true; + } + + return position_reached; + } + + bool PositionGoalChecker::getTolerances( + geometry_msgs::msg::Pose &pose_tolerance, + geometry_msgs::msg::Twist &vel_tolerance) + { + double invalid_field = std::numeric_limits::lowest(); + + pose_tolerance.position.x = xy_goal_tolerance_; + pose_tolerance.position.y = xy_goal_tolerance_; + pose_tolerance.position.z = invalid_field; + + // Return zero orientation tolerance as we don't check it + pose_tolerance.orientation.x = 0.0; + pose_tolerance.orientation.y = 0.0; + pose_tolerance.orientation.z = 0.0; + pose_tolerance.orientation.w = 1.0; + + vel_tolerance.linear.x = invalid_field; + vel_tolerance.linear.y = invalid_field; + vel_tolerance.linear.z = invalid_field; + + vel_tolerance.angular.x = invalid_field; + vel_tolerance.angular.y = invalid_field; + vel_tolerance.angular.z = invalid_field; + + return true; + } + + void nav2_controller::PositionGoalChecker::setXYGoalTolerance(double tolerance) + { + xy_goal_tolerance_ = tolerance; + xy_goal_tolerance_sq_ = tolerance * tolerance; + } + + rcl_interfaces::msg::SetParametersResult + PositionGoalChecker::dynamicParametersCallback(std::vector parameters) + { + rcl_interfaces::msg::SetParametersResult result; + for (auto ¶meter : parameters) + { + const auto &type = parameter.get_type(); + const auto &name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) + { + if (name == plugin_name_ + ".xy_goal_tolerance") + { + xy_goal_tolerance_ = parameter.as_double(); + xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; + } + } + else if (type == ParameterType::PARAMETER_BOOL) + { + if (name == plugin_name_ + ".stateful") + { + stateful_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; + } + +} // namespace nav2_controller + +PLUGINLIB_EXPORT_CLASS(nav2_controller::PositionGoalChecker, nav2_core::GoalChecker) \ No newline at end of file From 1e34e856a604e21675e868d0d2d90982f05b76ba Mon Sep 17 00:00:00 2001 From: PrabhavSaxena Date: Fri, 16 May 2025 00:08:52 +0530 Subject: [PATCH 2/5] updated rotation shim to use position goal checker Signed-off-by: PrabhavSaxena --- nav2_controller/CMakeLists.txt | 23 ++++++- nav2_rotation_shim_controller/CMakeLists.txt | 3 + .../nav2_rotation_shim_controller.hpp | 2 + .../tools/utils.hpp | 60 ------------------- nav2_rotation_shim_controller/package.xml | 1 + .../src/nav2_rotation_shim_controller.cpp | 17 ++++-- 6 files changed, 40 insertions(+), 66 deletions(-) delete mode 100644 nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 76725b8d47f..3cfaeeb9bf2 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -145,6 +145,26 @@ target_link_libraries(stopped_goal_checker PRIVATE pluginlib::pluginlib ) +add_library(position_goal_checker SHARED plugins/position_goal_checker.cpp) +target_include_directories(position_goal_checker + PUBLIC + "$" + "$" +) +target_link_libraries(position_goal_checker PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} +) +target_link_libraries(position_goal_checker PRIVATE + angles::angles + nav2_util::nav2_util_core + pluginlib::pluginlib +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -161,7 +181,7 @@ endif() rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer") -install(TARGETS simple_progress_checker pose_progress_checker simple_goal_checker stopped_goal_checker ${library_name} +install(TARGETS simple_progress_checker position_goal_checker pose_progress_checker simple_goal_checker stopped_goal_checker ${library_name} EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -181,6 +201,7 @@ ament_export_libraries(simple_progress_checker pose_progress_checker simple_goal_checker stopped_goal_checker + position_goal_checker ${library_name}) ament_export_dependencies( geometry_msgs diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt index fafa98863c1..1c8bcf0cea3 100644 --- a/nav2_rotation_shim_controller/CMakeLists.txt +++ b/nav2_rotation_shim_controller/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) +find_package(nav2_controller REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_util REQUIRED) @@ -29,6 +30,7 @@ target_include_directories(${library_name} ) target_link_libraries(${library_name} PUBLIC ${geometry_msgs_TARGETS} + nav2_controller::nav2_controller nav2_core::nav2_core nav2_costmap_2d::nav2_costmap_2d_core pluginlib::pluginlib @@ -69,6 +71,7 @@ ament_export_libraries(${library_name}) ament_export_dependencies( geometry_msgs nav2_core + nav2_controller nav2_costmap_2d pluginlib rclcpp 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 b587c4defd2..b5e16edf8c4 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 @@ -28,6 +28,7 @@ #include "nav2_core/controller.hpp" #include "nav2_core/controller_exceptions.hpp" #include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_controller/plugins/position_goal_checker.hpp" namespace nav2_rotation_shim_controller { @@ -192,6 +193,7 @@ class RotationShimController : public nav2_core::Controller // Dynamic parameters handler std::mutex mutex_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::unique_ptr position_goal_checker_; }; } // namespace nav2_rotation_shim_controller diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp deleted file mode 100644 index 0a4ff4ac163..00000000000 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov -// Copyright (c) 2023 Open Navigation LLC -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_ -#define NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_ - -#include "nav2_core/goal_checker.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace nav2_rotation_shim_controller::utils -{ - -/** -* @brief get the current pose of the robot -* @param goal_checker goal checker to get tolerances -* @param robot robot pose -* @param goal goal pose -* @return bool Whether the robot is in the distance tolerance ignoring rotation and speed -*/ -inline bool withinPositionGoalTolerance( - nav2_core::GoalChecker * goal_checker, - const geometry_msgs::msg::Pose & robot, - const geometry_msgs::msg::Pose & goal) -{ - if (goal_checker) { - geometry_msgs::msg::Pose pose_tolerance; - geometry_msgs::msg::Twist velocity_tolerance; - goal_checker->getTolerances(pose_tolerance, velocity_tolerance); - - const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x; - - auto dx = robot.position.x - goal.position.x; - auto dy = robot.position.y - goal.position.y; - - auto dist_sq = dx * dx + dy * dy; - - if (dist_sq < pose_tolerance_sq) { - return true; - } - } - - return false; -} - -} // namespace nav2_rotation_shim_controller::utils - -#endif // NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 9e450f75736..14e1e5547ed 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -13,6 +13,7 @@ angles geometry_msgs nav2_core + nav2_controller nav2_costmap_2d nav2_msgs nav2_util 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 e6e435adbf5..e5fc07368b0 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -24,7 +24,6 @@ #include "nav2_util/robot_utils.hpp" #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp" -#include "nav2_rotation_shim_controller/tools/utils.hpp" using rcl_interfaces::msg::ParameterType; @@ -44,6 +43,8 @@ void RotationShimController::configure( std::string name, std::shared_ptr tf, std::shared_ptr costmap_ros) { + position_goal_checker_ = std::make_unique(); + position_goal_checker_->initialize(parent, plugin_name_ + ".position_checker", costmap_ros); plugin_name_ = name; node_ = parent; auto node = parent.lock(); @@ -132,6 +133,7 @@ void RotationShimController::activate() std::bind( &RotationShimController::dynamicParametersCallback, this, std::placeholders::_1)); + position_goal_checker_->reset(); } void RotationShimController::deactivate() @@ -160,6 +162,7 @@ void RotationShimController::cleanup() primary_controller_->cleanup(); primary_controller_.reset(); + position_goal_checker_.reset(); } geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands( @@ -181,10 +184,12 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands throw nav2_core::ControllerTFError("Failed to transform pose to base frame!"); } - if (utils::withinPositionGoalTolerance( - goal_checker, - pose.pose, - sampled_pt_goal.pose)) + geometry_msgs::msg::Pose pose_tolerance; + geometry_msgs::msg::Twist vel_tolerance; + goal_checker->getTolerances(pose_tolerance, vel_tolerance); + position_goal_checker_->setXYGoalTolerance(pose_tolerance.position.x); + + if (position_goal_checker_->isGoalReached(pose.pose, sampled_pt_goal.pose, velocity)) { double pose_yaw = tf2::getYaw(pose.pose.orientation); double goal_yaw = tf2::getYaw(sampled_pt_goal.pose.orientation); @@ -391,6 +396,7 @@ void RotationShimController::setPlan(const nav_msgs::msg::Path & path) path_updated_ = rotate_to_heading_once_ ? isGoalChanged(path) : true; current_path_ = path; primary_controller_->setPlan(path); + position_goal_checker_->reset(); } void RotationShimController::setSpeedLimit(const double & speed_limit, const bool & percentage) @@ -402,6 +408,7 @@ void RotationShimController::reset() { last_angular_vel_ = std::numeric_limits::max(); primary_controller_->reset(); + position_goal_checker_->reset(); } rcl_interfaces::msg::SetParametersResult From 22b2483c0f7dc407b69b72a1a859b555400fd87c Mon Sep 17 00:00:00 2001 From: PrabhavSaxena Date: Fri, 16 May 2025 00:27:06 +0530 Subject: [PATCH 3/5] fixed lint Signed-off-by: PrabhavSaxena --- .../plugins/position_goal_checker.hpp | 74 +++--- .../plugins/position_goal_checker.cpp | 210 +++++++++--------- .../src/nav2_rotation_shim_controller.cpp | 6 +- 3 files changed, 141 insertions(+), 149 deletions(-) diff --git a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp index be856e02b82..78c4a39a5c3 100644 --- a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp @@ -26,53 +26,53 @@ namespace nav2_controller { - /** +/** * @class PositionGoalChecker * @brief Goal Checker plugin that only checks XY position, ignoring orientation */ - class PositionGoalChecker : public nav2_core::GoalChecker - { - public: - PositionGoalChecker(); - ~PositionGoalChecker() override = default; +class PositionGoalChecker : public nav2_core::GoalChecker +{ +public: + PositionGoalChecker(); + ~PositionGoalChecker() override = default; - void initialize( - const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, - const std::string &plugin_name, - const std::shared_ptr costmap_ros) override; + void initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr costmap_ros) override; - void reset() override; + void reset() override; - bool isGoalReached( - const geometry_msgs::msg::Pose &query_pose, const geometry_msgs::msg::Pose &goal_pose, - const geometry_msgs::msg::Twist &velocity) override; + bool isGoalReached( + const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, + const geometry_msgs::msg::Twist & velocity) override; - bool getTolerances( - geometry_msgs::msg::Pose &pose_tolerance, - geometry_msgs::msg::Twist &vel_tolerance) override; + bool getTolerances( + geometry_msgs::msg::Pose & pose_tolerance, + geometry_msgs::msg::Twist & vel_tolerance) override; - /** - * @brief Set the XY goal tolerance - * @param tolerance New tolerance value - */ - void setXYGoalTolerance(double tolerance); + /** + * @brief Set the XY goal tolerance + * @param tolerance New tolerance value + */ + void setXYGoalTolerance(double tolerance); - protected: - double xy_goal_tolerance_; - double xy_goal_tolerance_sq_; - bool stateful_; - bool position_reached_; - std::string plugin_name_; - rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; +protected: + double xy_goal_tolerance_; + double xy_goal_tolerance_sq_; + bool stateful_; + bool position_reached_; + std::string plugin_name_; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; - /** - * @brief Callback executed when a parameter change is detected - * @param parameters list of changed parameters - */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); - }; + /** + * @brief Callback executed when a parameter change is detected + * @param parameters list of changed parameters + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); +}; } // namespace nav2_controller -#endif // NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_ \ No newline at end of file +#endif // NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_ diff --git a/nav2_controller/plugins/position_goal_checker.cpp b/nav2_controller/plugins/position_goal_checker.cpp index d14efc28cdc..1a243e9864c 100644 --- a/nav2_controller/plugins/position_goal_checker.cpp +++ b/nav2_controller/plugins/position_goal_checker.cpp @@ -25,131 +25,123 @@ using std::placeholders::_1; namespace nav2_controller { - PositionGoalChecker::PositionGoalChecker() - : xy_goal_tolerance_(0.25), - xy_goal_tolerance_sq_(0.0625), - stateful_(true), - position_reached_(false) - { - } +PositionGoalChecker::PositionGoalChecker() +: xy_goal_tolerance_(0.25), + xy_goal_tolerance_sq_(0.0625), + stateful_(true), + position_reached_(false) +{ +} - void PositionGoalChecker::initialize( - const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, - const std::string &plugin_name, - const std::shared_ptr /*costmap_ros*/) - { - plugin_name_ = plugin_name; - auto node = parent.lock(); +void PositionGoalChecker::initialize( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + const std::string & plugin_name, + const std::shared_ptr/*costmap_ros*/) +{ + plugin_name_ = plugin_name; + auto node = parent.lock(); - nav2_util::declare_parameter_if_not_declared( - node, - plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25)); - nav2_util::declare_parameter_if_not_declared( - node, - plugin_name + ".stateful", rclcpp::ParameterValue(true)); + nav2_util::declare_parameter_if_not_declared( + node, + plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25)); + nav2_util::declare_parameter_if_not_declared( + node, + plugin_name + ".stateful", rclcpp::ParameterValue(true)); - node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); - node->get_parameter(plugin_name + ".stateful", stateful_); + node->get_parameter(plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_); + node->get_parameter(plugin_name + ".stateful", stateful_); - xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; + xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; - // Add callback for dynamic parameters - dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind(&PositionGoalChecker::dynamicParametersCallback, this, _1)); - } + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&PositionGoalChecker::dynamicParametersCallback, this, _1)); +} - void PositionGoalChecker::reset() - { - position_reached_ = false; - } +void PositionGoalChecker::reset() +{ + position_reached_ = false; +} - bool PositionGoalChecker::isGoalReached( - const geometry_msgs::msg::Pose &query_pose, const geometry_msgs::msg::Pose &goal_pose, - const geometry_msgs::msg::Twist &) - { - // If stateful and position was already reached, maintain state - if (stateful_ && position_reached_) - { - return true; - } - - // Check if position is within tolerance - double dx = query_pose.position.x - goal_pose.position.x; - double dy = query_pose.position.y - goal_pose.position.y; - - bool position_reached = (dx * dx + dy * dy <= xy_goal_tolerance_sq_); - - // If stateful, remember that we reached the position - if (stateful_ && position_reached) - { - position_reached_ = true; - } - - return position_reached; - } +bool PositionGoalChecker::isGoalReached( + const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose, + const geometry_msgs::msg::Twist &) +{ + // If stateful and position was already reached, maintain state + if (stateful_ && position_reached_) { + return true; + } - bool PositionGoalChecker::getTolerances( - geometry_msgs::msg::Pose &pose_tolerance, - geometry_msgs::msg::Twist &vel_tolerance) - { - double invalid_field = std::numeric_limits::lowest(); + // Check if position is within tolerance + double dx = query_pose.position.x - goal_pose.position.x; + double dy = query_pose.position.y - goal_pose.position.y; - pose_tolerance.position.x = xy_goal_tolerance_; - pose_tolerance.position.y = xy_goal_tolerance_; - pose_tolerance.position.z = invalid_field; + bool position_reached = (dx * dx + dy * dy <= xy_goal_tolerance_sq_); - // Return zero orientation tolerance as we don't check it - pose_tolerance.orientation.x = 0.0; - pose_tolerance.orientation.y = 0.0; - pose_tolerance.orientation.z = 0.0; - pose_tolerance.orientation.w = 1.0; + // If stateful, remember that we reached the position + if (stateful_ && position_reached) { + position_reached_ = true; + } - vel_tolerance.linear.x = invalid_field; - vel_tolerance.linear.y = invalid_field; - vel_tolerance.linear.z = invalid_field; + return position_reached; +} - vel_tolerance.angular.x = invalid_field; - vel_tolerance.angular.y = invalid_field; - vel_tolerance.angular.z = invalid_field; +bool PositionGoalChecker::getTolerances( + geometry_msgs::msg::Pose & pose_tolerance, + geometry_msgs::msg::Twist & vel_tolerance) +{ + double invalid_field = std::numeric_limits::lowest(); - return true; - } + pose_tolerance.position.x = xy_goal_tolerance_; + pose_tolerance.position.y = xy_goal_tolerance_; + pose_tolerance.position.z = invalid_field; - void nav2_controller::PositionGoalChecker::setXYGoalTolerance(double tolerance) - { - xy_goal_tolerance_ = tolerance; - xy_goal_tolerance_sq_ = tolerance * tolerance; - } + // Return zero orientation tolerance as we don't check it + pose_tolerance.orientation.x = 0.0; + pose_tolerance.orientation.y = 0.0; + pose_tolerance.orientation.z = 0.0; + pose_tolerance.orientation.w = 1.0; + + vel_tolerance.linear.x = invalid_field; + vel_tolerance.linear.y = invalid_field; + vel_tolerance.linear.z = invalid_field; + + vel_tolerance.angular.x = invalid_field; + vel_tolerance.angular.y = invalid_field; + vel_tolerance.angular.z = invalid_field; + + return true; +} - rcl_interfaces::msg::SetParametersResult - PositionGoalChecker::dynamicParametersCallback(std::vector parameters) - { - rcl_interfaces::msg::SetParametersResult result; - for (auto ¶meter : parameters) - { - const auto &type = parameter.get_type(); - const auto &name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) - { - if (name == plugin_name_ + ".xy_goal_tolerance") - { - xy_goal_tolerance_ = parameter.as_double(); - xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; - } - } - else if (type == ParameterType::PARAMETER_BOOL) - { - if (name == plugin_name_ + ".stateful") - { - stateful_ = parameter.as_bool(); - } - } - } - result.successful = true; - return result; +void nav2_controller::PositionGoalChecker::setXYGoalTolerance(double tolerance) +{ + xy_goal_tolerance_ = tolerance; + xy_goal_tolerance_sq_ = tolerance * tolerance; +} + +rcl_interfaces::msg::SetParametersResult +PositionGoalChecker::dynamicParametersCallback(std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + for (auto & parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".xy_goal_tolerance") { + xy_goal_tolerance_ = parameter.as_double(); + xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == plugin_name_ + ".stateful") { + stateful_ = parameter.as_bool(); + } } + } + result.successful = true; + return result; +} } // namespace nav2_controller -PLUGINLIB_EXPORT_CLASS(nav2_controller::PositionGoalChecker, nav2_core::GoalChecker) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(nav2_controller::PositionGoalChecker, nav2_core::GoalChecker) 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 e5fc07368b0..698f4e42590 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -189,8 +189,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands goal_checker->getTolerances(pose_tolerance, vel_tolerance); position_goal_checker_->setXYGoalTolerance(pose_tolerance.position.x); - if (position_goal_checker_->isGoalReached(pose.pose, sampled_pt_goal.pose, velocity)) - { + if (position_goal_checker_->isGoalReached(pose.pose, sampled_pt_goal.pose, velocity)) { double pose_yaw = tf2::getYaw(pose.pose.orientation); double goal_yaw = tf2::getYaw(sampled_pt_goal.pose.orientation); @@ -223,7 +222,8 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands 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, + angular_distance_to_heading = std::atan2( + sampled_pt_base.position.y, sampled_pt_base.position.x); } From e8ab73caec8ad41f1c5237a2986510dab5863db2 Mon Sep 17 00:00:00 2001 From: PrabhavSaxena Date: Fri, 16 May 2025 00:36:45 +0530 Subject: [PATCH 4/5] added comments Signed-off-by: PrabhavSaxena --- .../include/nav2_controller/plugins/position_goal_checker.hpp | 4 ++-- nav2_controller/plugins/position_goal_checker.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp index 78c4a39a5c3..b3f151812f8 100644 --- a/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp +++ b/nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp @@ -73,6 +73,6 @@ class PositionGoalChecker : public nav2_core::GoalChecker dynamicParametersCallback(std::vector parameters); }; -} // namespace nav2_controller +} // namespace nav2_controller -#endif // NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_ +#endif // NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_ diff --git a/nav2_controller/plugins/position_goal_checker.cpp b/nav2_controller/plugins/position_goal_checker.cpp index 1a243e9864c..95b481bf6fe 100644 --- a/nav2_controller/plugins/position_goal_checker.cpp +++ b/nav2_controller/plugins/position_goal_checker.cpp @@ -142,6 +142,6 @@ PositionGoalChecker::dynamicParametersCallback(std::vector pa return result; } -} // namespace nav2_controller +} // namespace nav2_controller PLUGINLIB_EXPORT_CLASS(nav2_controller::PositionGoalChecker, nav2_core::GoalChecker) From f0bd3ce622c8c851e36bedc5d23d5f1d5b5c44ab Mon Sep 17 00:00:00 2001 From: PrabhavSaxena Date: Fri, 16 May 2025 12:08:42 +0530 Subject: [PATCH 5/5] fixed dynamic parameter, CMake Signed-off-by: PrabhavSaxena --- nav2_controller/plugins/position_goal_checker.cpp | 15 +++++++++------ nav2_rotation_shim_controller/CMakeLists.txt | 2 +- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/nav2_controller/plugins/position_goal_checker.cpp b/nav2_controller/plugins/position_goal_checker.cpp index 95b481bf6fe..3d7c6be44e5 100644 --- a/nav2_controller/plugins/position_goal_checker.cpp +++ b/nav2_controller/plugins/position_goal_checker.cpp @@ -124,16 +124,19 @@ PositionGoalChecker::dynamicParametersCallback(std::vector pa { rcl_interfaces::msg::SetParametersResult result; for (auto & parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".xy_goal_tolerance") { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".xy_goal_tolerance") { xy_goal_tolerance_ = parameter.as_double(); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".stateful") { + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".stateful") { stateful_ = parameter.as_bool(); } } diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt index 1c8bcf0cea3..3323d3d767a 100644 --- a/nav2_rotation_shim_controller/CMakeLists.txt +++ b/nav2_rotation_shim_controller/CMakeLists.txt @@ -30,7 +30,7 @@ target_include_directories(${library_name} ) target_link_libraries(${library_name} PUBLIC ${geometry_msgs_TARGETS} - nav2_controller::nav2_controller + nav2_controller::position_goal_checker nav2_core::nav2_core nav2_costmap_2d::nav2_costmap_2d_core pluginlib::pluginlib