Public Member Functions | Protected Member Functions | Protected Attributes | Private Member Functions | List of all members
costmap_2d::ObstacleLayer Class Reference

#include <obstacle_layer.h>

Inheritance diagram for costmap_2d::ObstacleLayer:
Inheritance graph
[legend]

Public Member Functions

virtual void activate ()
 Restart publishers if they've been stopped. More...
 
void addStaticObservation (costmap_2d::Observation &obs, bool marking, bool clearing)
 
void clearStaticObservations (bool marking, bool clearing)
 
virtual void deactivate ()
 Stop publishers. More...
 
void laserScanCallback (const sensor_msgs::LaserScanConstPtr &message, const boost::shared_ptr< costmap_2d::ObservationBuffer > &buffer)
 A callback to handle buffering LaserScan messages. More...
 
void laserScanValidInfCallback (const sensor_msgs::LaserScanConstPtr &message, const boost::shared_ptr< ObservationBuffer > &buffer)
 A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max. More...
 
 ObstacleLayer ()
 
virtual void onInitialize ()
 This is called at the end of initialize(). Override to implement subclass-specific initialization. More...
 
void pointCloud2Callback (const sensor_msgs::PointCloud2ConstPtr &message, const boost::shared_ptr< costmap_2d::ObservationBuffer > &buffer)
 A callback to handle buffering PointCloud2 messages. More...
 
void pointCloudCallback (const sensor_msgs::PointCloudConstPtr &message, const boost::shared_ptr< costmap_2d::ObservationBuffer > &buffer)
 A callback to handle buffering PointCloud messages. More...
 
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. More...
 
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(). More...
 
virtual ~ObstacleLayer ()
 
- Public Member Functions inherited from costmap_2d::CostmapLayer
void addExtraBounds (double mx0, double my0, double mx1, double my1)
 
 CostmapLayer ()
 
bool isDiscretized ()
 
virtual void matchSize ()
 Implement this to make this layer match the size of the parent costmap. More...
 
- Public Member Functions inherited from costmap_2d::Layer
const std::vector< geometry_msgs::Point > & getFootprint () const
 Convenience function for layered_costmap_->getFootprint(). More...
 
std::string getName () const
 
void initialize (LayeredCostmap *parent, std::string name, tf::TransformListener *tf)
 
bool isCurrent () const
 Check to make sure all the data in the layer is up to date. If the layer is not up to date, then it may be unsafe to plan using the data from this layer, and the planner may need to know. More...
 
 Layer ()
 
virtual void onFootprintChanged ()
 LayeredCostmap calls this whenever the footprint there changes (via LayeredCostmap::setFootprint()). Override to be notified of changes to the robot's footprint. More...
 
virtual ~Layer ()
 
- Public Member Functions inherited from costmap_2d::Costmap2D
unsigned int cellDistance (double world_dist)
 Given distance in the world... convert it to cells. More...
 
void convexFillCells (const std::vector< MapLocation > &polygon, std::vector< MapLocation > &polygon_cells)
 Get the map cells that fill a convex polygon. More...
 
bool copyCostmapWindow (const Costmap2D &map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y)
 Turn this costmap into a copy of a window of a costmap passed in. More...
 
 Costmap2D (unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, double origin_y, unsigned char default_value=0)
 Constructor for a costmap. More...
 
 Costmap2D (const Costmap2D &map)
 Copy constructor for a costmap, creates a copy efficiently. More...
 
 Costmap2D ()
 Default constructor. More...
 
unsigned char * getCharMap () const
 Will return a pointer to the underlying unsigned char array used as the costmap. More...
 
unsigned char getCost (unsigned int mx, unsigned int my) const
 Get the cost of a cell in the costmap. More...
 
unsigned char getDefaultValue ()
 
unsigned int getIndex (unsigned int mx, unsigned int my) const
 Given two map coordinates... compute the associated index. More...
 
mutex_tgetMutex ()
 
double getOriginX () const
 Accessor for the x origin of the costmap. More...
 
double getOriginY () const
 Accessor for the y origin of the costmap. More...
 
double getResolution () const
 Accessor for the resolution of the costmap. More...
 
unsigned int getSizeInCellsX () const
 Accessor for the x size of the costmap in cells. More...
 
unsigned int getSizeInCellsY () const
 Accessor for the y size of the costmap in cells. More...
 
double getSizeInMetersX () const
 Accessor for the x size of the costmap in meters. More...
 
double getSizeInMetersY () const
 Accessor for the y size of the costmap in meters. More...
 
void indexToCells (unsigned int index, unsigned int &mx, unsigned int &my) const
 Given an index... compute the associated map coordinates. More...
 
void mapToWorld (unsigned int mx, unsigned int my, double &wx, double &wy) const
 Convert from map coordinates to world coordinates. More...
 
Costmap2Doperator= (const Costmap2D &map)
 Overloaded assignment operator. More...
 
void polygonOutlineCells (const std::vector< MapLocation > &polygon, std::vector< MapLocation > &polygon_cells)
 Get the map cells that make up the outline of a polygon. More...
 
void resetMap (unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
 
void resizeMap (unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y)
 
bool saveMap (std::string file_name)
 Save the costmap out to a pgm file. More...
 
bool setConvexPolygonCost (const std::vector< geometry_msgs::Point > &polygon, unsigned char cost_value)
 Sets the cost of a convex polygon to a desired value. More...
 
void setCost (unsigned int mx, unsigned int my, unsigned char cost)
 Set the cost of a cell in the costmap. More...
 
void setDefaultValue (unsigned char c)
 
virtual 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. More...
 
bool worldToMap (double wx, double wy, unsigned int &mx, unsigned int &my) const
 Convert from world coordinates to map coordinates. More...
 
void worldToMapEnforceBounds (double wx, double wy, int &mx, int &my) const
 Convert from world coordinates to map coordinates, constraining results to legal bounds. More...
 
void worldToMapNoBounds (double wx, double wy, int &mx, int &my) const
 Convert from world coordinates to map coordinates without checking for legal bounds. More...
 
virtual ~Costmap2D ()
 Destructor. More...
 

Protected Member Functions

bool getClearingObservations (std::vector< costmap_2d::Observation > &clearing_observations) const
 Get the observations used to clear space. More...
 
bool getMarkingObservations (std::vector< costmap_2d::Observation > &marking_observations) const
 Get the observations used to mark space. More...
 
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. More...
 
virtual void setupDynamicReconfigure (ros::NodeHandle &nh)
 
void updateFootprint (double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
 
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 Member Functions inherited from costmap_2d::CostmapLayer
void touch (double x, double y, double *min_x, double *min_y, double *max_x, double *max_y)
 
void updateWithAddition (costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
 
void updateWithMax (costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
 
void updateWithOverwrite (costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
 
void updateWithTrueOverwrite (costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
 
void useExtraBounds (double *min_x, double *min_y, double *max_x, double *max_y)
 
- Protected Member Functions inherited from costmap_2d::Costmap2D
template<typename data_type >
void copyMapRegion (data_type *source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, unsigned int sm_size_x, data_type *dest_map, unsigned int dm_lower_left_x, unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x, unsigned int region_size_y)
 Copy a region of a source map into a destination map. More...
 
virtual void deleteMaps ()
 Deletes the costmap, static_map, and markers data structures. More...
 
virtual void initMaps (unsigned int size_x, unsigned int size_y)
 Initializes the costmap, static_map, and markers data structures. More...
 
template<class ActionType >
void raytraceLine (ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int max_length=UINT_MAX)
 Raytrace a line and apply some action at each step. More...
 
virtual void resetMaps ()
 Resets the costmap and static_map to be unknown space. More...
 

Protected Attributes

std::vector< boost::shared_ptr< costmap_2d::ObservationBuffer > > clearing_buffers_
 Used to store observation buffers used for clearing obstacles. More...
 
int combination_method_
 
dynamic_reconfigure::Server< costmap_2d::ObstaclePluginConfig > * dsrv_
 
bool footprint_clearing_enabled_
 
std::string global_frame_
 The global frame for the costmap. More...
 
std::vector< boost::shared_ptr< costmap_2d::ObservationBuffer > > marking_buffers_
 Used to store observation buffers used for marking obstacles. More...
 
double max_obstacle_height_
 Max Obstacle Height. More...
 
std::vector< boost::shared_ptr< costmap_2d::ObservationBuffer > > observation_buffers_
 Used to store observations from various sensors. More...
 
std::vector< boost::shared_ptr< tf::MessageFilterBase > > observation_notifiers_
 Used to make sure that transforms are available for each sensor. More...
 
std::vector< boost::shared_ptr< message_filters::SubscriberBase > > observation_subscribers_
 Used for the observation message filters. More...
 
laser_geometry::LaserProjection projector_
 Used to project laser scans into point clouds. More...
 
bool rolling_window_
 
std::vector< costmap_2d::Observationstatic_clearing_observations_
 
std::vector< costmap_2d::Observationstatic_marking_observations_
 
std::vector< geometry_msgs::Pointtransformed_footprint_
 
- Protected Attributes inherited from costmap_2d::CostmapLayer
bool has_extra_bounds_
 
- Protected Attributes inherited from costmap_2d::Layer
bool current_
 
bool enabled_
 Currently this var is managed by subclasses. TODO: make this managed by this class and/or container class. More...
 
LayeredCostmaplayered_costmap_
 
std::string name_
 
tf::TransformListenertf_
 
- Protected Attributes inherited from costmap_2d::Costmap2D
unsigned char * costmap_
 
unsigned char default_value_
 
double origin_x_
 
double origin_y_
 
double resolution_
 
unsigned int size_x_
 
unsigned int size_y_
 

Private Member Functions

void reconfigureCB (costmap_2d::ObstaclePluginConfig &config, uint32_t level)
 

Additional Inherited Members

- Public Types inherited from costmap_2d::Costmap2D
typedef boost::recursive_mutex mutex_t
 

Detailed Description

Definition at line 62 of file obstacle_layer.h.

Constructor & Destructor Documentation

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.

Member Function Documentation

void costmap_2d::ObstacleLayer::activate ( )
virtual

Restart publishers if they've been stopped.

Reimplemented from costmap_2d::Layer.

Definition at line 578 of file obstacle_layer.cpp.

void costmap_2d::ObstacleLayer::addStaticObservation ( costmap_2d::Observation obs,
bool  marking,
bool  clearing 
)

Definition at line 450 of file obstacle_layer.cpp.

void costmap_2d::ObstacleLayer::clearStaticObservations ( bool  marking,
bool  clearing 
)

Definition at line 458 of file obstacle_layer.cpp.

void costmap_2d::ObstacleLayer::deactivate ( )
virtual

Stop publishers.

Reimplemented from costmap_2d::Layer.

Definition at line 593 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.

Parameters
clearing_observationsA reference to a vector that will be populated with the observations
Returns
True if all the observation buffers are current, false otherwise

Definition at line 482 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.

Parameters
marking_observationsA reference to a vector that will be populated with the observations
Returns
True if all the observation buffers are current, false otherwise

Definition at line 466 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.

Parameters
messageThe message returned from a message notifier
bufferA pointer to the observation buffer to update

Definition at line 252 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 filtering to turn Inf values into range_max.

Parameters
messageThe message returned from a message notifier
bufferA pointer to the observation buffer to update

Definition at line 277 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.

Parameters
messageThe message returned from a message notifier
bufferA pointer to the observation buffer to update

Definition at line 331 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.

Parameters
messageThe message returned from a message notifier
bufferA pointer to the observation buffer to update

Definition at line 314 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 
)
protectedvirtual

Clear freespace based on one observation.

Parameters
clearing_observationThe observation used to raytrace
min_x
min_y
max_x
max_y

Reimplemented in costmap_2d::VoxelLayer.

Definition at line 498 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 612 of file obstacle_layer.cpp.

void costmap_2d::ObstacleLayer::setupDynamicReconfigure ( ros::NodeHandle nh)
protectedvirtual

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 340 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 427 of file obstacle_layer.cpp.

void costmap_2d::ObstacleLayer::updateFootprint ( double  robot_x,
double  robot_y,
double  robot_yaw,
double *  min_x,
double *  min_y,
double *  max_x,
double *  max_y 
)
protected

Definition at line 415 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 602 of file obstacle_layer.cpp.

Member Data Documentation

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 161 of file obstacle_layer.h.

int costmap_2d::ObstacleLayer::combination_method_
protected

Definition at line 169 of file obstacle_layer.h.

dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>* costmap_2d::ObstacleLayer::dsrv_
protected

Definition at line 167 of file obstacle_layer.h.

bool costmap_2d::ObstacleLayer::footprint_clearing_enabled_
protected

Definition at line 148 of file obstacle_layer.h.

std::string costmap_2d::ObstacleLayer::global_frame_
protected

The global frame for the costmap.

Definition at line 152 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 160 of file obstacle_layer.h.

double costmap_2d::ObstacleLayer::max_obstacle_height_
protected

Max Obstacle Height.

Definition at line 153 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 159 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 158 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 157 of file obstacle_layer.h.

laser_geometry::LaserProjection costmap_2d::ObstacleLayer::projector_
protected

Used to project laser scans into point clouds.

Definition at line 155 of file obstacle_layer.h.

bool costmap_2d::ObstacleLayer::rolling_window_
protected

Definition at line 166 of file obstacle_layer.h.

std::vector<costmap_2d::Observation> costmap_2d::ObstacleLayer::static_clearing_observations_
protected

Definition at line 164 of file obstacle_layer.h.

std::vector<costmap_2d::Observation> costmap_2d::ObstacleLayer::static_marking_observations_
protected

Definition at line 164 of file obstacle_layer.h.

std::vector<geometry_msgs::Point> costmap_2d::ObstacleLayer::transformed_footprint_
protected

Definition at line 147 of file obstacle_layer.h.


The documentation for this class was generated from the following files:


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:42