63 layered_costmap_(NULL),
66 transform_tolerance_(0.3),
67 map_update_thread_shutdown_(false),
71 robot_stopped_(false),
72 map_update_thread_(NULL),
74 plugin_loader_(
"costmap_2d",
"costmap_2d::Layer"),
77 footprint_padding_(0.0)
108 ROS_WARN(
"Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
118 bool rolling_window, track_unknown_space, always_send_full_costmap;
119 private_nh.
param(
"rolling_window", rolling_window,
false);
120 private_nh.
param(
"track_unknown_space", track_unknown_space,
false);
121 private_nh.
param(
"always_send_full_costmap", always_send_full_costmap,
false);
125 if (!private_nh.
hasParam(
"plugins"))
133 private_nh.
getParam(
"plugins", my_list);
134 for (int32_t i = 0; i < my_list.
size(); ++i)
136 std::string pname =
static_cast<std::string
>(my_list[i][
"name"]);
137 std::string type =
static_cast<std::string
>(my_list[i][
"type"]);
138 ROS_INFO(
"Using plugin \"%s\"", pname.c_str());
147 std::string topic_param, topic;
148 if (!private_nh.
searchParam(
"footprint_topic", topic_param))
150 topic_param =
"footprint_topic";
153 private_nh.
param(topic_param, topic, std::string(
"footprint"));
156 if (!private_nh.
searchParam(
"published_footprint_topic", topic_param))
158 topic_param =
"published_footprint";
161 private_nh.
param(topic_param, topic, std::string(
"oriented_footprint"));
167 always_send_full_costmap);
181 dsrv_->setCallback(cb);
206 ROS_INFO(
"Loading from pre-hydro parameter style");
209 std::vector < XmlRpc::XmlRpcValue > plugins;
215 if (nh.
getParam(
"static_map", flag) && flag)
220 plugins.push_back(super_map);
230 if (nh.
getParam(
"map_type", s) && s ==
"voxel")
235 plugins.push_back(super_map);
249 plugins.push_back(super_map);
256 nh.
param(
"observation_sources", s, std::string(
""));
257 std::stringstream ss(s);
271 plugins.push_back(super_map);
274 nh.
setParam(
"plugins", super_array);
287 double map_update_frequency = config.update_frequency;
289 double map_publish_frequency = config.publish_frequency;
290 if (map_publish_frequency > 0)
296 double map_width_meters = config.width, map_height_meters = config.height, resolution = config.resolution, origin_x =
298 origin_y = config.origin_y;
303 (
unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
322 const costmap_2d::Costmap2DConfig &old_config)
328 if (new_config.footprint == old_config.footprint &&
329 new_config.robot_radius == old_config.robot_radius)
334 if (new_config.footprint !=
"" && new_config.footprint !=
"[]")
336 std::vector<geometry_msgs::Point> new_footprint;
343 ROS_ERROR(
"Invalid footprint string from dynamic reconfigure");
375 else if (fabs((
old_pose_.getOrigin() - new_pose.getOrigin()).
length()) < 1e-3
376 && fabs(
old_pose_.getRotation().angle(new_pose.getRotation())) < 1e-3)
391 if (frequency == 0.0)
398 struct timeval start, end;
399 double start_t, end_t, t_diff;
400 gettimeofday(&start, NULL);
404 gettimeofday(&end, NULL);
405 start_t = start.tv_sec + double(start.tv_usec) / 1e6;
406 end_t = end.tv_sec + double(end.tv_usec) / 1e6;
407 t_diff = end_t - start_t;
408 ROS_DEBUG(
"Map update time: %.9f", t_diff);
411 unsigned int x0, y0, xn, yn;
425 ROS_WARN(
"Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,
438 double x = pose.getOrigin().x(),
439 y = pose.getOrigin().y(),
444 geometry_msgs::PolygonStamped footprint;
465 (*plugin)->activate();
485 (*plugin)->deactivate();
522 global_pose.setIdentity();
524 robot_pose.setIdentity();
536 ROS_ERROR_THROTTLE(1.0,
"No Transform available Error looking up robot pose: %s\n", ex.what());
546 ROS_ERROR_THROTTLE(1.0,
"Extrapolation Error looking up robot pose: %s\n", ex.what());
553 "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
567 double yaw =
tf::getYaw(global_pose.getRotation());
std::vector< geometry_msgs::Point > makeFootprintFromParams(ros::NodeHandle &nh)
Read the ros-params "footprint" and/or "robot_radius" from the given NodeHandle using searchParam() t...
void start()
Subscribes to sensor topics if necessary and starts costmap updates, can be called to restart the cos...
bool map_update_thread_shutdown_
#define ROS_WARN_THROTTLE(rate,...)
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked=false)
bool getRobotPose(tf::Stamped< tf::Pose > &global_pose) const
Get the pose of the robot in the global frame of the costmap.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool deleteParam(const std::string &key) const
static double getYaw(const Quaternion &bt_q)
Duration cycleTime() const
costmap_2d::Costmap2DConfig old_config_
void pause()
Stops the costmap from updating, but sensor data still comes in over the wire.
dynamic_reconfigure::Server< costmap_2d::Costmap2DConfig > * dsrv_
ros::Duration publish_cycle
void readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config, const costmap_2d::Costmap2DConfig &old_config)
Set the footprint from the new_config object.
void setFootprint(const std::vector< geometry_msgs::Point > &footprint_spec)
Updates the stored footprint, updates the circumscribed and inscribed radii, and calls onFootprintCha...
void getOrientedFootprint(std::vector< geometry_msgs::Point > &oriented_footprint) const
Build the oriented footprint of the robot at the robot's current pose.
TFSIMD_FORCE_INLINE const tfScalar & y() const
std::vector< boost::shared_ptr< Layer > > * getPlugins()
boost::thread * map_update_thread_
A thread for updating the map.
void getBounds(unsigned int *x0, unsigned int *xn, unsigned int *y0, unsigned int *yn)
bool makeFootprintFromString(const std::string &footprint_string, std::vector< geometry_msgs::Point > &footprint)
Make the footprint from the given string.
tf::TransformListener & tf_
Used for transforming point clouds.
std::string resolve(const std::string &prefix, const std::string &frame_name)
std::vector< geometry_msgs::Point > toPointVector(geometry_msgs::Polygon polygon)
Convert Polygon msg to vector of Points.
LayeredCostmap * layered_costmap_
#define ROS_ERROR_THROTTLE(rate,...)
std::map< std::string, XmlRpcValue > ValueStruct
ros::Subscriber footprint_sub_
void addPlugin(boost::shared_ptr< Layer > plugin)
void setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon &footprint)
Set the footprint of the robot to be the given polygon, padded by footprint_padding.
std::string global_frame_
The global frame for the costmap.
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
std::vector< geometry_msgs::Point > makeFootprintFromRadius(double radius)
Create a circular footprint from a given radius.
void padFootprint(std::vector< geometry_msgs::Point > &footprint, double padding)
Adds the specified amount of padding to the footprint (in place)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void movementCB(const ros::TimerEvent &event)
TFSIMD_FORCE_INLINE const tfScalar & x() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
Costmap2DPublisher * publisher_
pluginlib::ClassLoader< Layer > plugin_loader_
std::string robot_base_frame_
The frame_id of the robot base.
void move_parameter(ros::NodeHandle &old_h, ros::NodeHandle &new_h, std::string name, bool should_delete=true)
A tool to periodically publish visualization data from a Costmap2D.
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
void updateBounds(unsigned int x0, unsigned int xn, unsigned int y0, unsigned int yn)
Include the given bounds in the changed-rectangle.
void mapUpdateLoop(double frequency)
void resetLayers()
Reset each individual layer.
void resume()
Resumes costmap updates.
bool getParam(const std::string &key, std::string &s) const
double transform_tolerance_
timeout before transform errors
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.
ros::Publisher footprint_pub_
void stop()
Stops costmap updates and unsubscribes from sensor topics.
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
tf::Stamped< tf::Pose > old_pose_
A 2D costmap provides a mapping between points in the world and their associated "costs".
bool hasParam(const std::string &key) const
Instantiates different layer plugins and aggregates them into one score.
void updateMap(double robot_x, double robot_y, double robot_yaw)
Update the underlying costmap with new data. If you want to update the map outside of the update loop...
ROSCPP_DECL void spinOnce()
void setArray(XmlRpc::XmlRpcValue::ValueArray *a)
void setStruct(XmlRpc::XmlRpcValue::ValueStruct *a)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
void transformFootprint(double x, double y, double theta, const std::vector< geometry_msgs::Point > &footprint_spec, std::vector< geometry_msgs::Point > &oriented_footprint)
Given a pose and base footprint, build the oriented footprint of the robot (list of Points) ...
std::vector< geometry_msgs::Point > padded_footprint_
void publishCostmap()
Publishes the visualization data over ROS.
void resetOldParameters(ros::NodeHandle &nh)
void resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
std::vector< geometry_msgs::Point > unpadded_footprint_