#include <obstacle_layer.h>
Public Member Functions | |
virtual void | activate () |
Restart publishers if they've been stopped. | |
void | addStaticObservation (costmap_2d::Observation &obs, bool marking, bool clearing) |
void | clearStaticObservations (bool marking, bool clearing) |
virtual void | deactivate () |
Stop publishers. | |
void | laserScanCallback (const sensor_msgs::LaserScanConstPtr &message, const boost::shared_ptr< costmap_2d::ObservationBuffer > &buffer) |
A callback to handle buffering LaserScan messages. | |
void | laserScanValidInfCallback (const sensor_msgs::LaserScanConstPtr &message, const boost::shared_ptr< ObservationBuffer > &buffer) |
A callback to handle buffering LaserScan messages which need to be filtered to turn Inf values into range_max. | |
ObstacleLayer () | |
virtual void | onInitialize () |
This is called at the end of initialize(). Override to implement subclass-specific initialization. | |
void | pointCloud2Callback (const sensor_msgs::PointCloud2ConstPtr &message, const boost::shared_ptr< costmap_2d::ObservationBuffer > &buffer) |
A callback to handle buffering PointCloud2 messages. | |
void | pointCloudCallback (const sensor_msgs::PointCloudConstPtr &message, const boost::shared_ptr< costmap_2d::ObservationBuffer > &buffer) |
A callback to handle buffering PointCloud messages. | |
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. | |
virtual void | updateCosts (costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) |
Actually update the underlying costmap, only within the bounds calculated during UpdateBounds(). | |
virtual | ~ObstacleLayer () |
Protected Member Functions | |
bool | getClearingObservations (std::vector< costmap_2d::Observation > &clearing_observations) const |
Get the observations used to clear space. | |
bool | getMarkingObservations (std::vector< costmap_2d::Observation > &marking_observations) const |
Get the observations used to mark space. | |
virtual void | onFootprintChanged () |
Overridden from superclass Layer to pass new footprint into footprint_layer_. | |
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. | |
virtual void | setupDynamicReconfigure (ros::NodeHandle &nh) |
void | updateRaytraceBounds (double ox, double oy, double wx, double wy, double range, double *min_x, double *min_y, double *max_x, double *max_y) |
Protected Attributes | |
std::vector< boost::shared_ptr < costmap_2d::ObservationBuffer > > | clearing_buffers_ |
Used to store observation buffers used for clearing obstacles. | |
int | combination_method_ |
dynamic_reconfigure::Server < costmap_2d::ObstaclePluginConfig > * | dsrv_ |
FootprintLayer | footprint_layer_ |
clears the footprint in this obstacle layer. | |
std::string | global_frame_ |
The global frame for the costmap. | |
std::vector< boost::shared_ptr < costmap_2d::ObservationBuffer > > | marking_buffers_ |
Used to store observation buffers used for marking obstacles. | |
double | max_obstacle_height_ |
Max Obstacle Height. | |
std::vector< boost::shared_ptr < costmap_2d::ObservationBuffer > > | observation_buffers_ |
Used to store observations from various sensors. | |
std::vector< boost::shared_ptr < tf::MessageFilterBase > > | observation_notifiers_ |
Used to make sure that transforms are available for each sensor. | |
std::vector< boost::shared_ptr < message_filters::SubscriberBase > > | observation_subscribers_ |
Used for the observation message filters. | |
laser_geometry::LaserProjection | projector_ |
Used to project laser scans into point clouds. | |
bool | rolling_window_ |
std::vector < costmap_2d::Observation > | static_clearing_observations_ |
std::vector < costmap_2d::Observation > | static_marking_observations_ |
Private Member Functions | |
void | reconfigureCB (costmap_2d::ObstaclePluginConfig &config, uint32_t level) |
Definition at line 62 of file obstacle_layer.h.
costmap_2d::ObstacleLayer::ObstacleLayer | ( | ) | [inline] |
Definition at line 65 of file obstacle_layer.h.
costmap_2d::ObstacleLayer::~ObstacleLayer | ( | ) | [virtual] |
Definition at line 239 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::activate | ( | ) | [virtual] |
Restart publishers if they've been stopped.
Reimplemented from costmap_2d::Layer.
Definition at line 554 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::addStaticObservation | ( | costmap_2d::Observation & | obs, |
bool | marking, | ||
bool | clearing | ||
) |
Definition at line 427 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::clearStaticObservations | ( | bool | marking, |
bool | clearing | ||
) |
Definition at line 435 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::deactivate | ( | ) | [virtual] |
Stop publishers.
Reimplemented from costmap_2d::Layer.
Definition at line 569 of file obstacle_layer.cpp.
bool costmap_2d::ObstacleLayer::getClearingObservations | ( | std::vector< costmap_2d::Observation > & | clearing_observations | ) | const [protected] |
Get the observations used to clear space.
clearing_observations | A reference to a vector that will be populated with the observations |
Definition at line 458 of file obstacle_layer.cpp.
bool costmap_2d::ObstacleLayer::getMarkingObservations | ( | std::vector< costmap_2d::Observation > & | marking_observations | ) | const [protected] |
Get the observations used to mark space.
marking_observations | A reference to a vector that will be populated with the observations |
Definition at line 442 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::laserScanCallback | ( | const sensor_msgs::LaserScanConstPtr & | message, |
const boost::shared_ptr< costmap_2d::ObservationBuffer > & | buffer | ||
) |
A callback to handle buffering LaserScan messages.
message | The message returned from a message notifier |
buffer | A pointer to the observation buffer to update |
Definition at line 251 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::laserScanValidInfCallback | ( | const sensor_msgs::LaserScanConstPtr & | message, |
const boost::shared_ptr< ObservationBuffer > & | buffer | ||
) |
A callback to handle buffering LaserScan messages which need to be filtered to turn Inf values into range_max.
message | The message returned from a message notifier |
buffer | A pointer to the observation buffer to update |
Definition at line 276 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::onFootprintChanged | ( | ) | [protected, virtual] |
Overridden from superclass Layer to pass new footprint into footprint_layer_.
Reimplemented from costmap_2d::Layer.
Definition at line 596 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::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::Layer.
Reimplemented in costmap_2d::VoxelLayer.
Definition at line 54 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::pointCloud2Callback | ( | const sensor_msgs::PointCloud2ConstPtr & | message, |
const boost::shared_ptr< costmap_2d::ObservationBuffer > & | buffer | ||
) |
A callback to handle buffering PointCloud2 messages.
message | The message returned from a message notifier |
buffer | A pointer to the observation buffer to update |
Definition at line 328 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::pointCloudCallback | ( | const sensor_msgs::PointCloudConstPtr & | message, |
const boost::shared_ptr< costmap_2d::ObservationBuffer > & | buffer | ||
) |
A callback to handle buffering PointCloud messages.
message | The message returned from a message notifier |
buffer | A pointer to the observation buffer to update |
Definition at line 311 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::raytraceFreespace | ( | const costmap_2d::Observation & | clearing_observation, |
double * | min_x, | ||
double * | min_y, | ||
double * | max_x, | ||
double * | max_y | ||
) | [protected, virtual] |
Clear freespace based on one observation.
clearing_observation | The observation used to raytrace |
min_x | |
min_y | |
max_x | |
max_y |
Reimplemented in costmap_2d::VoxelLayer.
Definition at line 474 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::reconfigureCB | ( | costmap_2d::ObstaclePluginConfig & | config, |
uint32_t | level | ||
) | [private] |
Definition at line 244 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::reset | ( | ) | [virtual] |
Reimplemented from costmap_2d::Layer.
Reimplemented in costmap_2d::VoxelLayer.
Definition at line 588 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::setupDynamicReconfigure | ( | ros::NodeHandle & | nh | ) | [protected, virtual] |
Reimplemented in costmap_2d::VoxelLayer.
Definition at line 231 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::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::Layer.
Reimplemented in costmap_2d::VoxelLayer.
Definition at line 337 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::updateCosts | ( | costmap_2d::Costmap2D & | master_grid, |
int | min_i, | ||
int | min_j, | ||
int | max_i, | ||
int | max_j | ||
) | [virtual] |
Actually update the underlying costmap, only within the bounds calculated during UpdateBounds().
Reimplemented from costmap_2d::Layer.
Definition at line 412 of file obstacle_layer.cpp.
void costmap_2d::ObstacleLayer::updateRaytraceBounds | ( | double | ox, |
double | oy, | ||
double | wx, | ||
double | wy, | ||
double | range, | ||
double * | min_x, | ||
double * | min_y, | ||
double * | max_x, | ||
double * | max_y | ||
) | [protected] |
Definition at line 578 of file obstacle_layer.cpp.
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > costmap_2d::ObstacleLayer::clearing_buffers_ [protected] |
Used to store observation buffers used for clearing obstacles.
Definition at line 160 of file obstacle_layer.h.
int costmap_2d::ObstacleLayer::combination_method_ [protected] |
Definition at line 170 of file obstacle_layer.h.
dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>* costmap_2d::ObstacleLayer::dsrv_ [protected] |
Reimplemented in costmap_2d::VoxelLayer.
Definition at line 166 of file obstacle_layer.h.
clears the footprint in this obstacle layer.
Definition at line 168 of file obstacle_layer.h.
std::string costmap_2d::ObstacleLayer::global_frame_ [protected] |
The global frame for the costmap.
Definition at line 151 of file obstacle_layer.h.
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > costmap_2d::ObstacleLayer::marking_buffers_ [protected] |
Used to store observation buffers used for marking obstacles.
Definition at line 159 of file obstacle_layer.h.
double costmap_2d::ObstacleLayer::max_obstacle_height_ [protected] |
Max Obstacle Height.
Definition at line 152 of file obstacle_layer.h.
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > costmap_2d::ObstacleLayer::observation_buffers_ [protected] |
Used to store observations from various sensors.
Definition at line 158 of file obstacle_layer.h.
std::vector<boost::shared_ptr<tf::MessageFilterBase> > costmap_2d::ObstacleLayer::observation_notifiers_ [protected] |
Used to make sure that transforms are available for each sensor.
Definition at line 157 of file obstacle_layer.h.
std::vector<boost::shared_ptr<message_filters::SubscriberBase> > costmap_2d::ObstacleLayer::observation_subscribers_ [protected] |
Used for the observation message filters.
Definition at line 156 of file obstacle_layer.h.
Used to project laser scans into point clouds.
Definition at line 154 of file obstacle_layer.h.
bool costmap_2d::ObstacleLayer::rolling_window_ [protected] |
Definition at line 165 of file obstacle_layer.h.
std::vector<costmap_2d::Observation> costmap_2d::ObstacleLayer::static_clearing_observations_ [protected] |
Definition at line 163 of file obstacle_layer.h.
std::vector<costmap_2d::Observation> costmap_2d::ObstacleLayer::static_marking_observations_ [protected] |
Definition at line 163 of file obstacle_layer.h.