A ROS wrapper for a 2D Costmap. Handles subscribing to topics that provide observations about obstacles in either the form of PointCloud or LaserScan messages. More...

#include <costmap_2d_ros.h>

Public Member Functions
void	addObservationBuffer (const boost::shared_ptr< ObservationBuffer > &buffer)
	If you want to manage your own observation buffer you can add it to the costmap. Note, this is somewhat experimental as this feature has not been used enough to have been proven reliable.
void	clearNonLethalWindow (double size_x, double size_y)
	Clear all non-lethal obstacles outside of a window around the robot... including cells with NO_INFORMATION.
void	clearRobotFootprint ()
	Clear the footprint of the robot in the costmap.
void	clearRobotFootprint (const tf::Stamped< tf::Pose > &global_pose)
	Clear the footprint of the robot in the costmap at a given pose.
	Costmap2DROS (std::string name, tf::TransformListener &tf)
	Constructor for the wrapper.
std::string	getBaseFrameID () const
	Returns the local frame of the costmap.
double	getCircumscribedRadius () const
	Returns the circumscribed radius of the costmap.
bool	getClearingObservations (std::vector< Observation > &clearing_observations) const
	Get the observations used to clear space.
void	getCostmapCopy (Costmap2D &costmap) const
	Returns a copy of the underlying costmap.
void	getCostmapWindowCopy (double win_size_x, double win_size_y, Costmap2D &costmap) const
	Get a copy of a window of the costmap centered at the robot's position. If the requested size of the window does not fit within the bounds of the costmap, the window is truncated automatically to fit.
void	getCostmapWindowCopy (double win_center_x, double win_center_y, double win_size_x, double win_size_y, Costmap2D &costmap) const
	Get a copy of a window of the costmap centered at a given position. If the requested size of the window does not fit within the bounds of the costmap, the window is truncated automatically to fit.
std::string	getGlobalFrameID () const
	Returns the global frame of the costmap.
double	getInflationRadius () const
	Returns the inflation radius of the costmap.
double	getInscribedRadius () const
	Returns the inscribed radius of the costmap.
bool	getMarkingObservations (std::vector< Observation > &marking_observations) const
	Get the observations used to mark space.
void	getOrientedFootprint (double x, double y, double theta, std::vector< geometry_msgs::Point > &oriented_footprint) const
	Given a pose, build the oriented footprint of the robot.
void	getOrientedFootprint (std::vector< geometry_msgs::Point > &oriented_footprint) const
	Build the oriented footprint of the robot at the robot's current pose.
double	getResolution () const
	Returns the resolution of the costmap in meters.
std::vector< geometry_msgs::Point >	getRobotFootprint () const
	Returns the footprint of the robot in the robot_base_frame. To get the footprint in the global_frame use getOrientedFootprint.
bool	getRobotPose (tf::Stamped< tf::Pose > &global_pose) const
	Get the pose of the robot in the global frame of the costmap.
unsigned int	getSizeInCellsX () const
	Returns the x size of the costmap in cells.
unsigned int	getSizeInCellsY () const
	Returns the y size of the costmap in cells.
bool	isCurrent ()
	Check if the observation buffers for the cost map are current.
void	pause ()
	Stops the costmap from updating, but sensor data still comes in over the wire.
void	resetMapOutsideWindow (double size_x, double size_y)
	Reset to the static map outside of a window around the robot specified by the user.
void	resume ()
	Resumes costmap updates.
bool	setConvexPolygonCost (const std::vector< geometry_msgs::Point > &polygon, unsigned char cost_value)
	Set a region in the costmap specified by a convex polygon to a cost.
void	start ()
	Subscribes to sensor topics if necessary and starts costmap updates, can be called to restart the costmap after calls to either stop() or pause()
void	stop ()
	Stops costmap updates and unsubscribes from sensor topics.
void	updateMap ()
	Update the underlying costmap with new sensor data. If you want to update the map outside of the update loop that runs, you can call this.
void	updateStaticMap (const nav_msgs::OccupancyGrid &new_map)
	Updates the costmap's static map with new information.
	~Costmap2DROS ()
	Destructor for the wrapper. Cleans up pointers.
Private Member Functions
double	distance (double x0, double y0, double x1, double y1)
double	distanceToLine (double pX, double pY, double x0, double y0, double x1, double y1)
	Return the shortest distance from a point to a line segment.
void	incomingMap (const nav_msgs::OccupancyGridConstPtr &new_map)
	Callback to update the costmap's map from the map_server.
void	initFromMap (const nav_msgs::OccupancyGrid &map)
	Initialize static_data from a map.
void	laserScanCallback (const sensor_msgs::LaserScanConstPtr &message, const boost::shared_ptr< 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.
std::vector< geometry_msgs::Point >	loadRobotFootprint (ros::NodeHandle node, double inscribed_radius, double circumscribed_radius)
	Grab the footprint of the robot from the parameter server if available.
void	mapUpdateLoop (double frequency)
	The loop that handles updating the costmap.
void	movementCB (const ros::TimerEvent &event)
void	pointCloud2Callback (const sensor_msgs::PointCloud2ConstPtr &message, const boost::shared_ptr< ObservationBuffer > &buffer)
	A callback to handle buffering PointCloud2 messages.
void	pointCloudCallback (const sensor_msgs::PointCloudConstPtr &message, const boost::shared_ptr< ObservationBuffer > &buffer)
	A callback to handle buffering PointCloud messages.
void	reconfigureCB (costmap_2d::Costmap2DConfig &config, uint32_t level)
	Callback for dynamic_reconfigure.
Private Attributes
std::vector< boost::shared_ptr < ObservationBuffer > >	clearing_buffers_
	Used to store observation buffers used for clearing obstacles.
boost::recursive_mutex	configuration_mutex_
Costmap2D *	costmap_
	The underlying costmap to update.
bool	costmap_initialized_
Costmap2DPublisher *	costmap_publisher_
bool	current_
	Whether or not all the observation buffers are updating at the desired rate.
costmap_2d::Costmap2DConfig	default_config_
dynamic_reconfigure::Server < costmap_2d::Costmap2DConfig > *	dsrv_
std::vector< geometry_msgs::Point >	footprint_spec_
std::string	global_frame_
	The global frame for the costmap.
bool	initialized_
std::vector< unsigned char >	input_data_
costmap_2d::Costmap2DConfig	last_config_
boost::recursive_mutex	lock_
boost::recursive_mutex	map_data_lock_
bool	map_initialized_
nav_msgs::MapMetaData	map_meta_data_
ros::Subscriber	map_sub_
boost::mutex	map_update_mutex_
boost::thread *	map_update_thread_
	A thread for updating the map.
bool	map_update_thread_shutdown_
costmap_2d::Costmap2DConfig	maptype_config_
std::vector< boost::shared_ptr < ObservationBuffer > >	marking_buffers_
	Used to store observation buffers used for marking obstacles.
std::string	name_
std::vector< boost::shared_ptr < 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.
tf::Stamped< tf::Pose >	old_pose_
laser_geometry::LaserProjection	projector_
	Used to project laser scans into point clouds.
bool	publish_voxel_
std::string	robot_base_frame_
	The frame_id of the robot base.
bool	robot_stopped_
bool	rolling_window_
	Whether or not the costmap should roll with the robot.
bool	save_debug_pgm_
bool	setup_
bool	static_map_
bool	stop_updates_
bool	stopped_
tf::TransformListener &	tf_
	Used for transforming point clouds.
std::string	tf_prefix_
ros::Timer	timer_
double	transform_tolerance_
ros::Publisher	voxel_pub_

Detailed Description

A ROS wrapper for a 2D Costmap. Handles subscribing to topics that provide observations about obstacles in either the form of PointCloud or LaserScan messages.

Definition at line 85 of file costmap_2d_ros.h.

Constructor & Destructor Documentation

costmap_2d::Costmap2DROS::Costmap2DROS	(	std::string	name,
		tf::TransformListener &	tf
	)

Constructor for the wrapper.

Parameters:

name	The name for this costmap
tf	A reference to a TransformListener

Definition at line 62 of file costmap_2d_ros.cpp.

costmap_2d::Costmap2DROS::~Costmap2DROS ( )

Destructor for the wrapper. Cleans up pointers.

Definition at line 962 of file costmap_2d_ros.cpp.

Member Function Documentation

void costmap_2d::Costmap2DROS::addObservationBuffer ( const boost::shared_ptr< ObservationBuffer > & buffer )

If you want to manage your own observation buffer you can add it to the costmap. Note, this is somewhat experimental as this feature has not been used enough to have been proven reliable.

Parameters:

buffer A shared pointer to your observation buffer

Definition at line 1011 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::clearNonLethalWindow	(	double	size_x,
		double	size_y
	)

Clear all non-lethal obstacles outside of a window around the robot... including cells with NO_INFORMATION.

Parameters:

size_x	The x size of the window to keep unchanged
size_y	The y size of the window to keep unchanged

Definition at line 1202 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::clearRobotFootprint ( )

Clear the footprint of the robot in the costmap.

Definition at line 1394 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::clearRobotFootprint ( const tf::Stamped< tf::Pose > & global_pose )

Clear the footprint of the robot in the costmap at a given pose.

Parameters:

global_pose The pose to clear the footprint at

Definition at line 1462 of file costmap_2d_ros.cpp.

double costmap_2d::Costmap2DROS::distance	(	double	x0,
		double	y0,
		double	x1,
		double	y1
	)		`[inline, private]`

Definition at line 378 of file costmap_2d_ros.h.

double costmap_2d::Costmap2DROS::distanceToLine	(	double	pX,
		double	pY,
		double	x0,
		double	y0,
		double	x1,
		double	y1
	)		`[private]`

Return the shortest distance from a point to a line segment.

Definition at line 796 of file costmap_2d_ros.cpp.

std::string costmap_2d::Costmap2DROS::getBaseFrameID ( ) const

Returns the local frame of the costmap.

Returns:: The local frame of the costmap

Definition at line 1443 of file costmap_2d_ros.cpp.

double costmap_2d::Costmap2DROS::getCircumscribedRadius ( ) const

Returns the circumscribed radius of the costmap.

Returns:: The circumscribed radius of the costmap

Definition at line 1452 of file costmap_2d_ros.cpp.

bool costmap_2d::Costmap2DROS::getClearingObservations ( std::vector< Observation > & clearing_observations ) const

Get the observations used to clear space.

Parameters:

marking_observations A 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 1135 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::getCostmapCopy ( Costmap2D & costmap ) const

Returns a copy of the underlying costmap.

Parameters:

costmap A reference to the map to populate

Definition at line 1235 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::getCostmapWindowCopy	(	double	win_size_x,
		double	win_size_y,
		Costmap2D &	costmap
	)		const

Get a copy of a window of the costmap centered at the robot's position. If the requested size of the window does not fit within the bounds of the costmap, the window is truncated automatically to fit.

Parameters:

win_size_x	The x size of the desired window in meters
win_size_y	The y size of the desired window in meters
costmap	A reference to the map to populate

Definition at line 1319 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::getCostmapWindowCopy	(	double	win_center_x,
		double	win_center_y,
		double	win_size_x,
		double	win_size_y,
		Costmap2D &	costmap
	)		const

Get a copy of a window of the costmap centered at a given position. If the requested size of the window does not fit within the bounds of the costmap, the window is truncated automatically to fit.

Parameters:

win_center_x	The x center point of the desired window in meters
win_center_y	The y center point of the desired window in meters
win_size_x	The x size of the desired window in meters
win_size_y	The y size of the desired window in meters
costmap	A reference to the map to populate

Definition at line 1329 of file costmap_2d_ros.cpp.

std::string costmap_2d::Costmap2DROS::getGlobalFrameID ( ) const

Returns the global frame of the costmap.

Returns:: The global frame of the costmap

Definition at line 1439 of file costmap_2d_ros.cpp.

double costmap_2d::Costmap2DROS::getInflationRadius ( ) const

Returns the inflation radius of the costmap.

Returns:: The inflation radius of the costmap

Definition at line 1457 of file costmap_2d_ros.cpp.

double costmap_2d::Costmap2DROS::getInscribedRadius ( ) const

Returns the inscribed radius of the costmap.

Returns:: The inscribed radius of the costmap

Definition at line 1447 of file costmap_2d_ros.cpp.

bool costmap_2d::Costmap2DROS::getMarkingObservations ( std::vector< Observation > & marking_observations ) const

Get the observations used to mark space.

Parameters:

marking_observations A 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 1123 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::getOrientedFootprint	(	double	x,
		double	y,
		double	theta,
		std::vector< geometry_msgs::Point > &	oriented_footprint
	)		const

Given a pose, build the oriented footprint of the robot.

Parameters:

x	The x position of the robot
y	The y position of the robot
theta	The orientation of the robot
oriented_footprint	Will be filled with the points in the oriented footprint of the robot

Definition at line 1416 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::getOrientedFootprint ( std::vector< geometry_msgs::Point > & oriented_footprint ) const

Build the oriented footprint of the robot at the robot's current pose.

Parameters:

oriented_footprint Will be filled with the points in the oriented footprint of the robot

Definition at line 1406 of file costmap_2d_ros.cpp.

double costmap_2d::Costmap2DROS::getResolution ( ) const

Returns the resolution of the costmap in meters.

Returns:: The resolution of the costmap in meters

Definition at line 1354 of file costmap_2d_ros.cpp.

std::vector< geometry_msgs::Point > costmap_2d::Costmap2DROS::getRobotFootprint ( ) const

Returns the footprint of the robot in the robot_base_frame. To get the footprint in the global_frame use getOrientedFootprint.

Returns:: The footprint of the robot in the robot_base_frame

Definition at line 1402 of file costmap_2d_ros.cpp.

bool costmap_2d::Costmap2DROS::getRobotPose ( tf::Stamped< tf::Pose > & global_pose ) const

Get the pose of the robot in the global frame of the costmap.

Parameters:

global_pose Will be set to the pose of the robot in the global frame of the costmap

Returns:: True if the pose was set successfully, false otherwise

Definition at line 1359 of file costmap_2d_ros.cpp.

unsigned int costmap_2d::Costmap2DROS::getSizeInCellsX ( ) const

Returns the x size of the costmap in cells.

Returns:: The x size of the costmap in cells

Definition at line 1344 of file costmap_2d_ros.cpp.

unsigned int costmap_2d::Costmap2DROS::getSizeInCellsY ( ) const

Returns the y size of the costmap in cells.

Returns:: The y size of the costmap in cells

Definition at line 1349 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::incomingMap ( const nav_msgs::OccupancyGridConstPtr & new_map ) [private]

Callback to update the costmap's map from the map_server.

Parameters:

new_map The map to put into the costmap. The origin of the new map along with its size will determine what parts of the costmap's static map are overwritten.

Definition at line 1240 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::initFromMap ( const nav_msgs::OccupancyGrid & map ) [private]

Initialize static_data from a map.

Parameters:

new_map The map to initialize from

Definition at line 1249 of file costmap_2d_ros.cpp.

bool costmap_2d::Costmap2DROS::isCurrent ( ) [inline]

Check if the observation buffers for the cost map are current.

Returns:: True if the buffers are current, false otherwise

Definition at line 276 of file costmap_2d_ros.h.

void costmap_2d::Costmap2DROS::laserScanCallback	(	const sensor_msgs::LaserScanConstPtr &	message,
		const boost::shared_ptr< ObservationBuffer > &	buffer
	)		`[private]`

A callback to handle buffering LaserScan messages.

Parameters:

message	The message returned from a message notifier
buffer	A pointer to the observation buffer to update

Definition at line 1016 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::laserScanValidInfCallback	(	const sensor_msgs::LaserScanConstPtr &	message,
		const boost::shared_ptr< ObservationBuffer > &	buffer
	)		`[private]`

A callback to handle buffering LaserScan messages which need to be filtered to turn Inf values into range_max.

Parameters:

message	The message returned from a message notifier
buffer	A pointer to the observation buffer to update

Definition at line 1038 of file costmap_2d_ros.cpp.

std::vector< geometry_msgs::Point > costmap_2d::Costmap2DROS::loadRobotFootprint	(	ros::NodeHandle	node,
		double	inscribed_radius,
		double	circumscribed_radius
	)		`[private]`

Grab the footprint of the robot from the parameter server if available.

Definition at line 827 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::mapUpdateLoop ( double frequency ) [private]

The loop that handles updating the costmap.

Parameters:

frequency The rate at which to run the loop

Definition at line 1093 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::movementCB ( const ros::TimerEvent & event ) [private]

Definition at line 430 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::pause ( ) [inline]

Stops the costmap from updating, but sensor data still comes in over the wire.

Definition at line 294 of file costmap_2d_ros.h.

void costmap_2d::Costmap2DROS::pointCloud2Callback	(	const sensor_msgs::PointCloud2ConstPtr &	message,
		const boost::shared_ptr< ObservationBuffer > &	buffer
	)		`[private]`

A callback to handle buffering PointCloud2 messages.

Parameters:

message	The message returned from a message notifier
buffer	A pointer to the observation buffer to update

Definition at line 1086 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::pointCloudCallback	(	const sensor_msgs::PointCloudConstPtr &	message,
		const boost::shared_ptr< ObservationBuffer > &	buffer
	)		`[private]`

A callback to handle buffering PointCloud messages.

Parameters:

message	The message returned from a message notifier
buffer	A pointer to the observation buffer to update

Definition at line 1072 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::reconfigureCB	(	costmap_2d::Costmap2DConfig &	config,
		uint32_t	level
	)		`[private]`

Callback for dynamic_reconfigure.

Definition at line 453 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::resetMapOutsideWindow	(	double	size_x,
		double	size_y
	)

Reset to the static map outside of a window around the robot specified by the user.

Parameters:

size_x	The x size of the window to keep unchanged
size_y	The y size of the window to keep unchanged

Definition at line 1218 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::resume ( ) [inline]

Resumes costmap updates.

Definition at line 303 of file costmap_2d_ros.h.

bool costmap_2d::Costmap2DROS::setConvexPolygonCost	(	const std::vector< geometry_msgs::Point > &	polygon,
		unsigned char	cost_value
	)

Set a region in the costmap specified by a convex polygon to a cost.

Parameters:

polygon	The polygon affected
cost_value	The cost to apply

Returns:: True if the operation was successful, false otherwise

Definition at line 1428 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::start ( )

Subscribes to sensor topics if necessary and starts costmap updates, can be called to restart the costmap after calls to either stop() or pause()

Definition at line 978 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::stop ( )

Stops costmap updates and unsubscribes from sensor topics.

Definition at line 1000 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::updateMap ( )

Update the underlying costmap with new sensor data. If you want to update the map outside of the update loop that runs, you can call this.

Definition at line 1148 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::updateStaticMap ( const nav_msgs::OccupancyGrid & new_map )

Updates the costmap's static map with new information.

Parameters:

new_map The map to put into the costmap. The origin of the new map along with its size will determine what parts of the costmap's static map are overwritten.

Definition at line 1262 of file costmap_2d_ros.cpp.