00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <costmap_2d/layered_costmap.h>
00039 #include <costmap_2d/costmap_2d_ros.h>
00040 #include <cstdio>
00041 #include <string>
00042 #include <algorithm>
00043 #include <vector>
00044
00045
00046 using namespace std;
00047
00048 namespace costmap_2d
00049 {
00050
00051 void move_parameter(ros::NodeHandle& old_h, ros::NodeHandle& new_h, std::string name, bool should_delete = true)
00052 {
00053 if (!old_h.hasParam(name))
00054 return;
00055
00056 XmlRpc::XmlRpcValue value;
00057 old_h.getParam(name, value);
00058 new_h.setParam(name, value);
00059 if (should_delete) old_h.deleteParam(name);
00060 }
00061
00062 Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) :
00063 layered_costmap_(NULL),
00064 name_(name),
00065 tf_(tf),
00066 transform_tolerance_(0.3),
00067 map_update_thread_shutdown_(false),
00068 stop_updates_(false),
00069 initialized_(true),
00070 stopped_(false),
00071 robot_stopped_(false),
00072 map_update_thread_(NULL),
00073 last_publish_(0),
00074 plugin_loader_("costmap_2d", "costmap_2d::Layer"),
00075 publisher_(NULL),
00076 dsrv_(NULL),
00077 footprint_padding_(0.0)
00078 {
00079
00080 old_pose_.setIdentity();
00081 old_pose_.setOrigin(tf::Vector3(1e30, 1e30, 1e30));
00082
00083 ros::NodeHandle private_nh("~/" + name);
00084 ros::NodeHandle g_nh;
00085
00086
00087 ros::NodeHandle prefix_nh;
00088 std::string tf_prefix = tf::getPrefixParam(prefix_nh);
00089
00090
00091 private_nh.param("global_frame", global_frame_, std::string("/map"));
00092 private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
00093
00094
00095 global_frame_ = tf::resolve(tf_prefix, global_frame_);
00096 robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_);
00097
00098 ros::Time last_error = ros::Time::now();
00099 std::string tf_error;
00100
00101 while (ros::ok()
00102 && !tf_.waitForTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), ros::Duration(0.01),
00103 &tf_error))
00104 {
00105 ros::spinOnce();
00106 if (last_error + ros::Duration(5.0) < ros::Time::now())
00107 {
00108 ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
00109 robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
00110 last_error = ros::Time::now();
00111 }
00112
00113
00114 tf_error.clear();
00115 }
00116
00117
00118 bool rolling_window, track_unknown_space, always_send_full_costmap;
00119 private_nh.param("rolling_window", rolling_window, false);
00120 private_nh.param("track_unknown_space", track_unknown_space, false);
00121 private_nh.param("always_send_full_costmap", always_send_full_costmap, false);
00122
00123 layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
00124
00125 if (!private_nh.hasParam("plugins"))
00126 {
00127 resetOldParameters(private_nh);
00128 }
00129
00130 if (private_nh.hasParam("plugins"))
00131 {
00132 XmlRpc::XmlRpcValue my_list;
00133 private_nh.getParam("plugins", my_list);
00134 for (int32_t i = 0; i < my_list.size(); ++i)
00135 {
00136 std::string pname = static_cast<std::string>(my_list[i]["name"]);
00137 std::string type = static_cast<std::string>(my_list[i]["type"]);
00138 ROS_INFO("Using plugin \"%s\"", pname.c_str());
00139
00140 boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
00141 layered_costmap_->addPlugin(plugin);
00142 plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
00143 }
00144 }
00145
00146
00147 std::string topic_param, topic;
00148 if (!private_nh.searchParam("footprint_topic", topic_param))
00149 {
00150 topic_param = "footprint_topic";
00151 }
00152
00153 private_nh.param(topic_param, topic, std::string("footprint"));
00154 footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this);
00155
00156 if (!private_nh.searchParam("published_footprint_topic", topic_param))
00157 {
00158 topic_param = "published_footprint";
00159 }
00160
00161 private_nh.param(topic_param, topic, std::string("oriented_footprint"));
00162 footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>("footprint", 1);
00163
00164 setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));
00165
00166 publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
00167 always_send_full_costmap);
00168
00169
00170 stop_updates_ = false;
00171 initialized_ = true;
00172 stopped_ = false;
00173
00174
00175 robot_stopped_ = false;
00176 double pose_update_frequency;
00177 private_nh.param("pose_update_frequency", pose_update_frequency, 10.0);
00178 timer_ = private_nh.createTimer(ros::Duration(1 / pose_update_frequency),
00179 &Costmap2DROS::movementCB, this);
00180
00181 dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
00182 dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1,
00183 _2);
00184 dsrv_->setCallback(cb);
00185 }
00186
00187 void Costmap2DROS::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
00188 {
00189 setUnpaddedRobotFootprint(toPointVector(footprint));
00190 }
00191
00192 Costmap2DROS::~Costmap2DROS()
00193 {
00194 map_update_thread_shutdown_ = true;
00195 if (map_update_thread_ != NULL)
00196 {
00197 map_update_thread_->join();
00198 delete map_update_thread_;
00199 }
00200 if (publisher_ != NULL)
00201 delete publisher_;
00202
00203 delete layered_costmap_;
00204 delete dsrv_;
00205 }
00206
00207 void Costmap2DROS::resetOldParameters(ros::NodeHandle& nh)
00208 {
00209 ROS_INFO("Loading from pre-hydro parameter style");
00210 bool flag;
00211 std::string s;
00212 std::vector < XmlRpc::XmlRpcValue > plugins;
00213
00214 XmlRpc::XmlRpcValue::ValueStruct map;
00215 SuperValue super_map;
00216 SuperValue super_array;
00217
00218 if (nh.getParam("static_map", flag) && flag)
00219 {
00220 map["name"] = XmlRpc::XmlRpcValue("static_layer");
00221 map["type"] = XmlRpc::XmlRpcValue("costmap_2d::StaticLayer");
00222 super_map.setStruct(&map);
00223 plugins.push_back(super_map);
00224
00225 ros::NodeHandle map_layer(nh, "static_layer");
00226 move_parameter(nh, map_layer, "map_topic");
00227 move_parameter(nh, map_layer, "unknown_cost_value");
00228 move_parameter(nh, map_layer, "lethal_cost_threshold");
00229 move_parameter(nh, map_layer, "track_unknown_space", false);
00230 }
00231
00232 ros::NodeHandle obstacles(nh, "obstacle_layer");
00233 if (nh.getParam("map_type", s) && s == "voxel")
00234 {
00235 map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
00236 map["type"] = XmlRpc::XmlRpcValue("costmap_2d::VoxelLayer");
00237 super_map.setStruct(&map);
00238 plugins.push_back(super_map);
00239
00240 move_parameter(nh, obstacles, "origin_z");
00241 move_parameter(nh, obstacles, "z_resolution");
00242 move_parameter(nh, obstacles, "z_voxels");
00243 move_parameter(nh, obstacles, "mark_threshold");
00244 move_parameter(nh, obstacles, "unknown_threshold");
00245 move_parameter(nh, obstacles, "publish_voxel_map");
00246 }
00247 else
00248 {
00249 map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
00250 map["type"] = XmlRpc::XmlRpcValue("costmap_2d::ObstacleLayer");
00251 super_map.setStruct(&map);
00252 plugins.push_back(super_map);
00253 }
00254
00255 move_parameter(nh, obstacles, "max_obstacle_height");
00256 move_parameter(nh, obstacles, "raytrace_range");
00257 move_parameter(nh, obstacles, "obstacle_range");
00258 move_parameter(nh, obstacles, "track_unknown_space", true);
00259 nh.param("observation_sources", s, std::string(""));
00260 std::stringstream ss(s);
00261 std::string source;
00262 while (ss >> source)
00263 {
00264 move_parameter(nh, obstacles, source);
00265 }
00266 move_parameter(nh, obstacles, "observation_sources");
00267
00268 ros::NodeHandle inflation(nh, "inflation_layer");
00269 move_parameter(nh, inflation, "cost_scaling_factor");
00270 move_parameter(nh, inflation, "inflation_radius");
00271 map["name"] = XmlRpc::XmlRpcValue("inflation_layer");
00272 map["type"] = XmlRpc::XmlRpcValue("costmap_2d::InflationLayer");
00273 super_map.setStruct(&map);
00274 plugins.push_back(super_map);
00275
00276 super_array.setArray(&plugins);
00277 nh.setParam("plugins", super_array);
00278 }
00279
00280 void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
00281 {
00282 transform_tolerance_ = config.transform_tolerance;
00283 if (map_update_thread_ != NULL)
00284 {
00285 map_update_thread_shutdown_ = true;
00286 map_update_thread_->join();
00287 delete map_update_thread_;
00288 }
00289 map_update_thread_shutdown_ = false;
00290 double map_update_frequency = config.update_frequency;
00291
00292 double map_publish_frequency = config.publish_frequency;
00293 if (map_publish_frequency > 0)
00294 publish_cycle = ros::Duration(1 / map_publish_frequency);
00295 else
00296 publish_cycle = ros::Duration(-1);
00297
00298
00299 double map_width_meters = config.width, map_height_meters = config.height, resolution = config.resolution, origin_x =
00300 config.origin_x,
00301 origin_y = config.origin_y;
00302
00303 if (!layered_costmap_->isSizeLocked())
00304 {
00305 layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
00306 (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
00307 }
00308
00309
00310
00311 if (footprint_padding_ != config.footprint_padding)
00312 {
00313 footprint_padding_ = config.footprint_padding;
00314 setUnpaddedRobotFootprint(unpadded_footprint_);
00315 }
00316
00317 readFootprintFromConfig(config, old_config_);
00318
00319 old_config_ = config;
00320
00321 map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
00322 }
00323
00324 void Costmap2DROS::readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config,
00325 const costmap_2d::Costmap2DConfig &old_config)
00326 {
00327
00328
00329
00330
00331 if (new_config.footprint == old_config.footprint &&
00332 new_config.robot_radius == old_config.robot_radius)
00333 {
00334 return;
00335 }
00336
00337 if (new_config.footprint != "" && new_config.footprint != "[]")
00338 {
00339 std::vector<geometry_msgs::Point> new_footprint;
00340 if (makeFootprintFromString(new_config.footprint, new_footprint))
00341 {
00342 setUnpaddedRobotFootprint(new_footprint);
00343 }
00344 else
00345 {
00346 ROS_ERROR("Invalid footprint string from dynamic reconfigure");
00347 }
00348 }
00349 else
00350 {
00351
00352 setUnpaddedRobotFootprint(makeFootprintFromRadius(new_config.robot_radius));
00353 }
00354 }
00355
00356 void Costmap2DROS::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)
00357 {
00358 unpadded_footprint_ = points;
00359 padded_footprint_ = points;
00360 padFootprint(padded_footprint_, footprint_padding_);
00361
00362 layered_costmap_->setFootprint(padded_footprint_);
00363 }
00364
00365 void Costmap2DROS::movementCB(const ros::TimerEvent &event)
00366 {
00367
00368
00369
00370 tf::Stamped < tf::Pose > new_pose;
00371
00372 if (!getRobotPose(new_pose))
00373 {
00374 ROS_WARN_THROTTLE(1.0, "Could not get robot pose, cancelling pose reconfiguration");
00375 robot_stopped_ = false;
00376 }
00377
00378 else if (fabs((old_pose_.getOrigin() - new_pose.getOrigin()).length()) < 1e-3
00379 && fabs(old_pose_.getRotation().angle(new_pose.getRotation())) < 1e-3)
00380 {
00381 old_pose_ = new_pose;
00382 robot_stopped_ = true;
00383 }
00384 else
00385 {
00386 old_pose_ = new_pose;
00387 robot_stopped_ = false;
00388 }
00389 }
00390
00391 void Costmap2DROS::mapUpdateLoop(double frequency)
00392 {
00393
00394 if (frequency == 0.0)
00395 return;
00396
00397 ros::NodeHandle nh;
00398 ros::Rate r(frequency);
00399 while (nh.ok() && !map_update_thread_shutdown_)
00400 {
00401 struct timeval start, end;
00402 double start_t, end_t, t_diff;
00403 gettimeofday(&start, NULL);
00404
00405 updateMap();
00406
00407 gettimeofday(&end, NULL);
00408 start_t = start.tv_sec + double(start.tv_usec) / 1e6;
00409 end_t = end.tv_sec + double(end.tv_usec) / 1e6;
00410 t_diff = end_t - start_t;
00411 ROS_DEBUG("Map update time: %.9f", t_diff);
00412 if (publish_cycle.toSec() > 0 && layered_costmap_->isInitialized())
00413 {
00414 unsigned int x0, y0, xn, yn;
00415 layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
00416 publisher_->updateBounds(x0, xn, y0, yn);
00417
00418 ros::Time now = ros::Time::now();
00419 if (last_publish_ + publish_cycle < now)
00420 {
00421 publisher_->publishCostmap();
00422 last_publish_ = now;
00423 }
00424 }
00425 r.sleep();
00426
00427 if (r.cycleTime() > ros::Duration(1 / frequency))
00428 ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,
00429 r.cycleTime().toSec());
00430 }
00431 }
00432
00433 void Costmap2DROS::updateMap()
00434 {
00435 if (!stop_updates_)
00436 {
00437
00438 tf::Stamped < tf::Pose > pose;
00439 if (getRobotPose (pose))
00440 {
00441 double x = pose.getOrigin().x(),
00442 y = pose.getOrigin().y(),
00443 yaw = tf::getYaw(pose.getRotation());
00444
00445 layered_costmap_->updateMap(x, y, yaw);
00446
00447 geometry_msgs::PolygonStamped footprint;
00448 footprint.header.frame_id = global_frame_;
00449 footprint.header.stamp = ros::Time::now();
00450 transformFootprint(x, y, yaw, padded_footprint_, footprint);
00451 footprint_pub_.publish(footprint);
00452
00453 initialized_ = true;
00454 }
00455 }
00456 }
00457
00458 void Costmap2DROS::start()
00459 {
00460 std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
00461
00462 if (stopped_)
00463 {
00464
00465 for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
00466 ++plugin)
00467 {
00468 (*plugin)->activate();
00469 }
00470 stopped_ = false;
00471 }
00472 stop_updates_ = false;
00473
00474
00475 ros::Rate r(100.0);
00476 while (ros::ok() && !initialized_)
00477 r.sleep();
00478 }
00479
00480 void Costmap2DROS::stop()
00481 {
00482 stop_updates_ = true;
00483 std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
00484
00485 for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
00486 ++plugin)
00487 {
00488 (*plugin)->deactivate();
00489 }
00490 initialized_ = false;
00491 stopped_ = true;
00492 }
00493
00494 void Costmap2DROS::pause()
00495 {
00496 stop_updates_ = true;
00497 initialized_ = false;
00498 }
00499
00500 void Costmap2DROS::resume()
00501 {
00502 stop_updates_ = false;
00503
00504
00505 ros::Rate r(100.0);
00506 while (!initialized_)
00507 r.sleep();
00508 }
00509
00510
00511 void Costmap2DROS::resetLayers()
00512 {
00513 Costmap2D* top = layered_costmap_->getCostmap();
00514 top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());
00515 std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
00516 for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
00517 ++plugin)
00518 {
00519 (*plugin)->reset();
00520 }
00521 }
00522
00523 bool Costmap2DROS::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
00524 {
00525 global_pose.setIdentity();
00526 tf::Stamped < tf::Pose > robot_pose;
00527 robot_pose.setIdentity();
00528 robot_pose.frame_id_ = robot_base_frame_;
00529 robot_pose.stamp_ = ros::Time();
00530 ros::Time current_time = ros::Time::now();
00531
00532
00533 try
00534 {
00535 tf_.transformPose(global_frame_, robot_pose, global_pose);
00536 }
00537 catch (tf::LookupException& ex)
00538 {
00539 ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
00540 return false;
00541 }
00542 catch (tf::ConnectivityException& ex)
00543 {
00544 ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
00545 return false;
00546 }
00547 catch (tf::ExtrapolationException& ex)
00548 {
00549 ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
00550 return false;
00551 }
00552
00553 if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_)
00554 {
00555 ROS_WARN_THROTTLE(1.0,
00556 "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
00557 current_time.toSec(), global_pose.stamp_.toSec(), transform_tolerance_);
00558 return false;
00559 }
00560
00561 return true;
00562 }
00563
00564 void Costmap2DROS::getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const
00565 {
00566 tf::Stamped<tf::Pose> global_pose;
00567 if (!getRobotPose(global_pose))
00568 return;
00569
00570 double yaw = tf::getYaw(global_pose.getRotation());
00571 transformFootprint(global_pose.getOrigin().x(), global_pose.getOrigin().y(), yaw,
00572 padded_footprint_, oriented_footprint);
00573 }
00574
00575 }