Goalchecker path argument and plugin

nav2_controller/CMakeLists.txt

@@ -145,6 +145,28 @@ target_link_libraries(stopped_goal_checker PRIVATE
   pluginlib::pluginlib
 )
 
+
+add_library(path_complete_goal_checker SHARED plugins/path_complete_goal_checker.cpp)
+target_include_directories(path_complete_goal_checker
+  PUBLIC
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+    "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+target_link_libraries(path_complete_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(path_complete_goal_checker PRIVATE
+  angles::angles
+  nav2_util::nav2_util_core
+  pluginlib::pluginlib
+  tf2::tf2
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
@@ -161,7 +183,7 @@ endif()
 
 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 pose_progress_checker simple_goal_checker stopped_goal_checker path_complete_goal_checker ${library_name}
   EXPORT ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -181,6 +203,7 @@ ament_export_libraries(simple_progress_checker
   pose_progress_checker
   simple_goal_checker
   stopped_goal_checker
+  path_complete_goal_checker
   ${library_name})
 ament_export_dependencies(
   geometry_msgs

nav2_controller/include/nav2_controller/plugins/path_complete_goal_checker.hpp

@@ -0,0 +1,84 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, Joseph Duchesne
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef NAV2_CONTROLLER__PLUGINS__PATH_COMPLETE_GOAL_CHECKER_HPP_
+#define NAV2_CONTROLLER__PLUGINS__PATH_COMPLETE_GOAL_CHECKER_HPP_
+
+#include <string>
+#include <memory>
+#include <vector>
+
+#include "nav2_controller/plugins/simple_goal_checker.hpp"
+
+namespace nav2_controller
+{
+
+/**
+ * @class PathCompleteGoalChecker
+ * @brief Goal Checker plugin that checks position delta, once path is shorter than a threshold.
+ *
+ * A
+ */
+class PathCompleteGoalChecker : public SimpleGoalChecker
+{
+public:
+  PathCompleteGoalChecker();
+
+  // Standard GoalChecker Interface
+  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, const nav_msgs::msg::Path & current_path) override;
+
+protected:
+  // threshold for path goal
+  int path_length_tolerance_;
+
+  /**
+   * @brief Callback executed when a paramter 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__PATH_COMPLETE_GOAL_CHECKER_HPP_

nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp

@@ -64,9 +64,11 @@ class SimpleGoalChecker : public nav2_core::GoalChecker
     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;
+    const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override;
+
   bool getTolerances(
     geometry_msgs::msg::Pose & pose_tolerance,
     geometry_msgs::msg::Twist & vel_tolerance) override;

nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp

@@ -61,7 +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 geometry_msgs::msg::Twist & velocity) override;
+    const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path) override;
   bool getTolerances(
     geometry_msgs::msg::Pose & pose_tolerance,
     geometry_msgs::msg::Twist & vel_tolerance) override;

nav2_controller/plugins.xml

@@ -19,4 +19,9 @@
         <description>Checks linear and angular velocity after stopping</description>
     </class>
   </library>
+  <library path="path_complete_goal_checker">
+    <class type="nav2_controller::PathCompleteGoalChecker" base_class_type="nav2_core::GoalChecker">
+        <description>Checks if path has completed enough poses, and then if the pose is within goal window for x, y and yaw</description>
+    </class>
+  </library>
 </class_libraries>

nav2_controller/plugins/path_complete_goal_checker.cpp

@@ -0,0 +1,111 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, Joseph Duchesne
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "nav2_controller/plugins/path_complete_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
+{
+
+PathCompleteGoalChecker::PathCompleteGoalChecker()
+: SimpleGoalChecker(), path_length_tolerance_(1)
+{
+}
+
+void PathCompleteGoalChecker::initialize(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  const std::string & plugin_name,
+  const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  SimpleGoalChecker::initialize(parent, plugin_name, costmap_ros);
+
+  auto node = parent.lock();
+
+  nav2_util::declare_parameter_if_not_declared(
+    node,
+    plugin_name + ".path_length_tolerance",
+    rclcpp::ParameterValue(path_length_tolerance_));
+
+  node->get_parameter(plugin_name + ".path_length_tolerance", path_length_tolerance_);
+
+  // Replace SimpleGoalChecker's callback for dynamic parameters
+  node->remove_on_set_parameters_callback(dyn_params_handler_.get());
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&PathCompleteGoalChecker::dynamicParametersCallback, this, _1));
+}
+
+void PathCompleteGoalChecker::reset()
+{
+  SimpleGoalChecker::reset();
+}
+
+bool PathCompleteGoalChecker::isGoalReached(
+  const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
+  const geometry_msgs::msg::Twist & twist, const nav_msgs::msg::Path & path)
+{
+  // return false if more than path_length_tolerance_ waypoints exist
+  // note: another useful version of this could check path length
+  if (path.poses.size() > (unsigned int)path_length_tolerance_) {
+    return false;
+  }
+  // otherwise defer to SimpleGoalChecker's isGoalReached
+  return SimpleGoalChecker::isGoalReached(query_pose, goal_pose, twist, path);
+}
+
+rcl_interfaces::msg::SetParametersResult
+PathCompleteGoalChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  // call the base class (might be unnessesary since the base class will already bind this event)
+  rcl_interfaces::msg::SetParametersResult result =
+    SimpleGoalChecker::dynamicParametersCallback(parameters);
+  for (auto & parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == ParameterType::PARAMETER_INTEGER) {
+      if (name == plugin_name_ + ".path_length_tolerance") {
+        path_length_tolerance_ = parameter.as_int();
+      }
+    }
+  }
+  return result;
+}
+
+}  // namespace nav2_controller
+
+PLUGINLIB_EXPORT_CLASS(nav2_controller::PathCompleteGoalChecker, nav2_core::GoalChecker)

nav2_controller/plugins/simple_goal_checker.cpp

@@ -97,7 +97,7 @@ void SimpleGoalChecker::reset()
 
 bool SimpleGoalChecker::isGoalReached(
   const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
-  const geometry_msgs::msg::Twist &)
+  const geometry_msgs::msg::Twist &, const nav_msgs::msg::Path &)
 {
   if (check_xy_) {
     double dx = query_pose.position.x - goal_pose.position.x,

nav2_controller/plugins/stopped_goal_checker.cpp

@@ -82,9 +82,9 @@ void StoppedGoalChecker::initialize(
 
 bool StoppedGoalChecker::isGoalReached(
   const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
-  const geometry_msgs::msg::Twist & velocity)
+  const geometry_msgs::msg::Twist & velocity, const nav_msgs::msg::Path & current_path)
 {
-  bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity);
+  bool ret = SimpleGoalChecker::isGoalReached(query_pose, goal_pose, velocity, current_path);
   if (!ret) {
     return ret;
   }

nav2_controller/plugins/test/CMakeLists.txt

@@ -2,4 +2,4 @@ ament_add_gtest(pctest progress_checker.cpp)
 target_link_libraries(pctest simple_progress_checker pose_progress_checker nav_2d_utils::conversions)
 
 ament_add_gtest(gctest goal_checker.cpp)
-target_link_libraries(gctest simple_goal_checker stopped_goal_checker nav_2d_utils::conversions)
+target_link_libraries(gctest simple_goal_checker stopped_goal_checker path_complete_goal_checker nav_2d_utils::conversions)