@@ -38,10 +38,23 @@ CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
38
38
std::string name)
39
39
: name_(name),
40
40
costmap_sub_ (costmap_sub),
41
- footprint_sub_(footprint_sub),
41
+ footprint_sub_(& footprint_sub),
42
42
collision_checker_(nullptr )
43
43
{}
44
44
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
+
45
58
bool CostmapTopicCollisionChecker::isCollisionFree (
46
59
const geometry_msgs::msg::Pose2D & pose,
47
60
bool fetch_costmap_and_footprint)
@@ -90,7 +103,10 @@ Footprint CostmapTopicCollisionChecker::getFootprint(
90
103
{
91
104
if (fetch_latest_footprint) {
92
105
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)) {
94
110
throw CollisionCheckerException (" Current footprint not available." );
95
111
}
96
112
}
0 commit comments