Simple single-state collision checker. More...
#include <collision_checker.hpp>
Public Member Functions | |
CollisionChecker (costmap_2d::Costmap2DROS *costmap_ros) | |
Initialise the checker with a costmap. More... | |
bool | inCollision (const float &x, const float &y, const float &yaw) |
Check for collision along the boundaries of the footprint. More... | |
Private Attributes | |
double | circumscribed_radius_ |
base_local_planner::CostmapModel | costmap_model_ |
costmap_2d::Costmap2DROS * | costmap_ros_ |
std::vector< geometry_msgs::Point > | footprint_ |
double | inscribed_radius_ |
Simple single-state collision checker.
Assumptions:
This may/should merge with the collision checker in the crrt local planner, however that is currently more complicated and entangled with the concept of a trajectory state.
Definition at line 39 of file collision_checker.hpp.
yocs_navi_toolkit::CollisionChecker::CollisionChecker | ( | costmap_2d::Costmap2DROS * | costmap_ros | ) |
Initialise the checker with a costmap.
costmap_ros | : usually either the local or global costmap |
Definition at line 21 of file collision_checker.cpp.
bool yocs_navi_toolkit::CollisionChecker::inCollision | ( | const float & | x, |
const float & | y, | ||
const float & | yaw | ||
) |
Check for collision along the boundaries of the footprint.
Keep in mind that it is only checking along the boundaries. This means that you can get false positives if you just carelessly look for collisions at any convenient point on the costmap as the boundaries might be fine, but there is an unlooked for collision inside the boundary of the footprint.
Definition at line 31 of file collision_checker.cpp.
|
private |
Definition at line 61 of file collision_checker.hpp.
|
private |
Definition at line 59 of file collision_checker.hpp.
|
private |
Definition at line 58 of file collision_checker.hpp.
|
private |
Definition at line 60 of file collision_checker.hpp.
|
private |
Definition at line 61 of file collision_checker.hpp.