Added costmap in goal_checker plugins

nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp

@@ -61,7 +61,8 @@ class SimpleGoalChecker : public nav2_core::GoalChecker
   // Standard GoalChecker Interface
   void initialize(
     const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
-    const std::string & plugin_name) override;
+    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,
@@ -79,6 +80,8 @@ class SimpleGoalChecker : public nav2_core::GoalChecker
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   std::string plugin_name_;
 
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+
   /**
    * @brief Callback executed when a paramter change is detected
    * @param parameters list of changed parameters

nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp

@@ -57,7 +57,8 @@ class StoppedGoalChecker : public SimpleGoalChecker
   // Standard GoalChecker Interface
   void initialize(
     const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
-    const std::string & plugin_name) override;
+    const std::string & plugin_name,
+    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;
@@ -71,6 +72,8 @@ class StoppedGoalChecker : public SimpleGoalChecker
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
   std::string plugin_name_;
 
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+
   /**
    * @brief Callback executed when a paramter change is detected
    * @param parameters list of changed parameters

nav2_controller/plugins/simple_goal_checker.cpp

@@ -63,11 +63,14 @@ SimpleGoalChecker::SimpleGoalChecker()
 
 void SimpleGoalChecker::initialize(
   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
-  const std::string & plugin_name)
+  const std::string & plugin_name,
+  const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros)
 {
   plugin_name_ = plugin_name;
   auto node = parent.lock();
 
+  costmap_ros_ = costmap_ros;
+
   nav2_util::declare_parameter_if_not_declared(
     node,
     plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue(0.25));

nav2_controller/plugins/stopped_goal_checker.cpp

@@ -57,13 +57,16 @@ StoppedGoalChecker::StoppedGoalChecker()
 
 void StoppedGoalChecker::initialize(
   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
-  const std::string & plugin_name)
+  const std::string & plugin_name,
+  const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros)
 {
   plugin_name_ = plugin_name;
-  SimpleGoalChecker::initialize(parent, plugin_name);
+  SimpleGoalChecker::initialize(parent, plugin_name, costmap_ros);
 
   auto node = parent.lock();
 
+  costmap_ros_ = costmap_ros;
+
   nav2_util::declare_parameter_if_not_declared(
     node,
     plugin_name + ".rot_stopped_velocity", rclcpp::ParameterValue(0.25));

nav2_controller/plugins/test/goal_checker.cpp

@@ -151,8 +151,10 @@ TEST(VelocityIterator, two_checks)
 
   SimpleGoalChecker gc;
   StoppedGoalChecker sgc;
-  gc.initialize(x, "nav2_controller");
-  sgc.initialize(x, "nav2_controller");
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+
+  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);
@@ -170,8 +172,10 @@ TEST(StoppedGoalChecker, get_tol_and_dynamic_params)
 
   SimpleGoalChecker gc;
   StoppedGoalChecker sgc;
-  sgc.initialize(x, "test");
-  gc.initialize(x, "test2");
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+
+  sgc.initialize(x, "test", costmap);
+  gc.initialize(x, "test2", costmap);
   geometry_msgs::msg::Pose pose_tol;
   geometry_msgs::msg::Twist vel_tol;
 

nav2_controller/src/controller_server.cpp

@@ -146,7 +146,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
       RCLCPP_INFO(
         get_logger(), "Created goal checker : %s of type %s",
         goal_checker_ids_[i].c_str(), goal_checker_types_[i].c_str());
-      goal_checker->initialize(node, goal_checker_ids_[i]);
+      goal_checker->initialize(node, goal_checker_ids_[i], costmap_ros_);
       goal_checkers_.insert({goal_checker_ids_[i], goal_checker});
     } catch (const pluginlib::PluginlibException & ex) {
       RCLCPP_FATAL(

nav2_core/include/nav2_core/goal_checker.hpp

@@ -43,6 +43,9 @@
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+
+
 namespace nav2_core
 {
 
@@ -68,7 +71,9 @@ class GoalChecker
    */
   virtual void initialize(
     const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
-    const std::string & plugin_name) = 0;
+    const std::string & plugin_name,
+    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> &) = 0;
+
   virtual void reset() = 0;
 
   /**

nav2_rotation_shim_controller/test/test_shim_controller.cpp

@@ -263,7 +263,7 @@ TEST(RotationShimControllerTest, computeVelocityTests)
   pose.header.frame_id = "base_link";
   geometry_msgs::msg::Twist velocity;
   nav2_controller::SimpleGoalChecker checker;
-  checker.initialize(node, "checker");
+  checker.initialize(node, "checker", costmap);
 
   // send without setting a path - should go to RPP immediately
   // then it should throw an exception because the path is empty and invalid