diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp index 4f645422fdd..c775e8128d7 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp +++ b/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 footprint_sub_; FootprintCollisionChecker> collision_checker_; rclcpp::Clock::SharedPtr clock_; Footprint footprint_; + std::string footprint_string_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp index 11168f63731..d13b1fad2d8 100644 --- a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp +++ b/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;