Skip to content

Commit

Permalink
Remove unorientFootprint function dependency
Browse files Browse the repository at this point in the history
  • Loading branch information
shivaang12 committed May 13, 2020
1 parent 0896059 commit 4bb1022
Showing 1 changed file with 4 additions and 4 deletions.
8 changes: 4 additions & 4 deletions nav2_costmap_2d/src/costmap_topic_collision_checker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -85,16 +85,16 @@ double CostmapTopicCollisionChecker::scorePose(
return collision_checker_.footprintCost(getFootprint(pose));
}

Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose)
Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & /*pose*/)
{
Footprint footprint;
if (!footprint_sub_.getFootprint(footprint)) {
throw CollisionCheckerException("Current footprint not available.");
}

Footprint footprint_spec;
unorientFootprint(footprint, footprint_spec);
transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint);
// Footprint footprint_spec;
// unorientFootprint(footprint, footprint_spec);
// transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint);

return footprint;
}
Expand Down

0 comments on commit 4bb1022

Please sign in to comment.