New Goal checker: AxisGoalChecker

nav2_controller/CMakeLists.txt

@@ -169,6 +169,27 @@ 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
+)
+
 add_library(feasible_path_handler SHARED plugins/feasible_path_handler.cpp)
 target_include_directories(feasible_path_handler
   PUBLIC
@@ -211,6 +232,7 @@ install(TARGETS
   pose_progress_checker
   simple_goal_checker
   stopped_goal_checker
+  axis_goal_checker
   feasible_path_handler
   ${library_name}
   EXPORT ${library_name}
@@ -233,6 +255,7 @@ ament_export_libraries(simple_progress_checker
   simple_goal_checker
   stopped_goal_checker
   position_goal_checker
+  axis_goal_checker
   feasible_path_handler
   ${library_name})
 ament_export_dependencies(

nav2_controller/include/nav2_controller/plugins/axis_goal_checker.hpp

@@ -0,0 +1,106 @@
+// 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 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 is false by default).
+  */
+class AxisGoalChecker : public nav2_core::GoalChecker
+{
+public:
+  /**
+   * @brief Construct a new Axis Goal Checker object
+   */
+  AxisGoalChecker();
+
+  // Standard GoalChecker Interface
+  /**
+   * @brief Initialize the goal checker
+   * @param parent Weak pointer to the lifecycle node
+   * @param plugin_name Name of the plugin
+   * @param costmap_ros Shared pointer to the costmap
+   */
+  void initialize(
+    const nav2::LifecycleNode::WeakPtr & parent,
+    const std::string & plugin_name,
+    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
+
+  /**
+   * @brief Reset the goal checker state
+   */
+  void reset() override;
+
+  /**
+   * @brief Check if the goal is reached
+   * @param query_pose Current pose of the robot
+   * @param goal_pose Target goal pose
+   * @param velocity Current velocity of the robot
+   * @param transformed_global_plan The transformed global plan
+   * @return true if goal is reached, false otherwise
+   */
+  bool isGoalReached(
+    const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+    const geometry_msgs::msg::Twist & velocity,
+    const nav_msgs::msg::Path & transformed_global_plan) override;
+
+  /**
+   * @brief Get the position and velocity tolerances
+   * @param pose_tolerance Output parameter for pose tolerance
+   * @param vel_tolerance Output parameter for velocity tolerance
+   * @return true if tolerances are available, false otherwise
+   */
+  bool getTolerances(
+    geometry_msgs::msg::Pose & pose_tolerance,
+    geometry_msgs::msg::Twist & vel_tolerance) override;
+
+protected:
+  double along_path_tolerance_;
+  double cross_track_tolerance_;
+  double path_length_tolerance_;
+  bool is_overshoot_valid_;
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  std::string plugin_name_;
+  rclcpp::Logger logger_{rclcpp::get_logger("AxisGoalChecker")};
+
+  /**
+   * @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/plugins.xml

@@ -24,6 +24,11 @@
         <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 last 2 poses of the path</description>
+    </class>
+  </library>
   <library path="feasible_path_handler">
     <class type="nav2_controller::FeasiblePathHandler" base_class_type="nav2_core::PathHandler">
       <description>Prunes the global plan around the robot and transforms the remaining portion into the odom frame for local control</description>

nav2_controller/plugins/axis_goal_checker.cpp

@@ -0,0 +1,200 @@
+// 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 <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()
+: along_path_tolerance_(0.25), cross_track_tolerance_(0.25),
+  path_length_tolerance_(1.0), is_overshoot_valid_(false)
+{
+}
+
+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();
+  logger_ = node->get_logger();
+
+  along_path_tolerance_ = node->declare_or_get_parameter(
+    plugin_name + ".along_path_tolerance", 0.25);
+  cross_track_tolerance_ = node->declare_or_get_parameter(
+    plugin_name + ".cross_track_tolerance", 0.25);
+  path_length_tolerance_ = node->declare_or_get_parameter(
+    plugin_name + ".path_length_tolerance", 1.0);
+  is_overshoot_valid_ = node->declare_or_get_parameter(
+    plugin_name + ".is_overshoot_valid", false);
+
+  // 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 geometry_msgs::msg::Twist &,
+  const nav_msgs::msg::Path & transformed_global_plan)
+{
+  // If the local plan length is longer than the tolerance, we skip the check
+  if (nav2_util::geometry_utils::calculate_path_length(transformed_global_plan) >
+    path_length_tolerance_)
+  {
+    return false;
+  }
+
+  // Check if we have at least 2 poses to determine path direction
+  if (transformed_global_plan.poses.size() >= 2) {
+    // Use axis-aligned goal checking with path direction
+    // Find a pose before goal that is sufficiently far from goal
+    const geometry_msgs::msg::Pose * before_goal_pose_ptr = nullptr;
+    double dx = 0.0;
+    double dy = 0.0;
+
+    for (int i = transformed_global_plan.poses.size() - 2; i >= 0; --i) {
+      const auto & candidate_pose = transformed_global_plan.poses[i].pose;
+      dx = goal_pose.position.x - candidate_pose.position.x;
+      dy = goal_pose.position.y - candidate_pose.position.y;
+      double pose_distance = std::hypot(dx, dy);
+
+      if (pose_distance >= 1e-6) {
+        before_goal_pose_ptr = &candidate_pose;
+        break;
+      }
+    }
+
+    // If all poses are too close to goal, fall back to simple distance check
+    if (!before_goal_pose_ptr) {
+      RCLCPP_DEBUG(
+        logger_,
+        "All poses in path are too close to goal, falling back to simple distance check");
+      double distance_to_goal = std::hypot(
+        goal_pose.position.x - query_pose.position.x,
+        goal_pose.position.y - query_pose.position.y);
+      double tolerance = std::hypot(along_path_tolerance_, cross_track_tolerance_);
+      return distance_to_goal < tolerance;
+    }
+
+    // end of path direction
+    double end_of_path_yaw = atan2(dy, dx);
+
+    // Check if robot is already at goal (would cause atan2(0,0))
+    double robot_to_goal_dx = goal_pose.position.x - query_pose.position.x;
+    double robot_to_goal_dy = goal_pose.position.y - query_pose.position.y;
+    double distance_to_goal = std::hypot(robot_to_goal_dx, robot_to_goal_dy);
+
+    if (distance_to_goal < 1e-6) {
+      return true;  // Robot is at goal
+    }
+
+    double robot_to_goal_yaw = atan2(robot_to_goal_dy, robot_to_goal_dx);
+    double projection_angle = angles::shortest_angular_distance(
+      robot_to_goal_yaw, end_of_path_yaw);
+    double along_path_distance = distance_to_goal * cos(projection_angle);
+    double cross_track_distance = distance_to_goal * sin(projection_angle);
+
+    if (is_overshoot_valid_) {
+      return along_path_distance < along_path_tolerance_ &&
+             fabs(cross_track_distance) < cross_track_tolerance_;
+    } else {
+      return fabs(along_path_distance) < along_path_tolerance_ &&
+             fabs(cross_track_distance) < cross_track_tolerance_;
+    }
+  } else {
+    // Fallback: path has only 1 point, use simple distance check
+    RCLCPP_DEBUG(
+      logger_,
+      "Path has fewer than 2 poses, falling back to simple distance check");
+    double distance_to_goal = std::hypot(
+      goal_pose.position.x - query_pose.position.x,
+      goal_pose.position.y - query_pose.position.y);
+    double tolerance = std::hypot(along_path_tolerance_, cross_track_tolerance_);
+    return distance_to_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 = std::min(along_path_tolerance_, cross_track_tolerance_);
+  pose_tolerance.position.y = std::min(along_path_tolerance_, cross_track_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 (name.find(plugin_name_ + ".") != 0) {
+      continue;
+    }
+    if (type == ParameterType::PARAMETER_DOUBLE) {
+      if (name == plugin_name_ + ".along_path_tolerance") {
+        along_path_tolerance_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".cross_track_tolerance") {
+        cross_track_tolerance_ = parameter.as_double();
+      } else if (name == plugin_name_ + ".path_length_tolerance") {
+        path_length_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/test/CMakeLists.txt

@@ -4,5 +4,8 @@ target_link_libraries(pctest simple_progress_checker pose_progress_checker)
 nav2_add_gtest(gctest goal_checker.cpp)
 target_link_libraries(gctest simple_goal_checker stopped_goal_checker position_goal_checker)
 
+nav2_add_gtest(agctest axis_goal_checker.cpp)
+target_link_libraries(agctest axis_goal_checker)
+
 nav2_add_gtest(phtest path_handler.cpp)
 target_link_libraries(phtest feasible_path_handler)