diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index e83f91c7cdf..03cc4cfd3d2 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -62,6 +62,10 @@ add_library(stopped_goal_checker SHARED plugins/stopped_goal_checker.cpp) target_link_libraries(stopped_goal_checker simple_goal_checker) ament_target_dependencies(stopped_goal_checker ${dependencies}) +add_library(position_goal_checker SHARED plugins/position_goal_checker.cpp) +target_link_libraries(position_goal_checker simple_goal_checker) +ament_target_dependencies(position_goal_checker ${dependencies}) + ament_target_dependencies(${library_name} ${dependencies} ) @@ -84,7 +88,7 @@ target_link_libraries(${executable_name} ${library_name}) 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} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -110,6 +114,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..fc4688f6560 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) @@ -30,6 +31,7 @@ set(dependencies nav2_core tf2 angles + nav2_controller ) set(library_name nav2_rotation_shim_controller) 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 6a70229621c..d1f4b957873 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,6 +31,7 @@ #include "nav2_core/controller_exceptions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_controller/plugins/position_goal_checker.hpp" #include "angles/angles.h" namespace nav2_rotation_shim_controller @@ -196,6 +197,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..5df529ca0fc 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -11,6 +11,7 @@ nav2_common nav2_core + nav2_controller 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 66ee627faf7..64a3a213bf0 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(); @@ -127,6 +128,7 @@ void RotationShimController::activate() std::bind( &RotationShimController::dynamicParametersCallback, this, std::placeholders::_1)); + position_goal_checker_->reset(); } void RotationShimController::deactivate() @@ -155,6 +157,7 @@ void RotationShimController::cleanup() primary_controller_->cleanup(); primary_controller_.reset(); + position_goal_checker_.reset(); } geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands( @@ -176,11 +179,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); @@ -213,7 +217,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); } @@ -386,6 +391,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) @@ -397,6 +403,7 @@ void RotationShimController::reset() { last_angular_vel_ = std::numeric_limits::max(); primary_controller_->reset(); + position_goal_checker_->reset(); } rcl_interfaces::msg::SetParametersResult diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index ce755bc2e04..8a23c1e79fa 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -177,14 +177,15 @@ TEST(RotationShimControllerTest, rotationAndTransformTests) std::string name = "PathFollower"; auto tf = std::make_shared(node->get_clock()); auto costmap = std::make_shared("fake_costmap"); - rclcpp_lifecycle::State state; - costmap->on_configure(state); + costmap->configure(); // 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("controller_frequency", 1.0); + auto controller = std::make_shared(); controller->configure(node, name, tf, costmap); controller->activate(); @@ -193,8 +194,8 @@ TEST(RotationShimControllerTest, rotationAndTransformTests) nav_msgs::msg::Path path; path.header.frame_id = "fake_frame"; path.poses.resize(10); - path.poses[1].pose.position.x = 0.1; - path.poses[1].pose.position.y = 0.1; + path.poses[1].pose.position.x = 0.15; + path.poses[1].pose.position.y = 0.15; path.poses[2].pose.position.x = 1.0; path.poses[2].pose.position.y = 1.0; path.poses[3].pose.position.x = 10.0; @@ -204,14 +205,14 @@ TEST(RotationShimControllerTest, rotationAndTransformTests) const geometry_msgs::msg::Twist velocity; EXPECT_EQ( controller->computeRotateToHeadingCommandWrapper( - 0.7, path.poses[0], velocity).twist.angular.z, 1.8); + 0.7, path.poses[1], velocity).twist.angular.z, 1.8); EXPECT_EQ( controller->computeRotateToHeadingCommandWrapper( - -0.7, path.poses[0], velocity).twist.angular.z, -1.8); + -0.7, path.poses[1], velocity).twist.angular.z, -1.8); EXPECT_EQ( controller->computeRotateToHeadingCommandWrapper( - 0.87, path.poses[0], velocity).twist.angular.z, 1.8); + 0.87, path.poses[1], velocity).twist.angular.z, 1.8); // in base_link, so should pass through values without issue geometry_msgs::msg::PoseStamped pt; @@ -237,8 +238,9 @@ TEST(RotationShimControllerTest, computeVelocityTests) 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); + costmap->set_parameter(rclcpp::Parameter("origin_x", -25.0)); + costmap->set_parameter(rclcpp::Parameter("origin_y", -25.0)); + costmap->configure(); auto tf_broadcaster = std::make_shared(node); geometry_msgs::msg::TransformStamped transform; @@ -254,6 +256,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) node->declare_parameter( "PathFollower.primary_controller", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + node->declare_parameter("controller_frequency", 1.0); auto controller = std::make_shared(); controller->configure(node, name, tf, costmap); @@ -308,7 +311,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // this should allow it to find the sampled point, then transform to base_link // validly because we setup the TF for it. The 1.0 should be selected since default min // is 0.5 and that should cause a pass off to the RPP controller which will throw - // and exception because the costmap is bogus + // and exception because it is off of the costmap controller->setPlan(path); tf_broadcaster->sendTransform(transform); EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error);