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 (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. | |
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 | readFootprintFromParams (ros::NodeHandle &nh) |
Read the ros-params "footprint" and/or "robot_radius" from the given NodeHandle using searchParam() to go up the tree. | |
bool | readFootprintFromString (const std::string &footprint_string) |
Set the footprint from the given string. | |
void | readFootprintFromXMLRPC (XmlRpc::XmlRpcValue &footprint_xmlrpc, const std::string &full_param_name) |
Set the footprint from the given XmlRpcValue. | |
void | reconfigureCB (costmap_2d::Costmap2DConfig &config, uint32_t level) |
void | resetOldParameters (ros::NodeHandle &nh) |
void | setFootprintFromRadius (double radius) |
Set the footprint to a circle of the given radius, in meters. | |
void | writeFootprintToParam (ros::NodeHandle &nh) |
Write the current unpadded_footprint_ to the "footprint" parameter of the given NodeHandle so that dynamic_reconfigure will see the new value. | |
Private Attributes | |
boost::recursive_mutex | configuration_mutex_ |
dynamic_reconfigure::Server < costmap_2d::Costmap2DConfig > * | dsrv_ |
float | footprint_padding_ |
ros::Subscriber | footprint_sub_ |
bool | got_footprint_ |
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_ |
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.
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 63 of file costmap_2d_ros.cpp.
Definition at line 163 of file costmap_2d_ros.cpp.
std::string costmap_2d::Costmap2DROS::getBaseFrameID | ( | ) | [inline] |
Returns the local frame of the costmap.
Definition at line 145 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 127 of file costmap_2d_ros.h.
std::string costmap_2d::Costmap2DROS::getGlobalFrameID | ( | ) | [inline] |
Returns the global frame of the costmap.
Definition at line 136 of file costmap_2d_ros.h.
LayeredCostmap* costmap_2d::Costmap2DROS::getLayeredCostmap | ( | ) | [inline] |
Definition at line 149 of file costmap_2d_ros.h.
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.
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 704 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.
oriented_footprint | Will be filled with the points in the oriented footprint of the robot |
Definition at line 695 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 168 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 155 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 653 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 180 of file costmap_2d_ros.h.
bool costmap_2d::Costmap2DROS::isCurrent | ( | ) | [inline] |
Same as getLayeredCostmap()->isCurrent().
Definition at line 112 of file costmap_2d_ros.h.
void costmap_2d::Costmap2DROS::mapUpdateLoop | ( | double | frequency | ) | [private] |
Definition at line 532 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::movementCB | ( | const ros::TimerEvent & | event | ) | [private] |
Definition at line 506 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 624 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 297 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::readFootprintFromParams | ( | ros::NodeHandle & | nh | ) | [private] |
Read the ros-params "footprint" and/or "robot_radius" from the given NodeHandle using searchParam() to go up the tree.
Calls setFootprint() when successful.
Definition at line 381 of file costmap_2d_ros.cpp.
bool costmap_2d::Costmap2DROS::readFootprintFromString | ( | const std::string & | footprint_string | ) | [private] |
Set the footprint from the given string.
Format should be bracketed array of arrays of floats, like so: [[1.0, 2.2], [3.3, 4.2], ...]
Definition at line 321 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::readFootprintFromXMLRPC | ( | XmlRpc::XmlRpcValue & | footprint_xmlrpc, |
const std::string & | full_param_name | ||
) | [private] |
Set the footprint from the given XmlRpcValue.
footprint_xmlrpc | should be an array of arrays, where the top-level array should have 3 or more elements, and the sub-arrays should all have exactly 2 elements (x and y coordinates). |
full_param_name | this is the full name of the rosparam from which the footprint_xmlrpc value came. It is used only for reporting errors. |
Definition at line 450 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::reconfigureCB | ( | costmap_2d::Costmap2DConfig & | config, |
uint32_t | level | ||
) | [private] |
Definition at line 253 of file costmap_2d_ros.cpp.
Reset each individual layer.
Definition at line 641 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::resetOldParameters | ( | ros::NodeHandle & | nh | ) | [private] |
Definition at line 178 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::resume | ( | ) |
Resumes costmap updates.
Definition at line 630 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::setFootprintFromRadius | ( | double | radius | ) | [private] |
Set the footprint to a circle of the given radius, in meters.
Definition at line 362 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 490 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 158 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 588 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::stop | ( | ) |
Stops costmap updates and unsubscribes from sensor topics.
Definition at line 610 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::updateMap | ( | ) |
Definition at line 574 of file costmap_2d_ros.cpp.
void costmap_2d::Costmap2DROS::writeFootprintToParam | ( | ros::NodeHandle & | nh | ) | [private] |
Write the current unpadded_footprint_ to the "footprint" parameter of the given NodeHandle so that dynamic_reconfigure will see the new value.
Definition at line 415 of file costmap_2d_ros.cpp.
boost::recursive_mutex costmap_2d::Costmap2DROS::configuration_mutex_ [private] |
Definition at line 288 of file costmap_2d_ros.h.
dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig>* costmap_2d::Costmap2DROS::dsrv_ [private] |
Definition at line 286 of file costmap_2d_ros.h.
float costmap_2d::Costmap2DROS::footprint_padding_ [private] |
Definition at line 294 of file costmap_2d_ros.h.
Definition at line 290 of file costmap_2d_ros.h.
std::string costmap_2d::Costmap2DROS::global_frame_ [protected] |
The global frame for the costmap.
Definition at line 228 of file costmap_2d_ros.h.
bool costmap_2d::Costmap2DROS::got_footprint_ [private] |
Definition at line 291 of file costmap_2d_ros.h.
bool costmap_2d::Costmap2DROS::initialized_ [private] |
Definition at line 278 of file costmap_2d_ros.h.
Definition at line 281 of file costmap_2d_ros.h.
LayeredCostmap* costmap_2d::Costmap2DROS::layered_costmap_ [protected] |
Definition at line 225 of file costmap_2d_ros.h.
boost::thread* costmap_2d::Costmap2DROS::map_update_thread_ [private] |
A thread for updating the map.
Definition at line 279 of file costmap_2d_ros.h.
bool costmap_2d::Costmap2DROS::map_update_thread_shutdown_ [private] |
Definition at line 277 of file costmap_2d_ros.h.
std::string costmap_2d::Costmap2DROS::name_ [protected] |
Definition at line 226 of file costmap_2d_ros.h.
costmap_2d::Costmap2DConfig costmap_2d::Costmap2DROS::old_config_ [private] |
Definition at line 295 of file costmap_2d_ros.h.
Definition at line 284 of file costmap_2d_ros.h.
std::vector<geometry_msgs::Point> costmap_2d::Costmap2DROS::padded_footprint_ [private] |
Definition at line 293 of file costmap_2d_ros.h.
Definition at line 283 of file costmap_2d_ros.h.
Definition at line 282 of file costmap_2d_ros.h.
Definition at line 285 of file costmap_2d_ros.h.
std::string costmap_2d::Costmap2DROS::robot_base_frame_ [protected] |
The frame_id of the robot base.
Definition at line 229 of file costmap_2d_ros.h.
bool costmap_2d::Costmap2DROS::robot_stopped_ [private] |
Definition at line 278 of file costmap_2d_ros.h.
bool costmap_2d::Costmap2DROS::stop_updates_ [private] |
Definition at line 278 of file costmap_2d_ros.h.
bool costmap_2d::Costmap2DROS::stopped_ [private] |
Definition at line 278 of file costmap_2d_ros.h.
tf::TransformListener& costmap_2d::Costmap2DROS::tf_ [protected] |
Used for transforming point clouds.
Definition at line 227 of file costmap_2d_ros.h.
ros::Timer costmap_2d::Costmap2DROS::timer_ [private] |
Definition at line 280 of file costmap_2d_ros.h.
double costmap_2d::Costmap2DROS::transform_tolerance_ [protected] |
timeout before transform errors
Definition at line 230 of file costmap_2d_ros.h.
std::vector<geometry_msgs::Point> costmap_2d::Costmap2DROS::unpadded_footprint_ [private] |
Definition at line 292 of file costmap_2d_ros.h.