add path argument to goal checker interface

Signed-off-by: Jonas Otto <[email protected]>

Author: ottojo

nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp

@@ -66,7 +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 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/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/goal_checker.cpp

@@ -39,6 +39,7 @@
 #include "nav2_controller/plugins/simple_goal_checker.hpp"
 #include "nav2_controller/plugins/stopped_goal_checker.hpp"
 #include "nav_2d_utils/conversions.hpp"
+#include "nav_msgs/msg/path.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 
 using nav2_controller::SimpleGoalChecker;
@@ -49,6 +50,7 @@ void checkMacro(
   double x0, double y0, double theta0,
   double x1, double y1, double theta1,
   double xv, double yv, double thetav,
+  const nav_msgs::msg::Path & path,
   bool expected_result)
 {
   gc.reset();
@@ -67,12 +69,12 @@ void checkMacro(
     EXPECT_TRUE(
       gc.isGoalReached(
         nav_2d_utils::pose2DToPose(pose0),
-        nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v)));
+        nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v), path));
   } else {
     EXPECT_FALSE(
       gc.isGoalReached(
         nav_2d_utils::pose2DToPose(pose0),
-        nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v)));
+        nav_2d_utils::pose2DToPose(pose1), nav_2d_utils::twist2Dto3D(v), path));
   }
 }
 
@@ -81,20 +83,22 @@ void sameResult(
   double x0, double y0, double theta0,
   double x1, double y1, double theta1,
   double xv, double yv, double thetav,
+  const nav_msgs::msg::Path & path,
   bool expected_result)
 {
-  checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
-  checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result);
+  checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, expected_result);
+  checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, expected_result);
 }
 
 void trueFalse(
   nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1,
   double x0, double y0, double theta0,
   double x1, double y1, double theta1,
-  double xv, double yv, double thetav)
+  double xv, double yv, double thetav,
+  const nav_msgs::msg::Path & path)
 {
-  checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, true);
-  checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, false);
+  checkMacro(gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, true);
+  checkMacro(gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, path, false);
 }
 class TestLifecycleNode : public nav2_util::LifecycleNode
 {
@@ -162,18 +166,19 @@ TEST(VelocityIterator, two_checks)
   SimpleGoalChecker gc;
   StoppedGoalChecker sgc;
   auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+  nav_msgs::msg::Path path;
 
   gc.initialize(x, "nav2_controller", costmap);
   sgc.initialize(x, "nav2_controller", costmap);
-  sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true);
-  sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false);
-  sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false);
-  sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, false);
-  sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, true);
-  trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0);
-  trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0);
-  trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0);
-  trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1);
+  sameResult(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, path, true);
+  sameResult(gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, path, false);
+  sameResult(gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, path, false);
+  sameResult(gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, path, false);
+  sameResult(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, path, true);
+  trueFalse(gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0, path);
+  trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0, path);
+  trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0, path);
+  trueFalse(gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1, path);
 }
 
 TEST(StoppedGoalChecker, get_tol_and_dynamic_params)

nav2_controller/src/controller_server.cpp

@@ -749,7 +749,7 @@ bool ControllerServer::isGoalReached()
 
   return goal_checkers_[current_goal_checker_]->isGoalReached(
     pose.pose, transformed_end_pose.pose,
-    velocity);
+    velocity, current_path_);
 }
 
 bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose)

nav2_core/include/nav2_core/goal_checker.hpp

@@ -42,6 +42,7 @@
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/twist.hpp"
+#include "nav_msgs/msg/path.hpp"
 
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 
@@ -84,8 +85,10 @@ class GoalChecker
    * @return True if goal is reached
    */
   virtual bool isGoalReached(
-    const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
-    const geometry_msgs::msg::Twist & velocity) = 0;
+    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) = 0;
 
   /**
    * @brief Get the maximum possible tolerances used for goal checking in the major types.

nav2_mppi_controller/test/utils_test.cpp

@@ -53,7 +53,8 @@ class TestGoalChecker : public nav2_core::GoalChecker
   virtual bool isGoalReached(
     const geometry_msgs::msg::Pose & /*query_pose*/,
     const geometry_msgs::msg::Pose & /*goal_pose*/,
-    const geometry_msgs::msg::Twist & /*velocity*/) {return false;}
+    const geometry_msgs::msg::Twist & /*velocity*/,
+    const nav_msgs::msg::Path & /*current_path*/) {return false;}
 
   virtual bool getTolerances(
     geometry_msgs::msg::Pose & pose_tolerance,