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. More... | |
std::string | getBaseFrameID () |
Returns the local frame of the costmap. More... | |
Costmap2D * | getCostmap () |
Return a pointer to the "master" costmap which receives updates from all the layers. More... | |
std::string | getGlobalFrameID () |
Returns the global frame of the costmap. More... | |
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. More... | |
std::vector< geometry_msgs::Point > | getRobotFootprint () |
Return the current footprint of the robot as a vector of points. More... | |
geometry_msgs::Polygon | getRobotFootprintPolygon () |
Returns the current padded footprint as a geometry_msgs::Polygon. More... | |
bool | getRobotPose (tf::Stamped< tf::Pose > &global_pose) const |
Get the pose of the robot in the global frame of the costmap. More... | |
std::vector< geometry_msgs::Point > | getUnpaddedRobotFootprint () |
Return the current unpadded footprint of the robot as a vector of points. More... | |
bool | isCurrent () |
Same as getLayeredCostmap()->isCurrent(). More... | |
void | pause () |
Stops the costmap from updating, but sensor data still comes in over the wire. More... | |
void | resetLayers () |
Reset each individual layer. More... | |
void | resume () |
Resumes costmap updates. More... | |
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. More... | |
void | setUnpaddedRobotFootprintPolygon (const geometry_msgs::Polygon &footprint) |
Set the footprint of the robot to be the given polygon, padded by footprint_padding. More... | |
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() More... | |
void | stop () |
Stops costmap updates and unsubscribes from sensor topics. More... | |
void | updateMap () |
~Costmap2DROS () | |
Protected Attributes | |
std::string | global_frame_ |
The global frame for the costmap. More... | |
LayeredCostmap * | layered_costmap_ |
std::string | name_ |
std::string | robot_base_frame_ |
The frame_id of the robot base. More... | |
tf::TransformListener & | tf_ |
Used for transforming point clouds. More... | |
double | transform_tolerance_ |
timeout before transform errors More... | |
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. More... | |
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. More... | |
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_ |
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.
costmap_2d::Costmap2DROS::Costmap2DROS | ( | std::string | name, |
tf::TransformListener & | tf | ||
) |
Constructor for the wrapper.
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 189 of file costmap_2d_ros.cpp.
|
inline |
Returns the local frame of the costmap.
Definition at line 146 of file costmap_2d_ros.h.
|
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.
|
inline |
Returns the global frame of the costmap.
Definition at line 137 of file costmap_2d_ros.h.
|
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.
oriented_footprint | Will be filled with the points in the oriented footprint of the robot |
Definition at line 561 of file costmap_2d_ros.cpp.
|
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.
|
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.
global_pose | Will be set to the pose of the robot in the global frame of the costmap |
Definition at line 520 of file costmap_2d_ros.cpp.
|
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.
|
inline |
Same as getLayeredCostmap()->isCurrent().
Definition at line 113 of file costmap_2d_ros.h.
|
private |
Definition at line 388 of file costmap_2d_ros.cpp.
|
private |
Definition at line 362 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 491 of file costmap_2d_ros.cpp.
|
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 321 of file costmap_2d_ros.cpp.
|
private |
Definition at line 277 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::resetLayers | ( | ) |
Reset each individual layer.
Definition at line 508 of file costmap_2d_ros.cpp.
|
private |
Definition at line 204 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::resume | ( | ) |
Resumes costmap updates.
Definition at line 497 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 353 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 184 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 455 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::stop | ( | ) |
Stops costmap updates and unsubscribes from sensor topics.
Definition at line 477 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::updateMap | ( | ) |
Definition at line 430 of file costmap_2d_ros.cpp.
|
private |
Definition at line 247 of file costmap_2d_ros.h.
|
private |
Definition at line 245 of file costmap_2d_ros.h.
|
private |
Definition at line 253 of file costmap_2d_ros.h.
|
private |
Definition at line 250 of file costmap_2d_ros.h.
|
private |
Definition at line 249 of file costmap_2d_ros.h.
|
protected |
The global frame for the costmap.
Definition at line 220 of file costmap_2d_ros.h.
|
private |
Definition at line 237 of file costmap_2d_ros.h.
|
private |
Definition at line 240 of file costmap_2d_ros.h.
|
protected |
Definition at line 217 of file costmap_2d_ros.h.
|
private |
A thread for updating the map.
Definition at line 238 of file costmap_2d_ros.h.
|
private |
Definition at line 236 of file costmap_2d_ros.h.
|
protected |
Definition at line 218 of file costmap_2d_ros.h.
|
private |
Definition at line 254 of file costmap_2d_ros.h.
|
private |
Definition at line 243 of file costmap_2d_ros.h.
|
private |
Definition at line 252 of file costmap_2d_ros.h.
|
private |
Definition at line 242 of file costmap_2d_ros.h.
|
private |
Definition at line 241 of file costmap_2d_ros.h.
|
private |
Definition at line 244 of file costmap_2d_ros.h.
|
protected |
The frame_id of the robot base.
Definition at line 221 of file costmap_2d_ros.h.
|
private |
Definition at line 237 of file costmap_2d_ros.h.
|
private |
Definition at line 237 of file costmap_2d_ros.h.
|
private |
Definition at line 237 of file costmap_2d_ros.h.
|
protected |
Used for transforming point clouds.
Definition at line 219 of file costmap_2d_ros.h.
|
private |
Definition at line 239 of file costmap_2d_ros.h.
|
protected |
timeout before transform errors
Definition at line 222 of file costmap_2d_ros.h.
|
private |
Definition at line 251 of file costmap_2d_ros.h.