[CostmapTopicCollisionChecker] Alternative constructor with footprint string (ros-navigation#4926)

* [CostmapTopicCollisionChecker] Alternative constructor with footprint

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* raw pointer

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* suggestions from review

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

---------

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

Author: 2 people

nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp

@@ -48,6 +48,15 @@ class CostmapTopicCollisionChecker
     FootprintSubscriber & footprint_sub,
     std::string name = "collision_checker");
 
+  /**
+   * @brief Alternative constructor with a footprint string instead of a subscriber. It needs to be
+   * defined relative to the robot frame
+   */
+  CostmapTopicCollisionChecker(
+    CostmapSubscriber & costmap_sub,
+    std::string footprint_string,
+    std::string name = "collision_checker");
+
   /**
    * @brief A destructor
    */
@@ -90,10 +99,11 @@ class CostmapTopicCollisionChecker
   // Name used for logging
   std::string name_;
   CostmapSubscriber & costmap_sub_;
-  FootprintSubscriber & footprint_sub_;
+  FootprintSubscriber * footprint_sub_ = nullptr;
   FootprintCollisionChecker<std::shared_ptr<Costmap2D>> collision_checker_;
   rclcpp::Clock::SharedPtr clock_;
   Footprint footprint_;
+  std::string footprint_string_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/src/costmap_topic_collision_checker.cpp

@@ -38,10 +38,23 @@ CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
   std::string name)
 : name_(name),
   costmap_sub_(costmap_sub),
-  footprint_sub_(footprint_sub),
+  footprint_sub_(&footprint_sub),
   collision_checker_(nullptr)
 {}
 
+CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
+  CostmapSubscriber & costmap_sub,
+  std::string footprint_string,
+  std::string name)
+: name_(name),
+  costmap_sub_(costmap_sub),
+  collision_checker_(nullptr)
+{
+  if (!makeFootprintFromString(footprint_string, footprint_)) {
+    throw CollisionCheckerException("Failed to create footprint from string");
+  }
+}
+
 bool CostmapTopicCollisionChecker::isCollisionFree(
   const geometry_msgs::msg::Pose2D & pose,
   bool fetch_costmap_and_footprint)
@@ -90,7 +103,10 @@ Footprint CostmapTopicCollisionChecker::getFootprint(
 {
   if (fetch_latest_footprint) {
     std_msgs::msg::Header header;
-    if (!footprint_sub_.getFootprintInRobotFrame(footprint_, header)) {
+
+    // if footprint_sub_ was not initialized (alternative constructor), we are using the
+    // footprint built from the footprint_string alternative constructor argument.
+    if (footprint_sub_ && !footprint_sub_->getFootprintInRobotFrame(footprint_, header)) {
       throw CollisionCheckerException("Current footprint not available.");
     }
   }