costmap_2d::Costmap2DROS Class Reference

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>

List of all members.

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 (const tf::Stamped< tf::Pose > &global_pose)
	Clear the footprint of the robot in the costmap at a given pose.
void	clearRobotFootprint ()
	Clear the footprint of the robot in the costmap.
	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_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.
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.
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 (std::vector< geometry_msgs::Point > &oriented_footprint) const
	Build the oriented footprint of the robot at the robot's current pose.
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.
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.
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	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.
Private Attributes
std::vector< boost::shared_ptr < ObservationBuffer > >	clearing_buffers_
	Used to store observation buffers used for clearing obstacles.
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.
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_
boost::recursive_mutex	lock_
boost::recursive_mutex	map_data_lock_
bool	map_initialized_
nav_msgs::MapMetaData	map_meta_data_
ros::Subscriber	map_sub_
boost::thread *	map_update_thread_
	A thread for updating the map.
bool	map_update_thread_shutdown_
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.
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	rolling_window_
	Whether or not the costmap should roll with the robot.
bool	save_debug_pgm_
bool	stop_updates_
bool	stopped_
tf::TransformListener &	tf_
	Used for transforming point clouds.
std::string	tf_prefix_
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 71 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 45 of file costmap_2d_ros.cpp.

costmap_2d::Costmap2DROS::~Costmap2DROS ( )

Destructor for the wrapper. Cleans up pointers.

Definition at line 473 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 521 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 676 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 934 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::clearRobotFootprint ( )

Clear the footprint of the robot in the costmap.

Definition at line 866 of file costmap_2d_ros.cpp.

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

Definition at line 350 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 391 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 915 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 924 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 609 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 709 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 802 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 792 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 911 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 929 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 919 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 597 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 878 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 888 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 827 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 874 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 832 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 817 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 822 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 714 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 723 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 262 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 526 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 422 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 569 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 280 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 562 of file costmap_2d_ros.cpp.

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