diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index e83f91c7cdf..3ff2a565ec3 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -66,6 +66,26 @@ ament_target_dependencies(${library_name} ${dependencies} ) +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 @@ -84,7 +104,12 @@ target_link_libraries(${executable_name} ${library_name}) rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer") +<<<<<<< HEAD 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} +>>>>>>> bd8f422c (Addition of position goal checker, fix of rotation shim controller (#5162)) ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -110,6 +135,7 @@ ament_export_libraries(simple_progress_checker pose_progress_checker simple_goal_checker stopped_goal_checker + position_goal_checker ${library_name}) ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_core plugins.xml) 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..b3f151812f8 --- /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_ 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..3d7c6be44e5 --- /dev/null +++ b/nav2_controller/plugins/position_goal_checker.cpp @@ -0,0 +1,150 @@ +// 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 & parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } + + 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 (param_type == ParameterType::PARAMETER_BOOL) { + if (param_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) diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt index 2e141d5f440..f13c0f92e1b 100644 --- a/nav2_rotation_shim_controller/CMakeLists.txt +++ b/nav2_rotation_shim_controller/CMakeLists.txt @@ -3,6 +3,7 @@ project(nav2_rotation_shim_controller) find_package(ament_cmake 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) @@ -35,10 +36,34 @@ set(dependencies set(library_name nav2_rotation_shim_controller) add_library(${library_name} SHARED +<<<<<<< HEAD src/nav2_rotation_shim_controller.cpp) ament_target_dependencies(${library_name} ${dependencies} +======= + src/nav2_rotation_shim_controller.cpp) +target_include_directories(${library_name} + PUBLIC + "$" + "$" +) +target_link_libraries(${library_name} PUBLIC + ${geometry_msgs_TARGETS} + nav2_controller::position_goal_checker + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + tf2_ros::tf2_ros +) +target_link_libraries(${library_name} PRIVATE + angles::angles + nav2_util::nav2_util_core + tf2::tf2 +>>>>>>> bd8f422c (Addition of position goal checker, fix of rotation shim controller (#5162)) ) install(TARGETS ${library_name} @@ -61,7 +86,22 @@ endif() ament_export_include_directories(include) ament_export_libraries(${library_name}) +<<<<<<< HEAD ament_export_dependencies(${dependencies}) +======= +ament_export_dependencies( + geometry_msgs + nav2_core + nav2_controller + nav2_costmap_2d + pluginlib + rclcpp + rclcpp_lifecycle + rcl_interfaces + tf2_ros +) +ament_export_targets(${library_name}) +>>>>>>> bd8f422c (Addition of position goal checker, fix of rotation shim controller (#5162)) pluginlib_export_plugin_description_file(nav2_core nav2_rotation_shim_controller.xml) 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 78307d7ebf6..8a9ae57dced 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 @@ -31,7 +31,11 @@ #include "nav2_core/controller_exceptions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_costmap_2d/footprint_collision_checker.hpp" +<<<<<<< HEAD #include "angles/angles.h" +======= +#include "nav2_controller/plugins/position_goal_checker.hpp" +>>>>>>> bd8f422c (Addition of position goal checker, fix of rotation shim controller (#5162)) namespace nav2_rotation_shim_controller { @@ -195,6 +199,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 c9331264692..8425c75e531 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -11,6 +11,12 @@ nav2_common nav2_core +<<<<<<< HEAD +======= + nav2_controller + nav2_costmap_2d + nav2_msgs +>>>>>>> bd8f422c (Addition of position goal checker, fix of rotation shim controller (#5162)) nav2_util nav2_costmap_2d rclcpp 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 10c6a7211c9..e12d1e059fe 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -19,7 +19,6 @@ #include #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp" -#include "nav2_rotation_shim_controller/tools/utils.hpp" using rcl_interfaces::msg::ParameterType; @@ -39,6 +38,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(); @@ -124,6 +125,7 @@ void RotationShimController::activate() std::bind( &RotationShimController::dynamicParametersCallback, this, std::placeholders::_1)); + position_goal_checker_->reset(); } void RotationShimController::deactivate() @@ -152,6 +154,7 @@ void RotationShimController::cleanup() primary_controller_->cleanup(); primary_controller_.reset(); + position_goal_checker_.reset(); } geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands( @@ -173,11 +176,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); @@ -202,10 +206,25 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands std::lock_guard lock_reinit(mutex_); try { +<<<<<<< HEAD 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(); + double angular_distance_to_heading; + if (use_path_orientations_) { + angular_distance_to_heading = 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); + angular_distance_to_heading = std::atan2( + sampled_pt_base.position.y, + sampled_pt_base.position.x); + } +>>>>>>> bd8f422c (Addition of position goal checker, fix of rotation shim controller (#5162)) double angular_thresh = in_rotation_ ? angular_disengage_threshold_ : angular_dist_threshold_; @@ -376,6 +395,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) @@ -387,6 +407,7 @@ void RotationShimController::reset() { last_angular_vel_ = std::numeric_limits::max(); primary_controller_->reset(); + position_goal_checker_->reset(); } rcl_interfaces::msg::SetParametersResult