#include <voxel_layer.h>
Public Member Functions | |
bool | isDiscretized () |
virtual void | matchSize () |
Implement this to make this layer match the size of the parent costmap. | |
virtual void | onInitialize () |
This is called at the end of initialize(). Override to implement subclass-specific initialization. | |
virtual void | reset () |
virtual void | updateBounds (double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y) |
This is called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to update. Each layer can increase the size of this bounds. | |
void | updateOrigin (double new_origin_x, double new_origin_y) |
Move the origin of the costmap to a new location.... keeping data when it can. | |
VoxelLayer () | |
virtual | ~VoxelLayer () |
Protected Member Functions | |
virtual void | resetMaps () |
Resets the costmap and static_map to be unknown space. | |
virtual void | setupDynamicReconfigure (ros::NodeHandle &nh) |
Private Member Functions | |
void | clearNonLethal (double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info) |
double | dist (double x0, double y0, double z0, double x1, double y1, double z1) |
void | mapToWorld3D (unsigned int mx, unsigned int my, unsigned int mz, double &wx, double &wy, double &wz) |
virtual void | raytraceFreespace (const costmap_2d::Observation &clearing_observation, double *min_x, double *min_y, double *max_x, double *max_y) |
Clear freespace based on one observation. | |
void | reconfigureCB (costmap_2d::VoxelPluginConfig &config, uint32_t level) |
bool | worldToMap3D (double wx, double wy, double wz, unsigned int &mx, unsigned int &my, unsigned int &mz) |
bool | worldToMap3DFloat (double wx, double wy, double wz, double &mx, double &my, double &mz) |
Private Attributes | |
sensor_msgs::PointCloud | clearing_endpoints_ |
ros::Publisher | clearing_endpoints_pub_ |
unsigned int | mark_threshold_ |
double | origin_z_ |
bool | publish_voxel_ |
unsigned int | size_z_ |
unsigned int | unknown_threshold_ |
dynamic_reconfigure::Server < costmap_2d::VoxelPluginConfig > * | voxel_dsrv_ |
voxel_grid::VoxelGrid | voxel_grid_ |
ros::Publisher | voxel_pub_ |
double | z_resolution_ |
Definition at line 62 of file voxel_layer.h.
costmap_2d::VoxelLayer::VoxelLayer | ( | ) | [inline] |
Definition at line 65 of file voxel_layer.h.
costmap_2d::VoxelLayer::~VoxelLayer | ( | ) | [virtual] |
Definition at line 75 of file voxel_layer.cpp.
void costmap_2d::VoxelLayer::clearNonLethal | ( | double | wx, |
double | wy, | ||
double | w_size_x, | ||
double | w_size_y, | ||
bool | clear_no_info | ||
) | [private] |
Definition at line 215 of file voxel_layer.cpp.
double costmap_2d::VoxelLayer::dist | ( | double | x0, |
double | y0, | ||
double | z0, | ||
double | x1, | ||
double | y1, | ||
double | z1 | ||
) | [inline, private] |
Definition at line 143 of file voxel_layer.h.
bool costmap_2d::VoxelLayer::isDiscretized | ( | ) | [inline] |
Reimplemented from costmap_2d::CostmapLayer.
Definition at line 78 of file voxel_layer.h.
void costmap_2d::VoxelLayer::mapToWorld3D | ( | unsigned int | mx, |
unsigned int | my, | ||
unsigned int | mz, | ||
double & | wx, | ||
double & | wy, | ||
double & | wz | ||
) | [inline, private] |
Definition at line 135 of file voxel_layer.h.
void costmap_2d::VoxelLayer::matchSize | ( | ) | [virtual] |
Implement this to make this layer match the size of the parent costmap.
Reimplemented from costmap_2d::CostmapLayer.
Definition at line 95 of file voxel_layer.cpp.
void costmap_2d::VoxelLayer::onInitialize | ( | ) | [virtual] |
This is called at the end of initialize(). Override to implement subclass-specific initialization.
tf_, name_, and layered_costmap_ will all be set already when this is called.
Reimplemented from costmap_2d::ObstacleLayer.
Definition at line 55 of file voxel_layer.cpp.
void costmap_2d::VoxelLayer::raytraceFreespace | ( | const costmap_2d::Observation & | clearing_observation, |
double * | min_x, | ||
double * | min_y, | ||
double * | max_x, | ||
double * | max_y | ||
) | [private, virtual] |
Clear freespace based on one observation.
clearing_observation | The observation used to raytrace |
min_x | |
min_y | |
max_x | |
max_y |
Reimplemented from costmap_2d::ObstacleLayer.
Definition at line 266 of file voxel_layer.cpp.
void costmap_2d::VoxelLayer::reconfigureCB | ( | costmap_2d::VoxelPluginConfig & | config, |
uint32_t | level | ||
) | [private] |
Definition at line 81 of file voxel_layer.cpp.
void costmap_2d::VoxelLayer::reset | ( | ) | [virtual] |
Reimplemented from costmap_2d::ObstacleLayer.
Definition at line 102 of file voxel_layer.cpp.
void costmap_2d::VoxelLayer::resetMaps | ( | ) | [protected, virtual] |
Resets the costmap and static_map to be unknown space.
Reimplemented from costmap_2d::Costmap2D.
Definition at line 110 of file voxel_layer.cpp.
void costmap_2d::VoxelLayer::setupDynamicReconfigure | ( | ros::NodeHandle & | nh | ) | [protected, virtual] |
Reimplemented from costmap_2d::ObstacleLayer.
Definition at line 67 of file voxel_layer.cpp.
void costmap_2d::VoxelLayer::updateBounds | ( | double | robot_x, |
double | robot_y, | ||
double | robot_yaw, | ||
double * | min_x, | ||
double * | min_y, | ||
double * | max_x, | ||
double * | max_y | ||
) | [virtual] |
This is called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to update. Each layer can increase the size of this bounds.
For more details, see "Layered Costmaps for Context-Sensitive Navigation", by Lu et. Al, IROS 2014.
Reimplemented from costmap_2d::ObstacleLayer.
Definition at line 116 of file voxel_layer.cpp.
void costmap_2d::VoxelLayer::updateOrigin | ( | double | new_origin_x, |
double | new_origin_y | ||
) | [virtual] |
Move the origin of the costmap to a new location.... keeping data when it can.
new_origin_x | The x coordinate of the new origin |
new_origin_y | The y coordinate of the new origin |
Reimplemented from costmap_2d::Costmap2D.
Definition at line 385 of file voxel_layer.cpp.
bool costmap_2d::VoxelLayer::worldToMap3D | ( | double | wx, |
double | wy, | ||
double | wz, | ||
unsigned int & | mx, | ||
unsigned int & | my, | ||
unsigned int & | mz | ||
) | [inline, private] |
Definition at line 120 of file voxel_layer.h.
bool costmap_2d::VoxelLayer::worldToMap3DFloat | ( | double | wx, |
double | wy, | ||
double | wz, | ||
double & | mx, | ||
double & | my, | ||
double & | mz | ||
) | [inline, private] |
Definition at line 107 of file voxel_layer.h.
Definition at line 105 of file voxel_layer.h.
Definition at line 104 of file voxel_layer.h.
unsigned int costmap_2d::VoxelLayer::mark_threshold_ [private] |
Definition at line 103 of file voxel_layer.h.
double costmap_2d::VoxelLayer::origin_z_ [private] |
Definition at line 102 of file voxel_layer.h.
bool costmap_2d::VoxelLayer::publish_voxel_ [private] |
Definition at line 99 of file voxel_layer.h.
unsigned int costmap_2d::VoxelLayer::size_z_ [private] |
Definition at line 103 of file voxel_layer.h.
unsigned int costmap_2d::VoxelLayer::unknown_threshold_ [private] |
Definition at line 103 of file voxel_layer.h.
dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>* costmap_2d::VoxelLayer::voxel_dsrv_ [private] |
Definition at line 97 of file voxel_layer.h.
Definition at line 101 of file voxel_layer.h.
Definition at line 100 of file voxel_layer.h.
double costmap_2d::VoxelLayer::z_resolution_ [private] |
Definition at line 102 of file voxel_layer.h.