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
	Costmap2DROS (std::string name, tf::TransformListener &tf)
	Constructor for the wrapper.
std::string	getBaseFrameID ()
	Returns the local frame of the costmap.
Costmap2D *	getCostmap ()
	Return a pointer to the "master" costmap which receives updates from all the layers.
std::string	getGlobalFrameID ()
	Returns the global frame of the costmap.
LayeredCostmap *	getLayeredCostmap ()
void	getOrientedFootprint (std::vector< geometry_msgs::Point > &oriented_footprint) const
	Build the oriented footprint of the robot at the robot's current pose.
std::vector< geometry_msgs::Point >	getRobotFootprint ()
	Return the current footprint of the robot as a vector of points.
geometry_msgs::Polygon	getRobotFootprintPolygon ()
	Returns the current padded footprint as a geometry_msgs::Polygon.
bool	getRobotPose (tf::Stamped< tf::Pose > &global_pose) const
	Get the pose of the robot in the global frame of the costmap.
std::vector< geometry_msgs::Point >	getUnpaddedRobotFootprint ()
	Return the current unpadded footprint of the robot as a vector of points.
bool	isCurrent ()
	Same as getLayeredCostmap()->isCurrent().
void	pause ()
	Stops the costmap from updating, but sensor data still comes in over the wire.
void	resetLayers ()
	Reset each individual layer.
void	resume ()
	Resumes costmap updates.
void	setUnpaddedRobotFootprint (const std::vector< geometry_msgs::Point > &points)
	Set the footprint of the robot to be the given set of points, padded by footprint_padding.
void	setUnpaddedRobotFootprintPolygon (const geometry_msgs::Polygon &footprint)
	Set the footprint of the robot to be the given polygon, padded by footprint_padding.
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 ()
	~Costmap2DROS ()
Protected Attributes
std::string	global_frame_
	The global frame for the costmap.
LayeredCostmap *	layered_costmap_
std::string	name_
std::string	robot_base_frame_
	The frame_id of the robot base.
tf::TransformListener &	tf_
	Used for transforming point clouds.
double	transform_tolerance_
	timeout before transform errors
Private Member Functions
void	mapUpdateLoop (double frequency)
void	movementCB (const ros::TimerEvent &event)
void	readFootprintFromConfig (const costmap_2d::Costmap2DConfig &new_config, const costmap_2d::Costmap2DConfig &old_config)
	Set the footprint from the new_config object.
void	reconfigureCB (costmap_2d::Costmap2DConfig &config, uint32_t level)
void	resetOldParameters (ros::NodeHandle &nh)
Private Attributes
boost::recursive_mutex	configuration_mutex_
dynamic_reconfigure::Server < costmap_2d::Costmap2DConfig > *	dsrv_
float	footprint_padding_
ros::Publisher	footprint_pub_
ros::Subscriber	footprint_sub_
bool	initialized_
ros::Time	last_publish_
boost::thread *	map_update_thread_
	A thread for updating the map.
bool	map_update_thread_shutdown_
costmap_2d::Costmap2DConfig	old_config_
tf::Stamped< tf::Pose >	old_pose_
std::vector< geometry_msgs::Point >	padded_footprint_
pluginlib::ClassLoader< Layer >	plugin_loader_
ros::Duration	publish_cycle
Costmap2DPublisher *	publisher_
bool	robot_stopped_
bool	stop_updates_
bool	stopped_
ros::Timer	timer_
std::vector< geometry_msgs::Point >	unpadded_footprint_

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 72 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 ( )

Definition at line 192 of file costmap_2d_ros.cpp.

Member Function Documentation

std::string costmap_2d::Costmap2DROS::getBaseFrameID ( ) [inline]

Returns the local frame of the costmap.

Returns:: The local frame of the costmap

Definition at line 146 of file costmap_2d_ros.h.

Costmap2D* costmap_2d::Costmap2DROS::getCostmap ( ) [inline]

Return a pointer to the "master" costmap which receives updates from all the layers.

Same as calling getLayeredCostmap()->getCostmap().

Definition at line 128 of file costmap_2d_ros.h.

std::string costmap_2d::Costmap2DROS::getGlobalFrameID ( ) [inline]

Returns the global frame of the costmap.

Returns:: The global frame of the costmap

Definition at line 137 of file costmap_2d_ros.h.

LayeredCostmap* costmap_2d::Costmap2DROS::getLayeredCostmap ( ) [inline]

Definition at line 150 of file costmap_2d_ros.h.

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 564 of file costmap_2d_ros.cpp.

std::vector<geometry_msgs::Point> costmap_2d::Costmap2DROS::getRobotFootprint ( ) [inline]

Return the current footprint of the robot as a vector of points.

This version of the footprint is padded by the footprint_padding_ distance, set in the rosparam "footprint_padding".

The footprint initially comes from the rosparam "footprint" but can be overwritten by dynamic reconfigure or by messages received on the "footprint" topic.

Definition at line 169 of file costmap_2d_ros.h.

geometry_msgs::Polygon costmap_2d::Costmap2DROS::getRobotFootprintPolygon ( ) [inline]

Returns the current padded footprint as a geometry_msgs::Polygon.

Definition at line 156 of file costmap_2d_ros.h.

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 523 of file costmap_2d_ros.cpp.

std::vector<geometry_msgs::Point> costmap_2d::Costmap2DROS::getUnpaddedRobotFootprint ( ) [inline]

Return the current unpadded footprint of the robot as a vector of points.

This is the raw version of the footprint without padding.

The footprint initially comes from the rosparam "footprint" but can be overwritten by dynamic reconfigure or by messages received on the "footprint" topic.

Definition at line 181 of file costmap_2d_ros.h.

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

Same as getLayeredCostmap()->isCurrent().

Definition at line 113 of file costmap_2d_ros.h.

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

Definition at line 391 of file costmap_2d_ros.cpp.

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

Definition at line 365 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::pause ( )

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

Definition at line 494 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::readFootprintFromConfig	(	const costmap_2d::Costmap2DConfig &	new_config,
		const costmap_2d::Costmap2DConfig &	old_config
	)		`[private]`

Set the footprint from the new_config object.

If the values of footprint and robot_radius are the same in new_config and old_config, nothing is changed.

Definition at line 324 of file costmap_2d_ros.cpp.

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

Definition at line 280 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::resetLayers ( )

Reset each individual layer.

Definition at line 511 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::resetOldParameters ( ros::NodeHandle & nh ) [private]

Definition at line 207 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::resume ( )

Resumes costmap updates.

Definition at line 500 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::setUnpaddedRobotFootprint ( const std::vector< geometry_msgs::Point > & points )

Set the footprint of the robot to be the given set of points, padded by footprint_padding.

Should be a convex polygon, though this is not enforced.

First expands the given polygon by footprint_padding_ and then sets padded_footprint_ and calls layered_costmap_->setFootprint(). Also saves the unpadded footprint, which is available from getUnpaddedRobotFootprint().

Definition at line 356 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::setUnpaddedRobotFootprintPolygon ( const geometry_msgs::Polygon & footprint )

Set the footprint of the robot to be the given polygon, padded by footprint_padding.

Should be a convex polygon, though this is not enforced.

First expands the given polygon by footprint_padding_ and then sets padded_footprint_ and calls layered_costmap_->setFootprint(). Also saves the unpadded footprint, which is available from getUnpaddedRobotFootprint().

Definition at line 187 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 458 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::stop ( )

Stops costmap updates and unsubscribes from sensor topics.

Definition at line 480 of file costmap_2d_ros.cpp.

void costmap_2d::Costmap2DROS::updateMap ( )

Definition at line 433 of file costmap_2d_ros.cpp.