[DEX][CostmapTopicCollisionChecker] Alternative constructor footprint

Signed-off-by: Guillaume Doisy <[email protected]>

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_;
+  std::shared_ptr<FootprintSubscriber> footprint_sub_;
   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,21 @@ 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)
+{
+  makeFootprintFromString(footprint_string, footprint_);
+}
+
 bool CostmapTopicCollisionChecker::isCollisionFree(
   const geometry_msgs::msg::Pose2D & pose,
   bool fetch_costmap_and_footprint)
@@ -90,8 +101,13 @@ Footprint CostmapTopicCollisionChecker::getFootprint(
 {
   if (fetch_latest_footprint) {
     std_msgs::msg::Header header;
-    if (!footprint_sub_.getFootprintInRobotFrame(footprint_, header)) {
-      throw CollisionCheckerException("Current footprint not available.");
+
+    // if footprint_sub_ was not initialized (alternative constructor), we are using the
+    // footprint built from the footprint_string alternative constructor argument.
+    if (footprint_sub_) {
+      if (!footprint_sub_->getFootprintInRobotFrame(footprint_, header)) {
+        throw CollisionCheckerException("Current footprint not available.");
+      }
     }
   }
   Footprint footprint;