Addition of position goal checker, fix of rotation shim controller (backport #5162)

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
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+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)

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 <string>
+#include <memory>
+#include <vector>
+#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<nav2_costmap_2d::Costmap2DROS> 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<rclcpp::Parameter> parameters);
+};
+
+}  // namespace nav2_controller
+
+#endif  // NAV2_CONTROLLER__PLUGINS__POSITION_GOAL_CHECKER_HPP_

nav2_controller/plugins.xml

@@ -19,4 +19,9 @@
         <description>Checks linear and angular velocity after stopping</description>
     </class>
   </library>
+  <library path="position_goal_checker">
+    <class type="nav2_controller::PositionGoalChecker" base_class_type="nav2_core::GoalChecker">
+        <description>Goal checker that only checks XY position and ignores orientation</description>
+    </class>
+  </library>
 </class_libraries>

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 <memory>
+#include <string>
+#include <limits>
+#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<nav2_costmap_2d::Costmap2DROS>/*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<double>::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<rclcpp::Parameter> 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)

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
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+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)
 

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<nav2_controller::PositionGoalChecker> position_goal_checker_;
 };
 
 }  // namespace nav2_rotation_shim_controller