Skip to content

Commit f8f1551

Browse files
doisygGuillaume Doisy
andauthored
[CostmapTopicCollisionChecker] Alternative constructor with footprint string (#4926)
* [CostmapTopicCollisionChecker] Alternative constructor with footprint Signed-off-by: Guillaume Doisy <[email protected]> * raw pointer Signed-off-by: Guillaume Doisy <[email protected]> * suggestions from review Signed-off-by: Guillaume Doisy <[email protected]> --------- Signed-off-by: Guillaume Doisy <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]>
1 parent 5bb0346 commit f8f1551

File tree

2 files changed

+29
-3
lines changed

2 files changed

+29
-3
lines changed

nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,15 @@ class CostmapTopicCollisionChecker
4848
FootprintSubscriber & footprint_sub,
4949
std::string name = "collision_checker");
5050

51+
/**
52+
* @brief Alternative constructor with a footprint string instead of a subscriber. It needs to be
53+
* defined relative to the robot frame
54+
*/
55+
CostmapTopicCollisionChecker(
56+
CostmapSubscriber & costmap_sub,
57+
std::string footprint_string,
58+
std::string name = "collision_checker");
59+
5160
/**
5261
* @brief A destructor
5362
*/
@@ -90,10 +99,11 @@ class CostmapTopicCollisionChecker
9099
// Name used for logging
91100
std::string name_;
92101
CostmapSubscriber & costmap_sub_;
93-
FootprintSubscriber & footprint_sub_;
102+
FootprintSubscriber * footprint_sub_ = nullptr;
94103
FootprintCollisionChecker<std::shared_ptr<Costmap2D>> collision_checker_;
95104
rclcpp::Clock::SharedPtr clock_;
96105
Footprint footprint_;
106+
std::string footprint_string_;
97107
};
98108

99109
} // namespace nav2_costmap_2d

nav2_costmap_2d/src/costmap_topic_collision_checker.cpp

Lines changed: 18 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -38,10 +38,23 @@ CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
3838
std::string name)
3939
: name_(name),
4040
costmap_sub_(costmap_sub),
41-
footprint_sub_(footprint_sub),
41+
footprint_sub_(&footprint_sub),
4242
collision_checker_(nullptr)
4343
{}
4444

45+
CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
46+
CostmapSubscriber & costmap_sub,
47+
std::string footprint_string,
48+
std::string name)
49+
: name_(name),
50+
costmap_sub_(costmap_sub),
51+
collision_checker_(nullptr)
52+
{
53+
if (!makeFootprintFromString(footprint_string, footprint_)) {
54+
throw CollisionCheckerException("Failed to create footprint from string");
55+
}
56+
}
57+
4558
bool CostmapTopicCollisionChecker::isCollisionFree(
4659
const geometry_msgs::msg::Pose2D & pose,
4760
bool fetch_costmap_and_footprint)
@@ -90,7 +103,10 @@ Footprint CostmapTopicCollisionChecker::getFootprint(
90103
{
91104
if (fetch_latest_footprint) {
92105
std_msgs::msg::Header header;
93-
if (!footprint_sub_.getFootprintInRobotFrame(footprint_, header)) {
106+
107+
// if footprint_sub_ was not initialized (alternative constructor), we are using the
108+
// footprint built from the footprint_string alternative constructor argument.
109+
if (footprint_sub_ && !footprint_sub_->getFootprintInRobotFrame(footprint_, header)) {
94110
throw CollisionCheckerException("Current footprint not available.");
95111
}
96112
}

0 commit comments

Comments
 (0)