Added costmap in goal_checker plugins (#2965)

* Added costmap in the goal checker * Added costmap in goal_checker * Adressed comments in PR * Fixed linting error
ros-navigation · May 27, 2022 · 32d1004 · 32d1004
1 parent 35561ba
commit 32d1004
Show file tree

Hide file tree

Showing 8 changed files with 25 additions and 12 deletions.
diff --git a/nav2_controller/include/nav2_controller/plugins/simple_goal_checker.hpp b/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,

diff --git a/nav2_controller/include/nav2_controller/plugins/stopped_goal_checker.hpp b/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;

diff --git a/nav2_controller/plugins/simple_goal_checker.cpp b/nav2_controller/plugins/simple_goal_checker.cpp
@@ -63,7 +63,8 @@ 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();

diff --git a/nav2_controller/plugins/stopped_goal_checker.cpp b/nav2_controller/plugins/stopped_goal_checker.cpp
@@ -57,10 +57,11 @@ 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();
 

diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/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;
 

diff --git a/nav2_controller/src/controller_server.cpp b/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(

diff --git a/nav2_core/include/nav2_core/goal_checker.hpp b/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> costmap_ros) = 0;
+
   virtual void reset() = 0;
 
   /**

diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/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