Merge pull request #5179 from mini-1235/bp/5162

Backport 5162 to Jazzy

Author: SteveMacenski

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)

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)
@@ -30,6 +31,7 @@ set(dependencies
   nav2_core
   tf2
   angles
+  nav2_controller
 )
 
 set(library_name nav2_rotation_shim_controller)

nav2_rotation_shim_controller/README.md

@@ -31,13 +31,14 @@ See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/
 
 | Parameter | Description | 
 |-----|----|
-| `angular_dist_threshold` | Maximum angular distance, in radians, away from the path heading to trigger rotation until within. | 
-| `forward_sampling_distance` | Forward distance, in meters, along path to select a sampling point to use to approximate path heading | 
-| `rotate_to_heading_angular_vel` | Angular rotational velocity, in rad/s, to rotate to the path heading | 
-| `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading | 
-| `max_angular_accel` | Maximum angular acceleration for rotation to heading | 
-| `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. | 
-| `rotate_to_goal_heading` | If true, the rotationShimController will take back control of the robot when in XY tolerance of the goal and start rotating to the goal heading | 
+| `angular_dist_threshold` | Maximum angular distance, in radians, away from the path heading to trigger rotation until within. |
+| `forward_sampling_distance` | Forward distance, in meters, along path to select a sampling point to use to approximate path heading |
+| `rotate_to_heading_angular_vel` | Angular rotational velocity, in rad/s, to rotate to the path heading |
+| `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading |
+| `max_angular_accel` | Maximum angular acceleration for rotation to heading |
+| `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. |
+| `rotate_to_goal_heading` | If true, the rotationShimController will take back control of the robot when in XY tolerance of the goal and start rotating to the goal heading |
+| `use_path_orientations` | If true, the controller will use the orientations of the path points to compute the heading of the path instead of computing the heading from the path points. If true, the controller will use the orientations of the path points to compute the heading of the path instead of computing the heading from the path points. Use for for feasible planners like the Smac Planner which generate feasible paths with orientations for forward and reverse motion. |
 
 Example fully-described XML with default parameter values:
 
@@ -70,6 +71,7 @@ controller_server:
       max_angular_accel: 3.2
       simulate_ahead_time: 1.0
       rotate_to_goal_heading: false
+      use_path_orientations: false
 
       # DWB parameters
       ...

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
@@ -190,11 +191,13 @@ class RotationShimController : public nav2_core::Controller
   double control_duration_, simulate_ahead_time_;
   bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_;
   bool closed_loop_;
+  bool use_path_orientations_;
   double last_angular_vel_ = std::numeric_limits<double>::max();
 
   // 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