New Goal checker: AxisGoalChecker

nav2_controller/CMakeLists.txt

@@ -167,6 +167,28 @@ target_link_libraries(position_goal_checker PRIVATE
   pluginlib::pluginlib
 )
 
+add_library(axis_goal_checker SHARED plugins/axis_goal_checker.cpp)
+target_include_directories(axis_goal_checker
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+target_link_libraries(axis_goal_checker PUBLIC
+  ${geometry_msgs_TARGETS}
+  nav2_core::nav2_core
+  nav2_ros_common::nav2_ros_common
+  nav2_costmap_2d::nav2_costmap_2d_core
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  ${rcl_interfaces_TARGETS}
+)
+target_link_libraries(axis_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
@@ -183,7 +205,7 @@ endif()
 
 rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer")
 
-install(TARGETS simple_progress_checker position_goal_checker pose_progress_checker simple_goal_checker stopped_goal_checker ${library_name}
+install(TARGETS simple_progress_checker position_goal_checker axis_goal_checker pose_progress_checker simple_goal_checker stopped_goal_checker ${library_name}
   EXPORT ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -204,6 +226,7 @@ ament_export_libraries(simple_progress_checker
   simple_goal_checker
   stopped_goal_checker
   position_goal_checker
+  axis_goal_checker
   ${library_name})
 ament_export_dependencies(
   geometry_msgs

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -280,6 +280,7 @@ class ControllerServer : public nav2::LifecycleNode
 
   // Whether we've published the single controller warning yet
   geometry_msgs::msg::PoseStamped end_pose_;
+  std::optional<geometry_msgs::msg::PoseStamped> before_end_pose_;
 
   geometry_msgs::msg::PoseStamped transformed_end_pose_;
 

nav2_controller/include/nav2_controller/plugins/axis_goal_checker.hpp

@@ -0,0 +1,72 @@
+// Copyright (c) 2025 Dexory
+//
+// 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__AXIS_GOAL_CHECKER_HPP_
+#define NAV2_CONTROLLER__PLUGINS__AXIS_GOAL_CHECKER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_ros_common/lifecycle_node.hpp"
+#include "nav2_core/goal_checker.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+
+namespace nav2_controller
+{
+
+/**  * @class AxisGoalChecker
+  * @brief Goal Checker plugin that checks progress along the axis defined by the
+  * 2 last poses of the pathalong the axis defined by the last segment of the path to the goal.
+  *
+  * This class can be configured to allow overshoot past the goal if the is_overshoot_valid
+  *  parameter is set to true (which it is false by default).
+  */
+class AxisGoalChecker : public nav2_core::GoalChecker
+{
+public:
+  AxisGoalChecker();
+  // Standard GoalChecker Interface
+  void initialize(
+    const nav2::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 std::optional<geometry_msgs::msg::Pose> & before_goal_pose,
+    const geometry_msgs::msg::Twist & velocity) override;
+  bool getTolerances(
+    geometry_msgs::msg::Pose & pose_tolerance,
+    geometry_msgs::msg::Twist & vel_tolerance) override;
+
+protected:
+  double goal_tolerance_;
+  bool is_overshoot_valid_;
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  std::string plugin_name_;
+
+  /**
+   * @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__AXIS_GOAL_CHECKER_HPP_

nav2_controller/include/nav2_controller/plugins/position_goal_checker.hpp

@@ -45,6 +45,7 @@ class PositionGoalChecker : public nav2_core::GoalChecker
 
   bool isGoalReached(
     const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+    const std::optional<geometry_msgs::msg::Pose> & before_goal_pose,
     const geometry_msgs::msg::Twist & velocity) override;
 
   bool getTolerances(

nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp

@@ -66,6 +66,7 @@ class SimpleGoalChecker : public nav2_core::GoalChecker
   void reset() override;
   bool isGoalReached(
     const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+    const std::optional<geometry_msgs::msg::Pose> & before_goal_pose,
     const geometry_msgs::msg::Twist & velocity) override;
   bool getTolerances(
     geometry_msgs::msg::Pose & pose_tolerance,

nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp

@@ -61,6 +61,7 @@ class StoppedGoalChecker : public SimpleGoalChecker
     const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
   bool isGoalReached(
     const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+    const std::optional<geometry_msgs::msg::Pose> & before_goal_pose,
     const geometry_msgs::msg::Twist & velocity) override;
   bool getTolerances(
     geometry_msgs::msg::Pose & pose_tolerance,

nav2_controller/plugins.xml

@@ -24,4 +24,9 @@
         <description>Goal checker that only checks XY position and ignores orientation</description>
     </class>
   </library>
+  <library path="axis_goal_checker">
+    <class type="nav2_controller::AxisGoalChecker" base_class_type="nav2_core::GoalChecker">
+        <description>Goal checker that checks progress along the axis defined by the 2 last poses of the path</description>
+    </class>
+  </library>
 </class_libraries>

nav2_controller/plugins/axis_goal_checker.cpp

@@ -0,0 +1,151 @@
+// Copyright (c) 2025 Dexory
+//
+// 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 <tf2/utils.h>
+
+#include <memory>
+#include <string>
+#include <limits>
+#include <vector>
+
+#include "angles/angles.h"
+#include "nav2_controller/plugins/axis_goal_checker.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "nav2_ros_common/node_utils.hpp"
+#include "nav2_util/geometry_utils.hpp"
+
+
+using rcl_interfaces::msg::ParameterType;
+using std::placeholders::_1;
+
+namespace nav2_controller
+{
+
+AxisGoalChecker::AxisGoalChecker()
+: goal_tolerance_(0.25)
+{
+}
+
+void AxisGoalChecker::initialize(
+  const nav2::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::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".goal_tolerance", rclcpp::ParameterValue(0.25));
+  node->get_parameter(plugin_name + ".goal_tolerance", goal_tolerance_);
+
+  nav2::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".is_overshoot_valid", rclcpp::ParameterValue(false));
+  node->get_parameter(plugin_name + ".is_overshoot_valid", is_overshoot_valid_);
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&AxisGoalChecker::dynamicParametersCallback, this, _1));
+}
+
+void AxisGoalChecker::reset()
+{
+}
+
+bool AxisGoalChecker::isGoalReached(
+  const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+  const std::optional<geometry_msgs::msg::Pose> & before_goal_pose,
+  const geometry_msgs::msg::Twist &)
+{
+  if (before_goal_pose.has_value()) {
+    // end of path direction
+    double end_of_path_yaw = atan2(
+      goal_pose.position.y - before_goal_pose->position.y,
+      goal_pose.position.x - before_goal_pose->position.x);
+
+    double robot_to_goal_yaw = atan2(
+      goal_pose.position.y - query_pose.position.y,
+      goal_pose.position.x - query_pose.position.x);
+
+    double projection_angle = angles::shortest_angular_distance(
+      robot_to_goal_yaw, end_of_path_yaw);
+
+    double projected_distance_to_goal = std::hypot(
+      goal_pose.position.x - query_pose.position.x,
+      goal_pose.position.y - query_pose.position.y) *
+      cos(projection_angle);
+
+    if (is_overshoot_valid_) {
+      return projected_distance_to_goal < goal_tolerance_;
+    } else {
+      return fabs(projected_distance_to_goal) < goal_tolerance_;
+    }
+  } else {
+    // handle path with only 1 point, in that case reverting to single distance check
+    double distance_to_goal = std::hypot(
+      goal_pose.position.x - query_pose.position.x,
+      goal_pose.position.y - query_pose.position.y);
+    return fabs(distance_to_goal) < goal_tolerance_;
+  }
+}
+
+bool AxisGoalChecker::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 = goal_tolerance_;
+  pose_tolerance.position.y = goal_tolerance_;
+  pose_tolerance.position.z = invalid_field;
+  pose_tolerance.orientation =
+    nav2_util::geometry_utils::orientationAroundZAxis(M_PI_2);
+
+  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
+AxisGoalChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> 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_ + ".segment_axis_goal_tolerance") {
+        goal_tolerance_ = parameter.as_double();
+      }
+    } else if (type == ParameterType::PARAMETER_BOOL) {
+      if (name == plugin_name_ + ".is_overshoot_valid") {
+        is_overshoot_valid_ = parameter.as_bool();
+      }
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_controller
+
+PLUGINLIB_EXPORT_CLASS(nav2_controller::AxisGoalChecker, nav2_core::GoalChecker)

nav2_controller/plugins/position_goal_checker.cpp

@@ -58,6 +58,7 @@ void PositionGoalChecker::reset()
 
 bool PositionGoalChecker::isGoalReached(
   const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+  const std::optional<geometry_msgs::msg::Pose> &,
   const geometry_msgs::msg::Twist &)
 {
   // If stateful and position was already reached, maintain state

nav2_controller/plugins/simple_goal_checker.cpp

@@ -87,6 +87,7 @@ void SimpleGoalChecker::reset()
 
 bool SimpleGoalChecker::isGoalReached(
   const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+  const std::optional<geometry_msgs::msg::Pose> &,
   const geometry_msgs::msg::Twist &)
 {
   if (check_xy_) {

nav2_controller/plugins/stopped_goal_checker.cpp

@@ -77,9 +77,10 @@ void StoppedGoalChecker::initialize(
 
 bool StoppedGoalChecker::isGoalReached(
   const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+  const std::optional<geometry_msgs::msg::Pose> & before_goal_pose,
   const geometry_msgs::msg::Twist & velocity)
 {
-  bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity);
+  bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, before_goal_pose, velocity);
   if (!ret) {
     return ret;
   }