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
00039
00040
00041 #include <nav_msgs/Path.h>
00042 #include <geometry_msgs/PoseArray.h>
00043 #include <costmap_2d/costmap_2d_ros.h>
00044 #include <base_local_planner/footprint_helper.h>
00045 #include <mbf_msgs/MoveBaseAction.h>
00046 #include <mbf_abstract_nav/MoveBaseFlexConfig.h>
00047 #include <actionlib/client/simple_action_client.h>
00048 #include <nav_core_wrapper/wrapper_global_planner.h>
00049 #include <nav_core_wrapper/wrapper_local_planner.h>
00050 #include <nav_core_wrapper/wrapper_recovery_behavior.h>
00051
00052 #include "mbf_costmap_nav/costmap_navigation_server.h"
00053
00054 namespace mbf_costmap_nav
00055 {
00056
00057
00058 CostmapNavigationServer::CostmapNavigationServer(const TFPtr &tf_listener_ptr) :
00059 AbstractNavigationServer(tf_listener_ptr),
00060 recovery_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapRecovery"),
00061 nav_core_recovery_plugin_loader_("nav_core", "nav_core::RecoveryBehavior"),
00062 controller_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapController"),
00063 nav_core_controller_plugin_loader_("nav_core", "nav_core::BaseLocalPlanner"),
00064 planner_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapPlanner"),
00065 nav_core_planner_plugin_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
00066 global_costmap_ptr_(new costmap_2d::Costmap2DROS("global_costmap", *tf_listener_ptr_)),
00067 local_costmap_ptr_(new costmap_2d::Costmap2DROS("local_costmap", *tf_listener_ptr_)),
00068 setup_reconfigure_(false), shutdown_costmaps_(false), costmaps_users_(0)
00069 {
00070
00071
00072 private_nh_.param("shutdown_costmaps", shutdown_costmaps_, false);
00073
00074
00075 if (shutdown_costmaps_)
00076 {
00077 local_costmap_ptr_->stop();
00078 global_costmap_ptr_->stop();
00079 }
00080
00081
00082 check_point_cost_srv_ = private_nh_.advertiseService("check_point_cost",
00083 &CostmapNavigationServer::callServiceCheckPointCost, this);
00084 check_pose_cost_srv_ = private_nh_.advertiseService("check_pose_cost",
00085 &CostmapNavigationServer::callServiceCheckPoseCost, this);
00086 check_path_cost_srv_ = private_nh_.advertiseService("check_path_cost",
00087 &CostmapNavigationServer::callServiceCheckPathCost, this);
00088 clear_costmaps_srv_ = private_nh_.advertiseService("clear_costmaps",
00089 &CostmapNavigationServer::callServiceClearCostmaps, this);
00090
00091
00092 dsrv_costmap_ = boost::make_shared<dynamic_reconfigure::Server<mbf_costmap_nav::MoveBaseFlexConfig> >(private_nh_);
00093 dsrv_costmap_->setCallback(boost::bind(&CostmapNavigationServer::reconfigure, this, _1, _2));
00094
00095
00096 initializeServerComponents();
00097
00098
00099 startActionServers();
00100 }
00101
00102 mbf_abstract_nav::AbstractPlannerExecution::Ptr CostmapNavigationServer::newPlannerExecution(
00103 const std::string name,
00104 const mbf_abstract_core::AbstractPlanner::Ptr plugin_ptr)
00105 {
00106 return boost::make_shared<mbf_costmap_nav::CostmapPlannerExecution>(
00107 name,
00108 boost::static_pointer_cast<mbf_costmap_core::CostmapPlanner>(plugin_ptr),
00109 boost::ref(global_costmap_ptr_),
00110 last_config_,
00111 boost::bind(&CostmapNavigationServer::checkActivateCostmaps, this),
00112 boost::bind(&CostmapNavigationServer::checkDeactivateCostmaps, this));
00113 }
00114
00115 mbf_abstract_nav::AbstractControllerExecution::Ptr CostmapNavigationServer::newControllerExecution(
00116 const std::string name,
00117 const mbf_abstract_core::AbstractController::Ptr plugin_ptr)
00118 {
00119 return boost::make_shared<mbf_costmap_nav::CostmapControllerExecution>(
00120 name,
00121 boost::static_pointer_cast<mbf_costmap_core::CostmapController>(plugin_ptr),
00122 vel_pub_,
00123 goal_pub_,
00124 tf_listener_ptr_,
00125 boost::ref(local_costmap_ptr_),
00126 last_config_,
00127 boost::bind(&CostmapNavigationServer::checkActivateCostmaps, this),
00128 boost::bind(&CostmapNavigationServer::checkDeactivateCostmaps, this));
00129 }
00130
00131 mbf_abstract_nav::AbstractRecoveryExecution::Ptr CostmapNavigationServer::newRecoveryExecution(
00132 const std::string name,
00133 const mbf_abstract_core::AbstractRecovery::Ptr plugin_ptr)
00134 {
00135 return boost::make_shared<mbf_costmap_nav::CostmapRecoveryExecution>(
00136 name,
00137 boost::static_pointer_cast<mbf_costmap_core::CostmapRecovery>(plugin_ptr),
00138 tf_listener_ptr_,
00139 boost::ref(global_costmap_ptr_),
00140 boost::ref(local_costmap_ptr_),
00141 last_config_,
00142 boost::bind(&CostmapNavigationServer::checkActivateCostmaps, this),
00143 boost::bind(&CostmapNavigationServer::checkDeactivateCostmaps, this));
00144 }
00145
00146 mbf_abstract_core::AbstractPlanner::Ptr CostmapNavigationServer::loadPlannerPlugin(const std::string& planner_type)
00147 {
00148 mbf_abstract_core::AbstractPlanner::Ptr planner_ptr;
00149 try
00150 {
00151 planner_ptr = boost::static_pointer_cast<mbf_abstract_core::AbstractPlanner>(
00152 planner_plugin_loader_.createInstance(planner_type));
00153 std::string planner_name = planner_plugin_loader_.getName(planner_type);
00154 ROS_DEBUG_STREAM("mbf_costmap_core-based planner plugin " << planner_name << " loaded.");
00155 }
00156 catch (const pluginlib::PluginlibException &ex_mbf_core)
00157 {
00158 ROS_DEBUG_STREAM("Failed to load the " << planner_type << " planner as a mbf_costmap_core-based plugin."
00159 << " Try to load as a nav_core-based plugin. " << ex_mbf_core.what());
00160 try
00161 {
00162
00163 boost::shared_ptr<nav_core::BaseGlobalPlanner> nav_core_planner_ptr = nav_core_planner_plugin_loader_.createInstance(planner_type);
00164 planner_ptr = boost::make_shared<mbf_nav_core_wrapper::WrapperGlobalPlanner>(nav_core_planner_ptr);
00165 std::string planner_name = nav_core_planner_plugin_loader_.getName(planner_type);
00166 ROS_DEBUG_STREAM("nav_core-based planner plugin " << planner_name << " loaded");
00167 }
00168 catch (const pluginlib::PluginlibException &ex_nav_core)
00169 {
00170 ROS_FATAL_STREAM("Failed to load the " << planner_type << " planner, are you sure it's properly registered"
00171 << " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what());
00172 }
00173 }
00174
00175 return planner_ptr;
00176 }
00177
00178 bool CostmapNavigationServer::initializePlannerPlugin(
00179 const std::string& name,
00180 const mbf_abstract_core::AbstractPlanner::Ptr& planner_ptr
00181 )
00182 {
00183 mbf_costmap_core::CostmapPlanner::Ptr costmap_planner_ptr
00184 = boost::static_pointer_cast<mbf_costmap_core::CostmapPlanner>(planner_ptr);
00185 ROS_DEBUG_STREAM("Initialize planner \"" << name << "\".");
00186
00187 if (!global_costmap_ptr_)
00188 {
00189 ROS_FATAL_STREAM("The costmap pointer has not been initialized!");
00190 return false;
00191 }
00192
00193 costmap_planner_ptr->initialize(name, global_costmap_ptr_.get());
00194 ROS_DEBUG("Planner plugin initialized.");
00195 return true;
00196 }
00197
00198
00199 mbf_abstract_core::AbstractController::Ptr CostmapNavigationServer::loadControllerPlugin(const std::string& controller_type)
00200 {
00201 mbf_abstract_core::AbstractController::Ptr controller_ptr;
00202 try
00203 {
00204 controller_ptr = controller_plugin_loader_.createInstance(controller_type);
00205 std::string controller_name = controller_plugin_loader_.getName(controller_type);
00206 ROS_DEBUG_STREAM("mbf_costmap_core-based controller plugin " << controller_name << " loaded.");
00207 }
00208 catch (const pluginlib::PluginlibException &ex_mbf_core)
00209 {
00210 ROS_DEBUG_STREAM("Failed to load the " << controller_type << " controller as a mbf_costmap_core-based plugin;"
00211 << " we will retry to load as a nav_core-based plugin. " << ex_mbf_core.what());
00212 try
00213 {
00214
00215 boost::shared_ptr<nav_core::BaseLocalPlanner> nav_core_controller_ptr
00216 = nav_core_controller_plugin_loader_.createInstance(controller_type);
00217 controller_ptr = boost::make_shared<mbf_nav_core_wrapper::WrapperLocalPlanner>(nav_core_controller_ptr);
00218 std::string controller_name = nav_core_controller_plugin_loader_.getName(controller_type);
00219 ROS_DEBUG_STREAM("nav_core-based controller plugin " << controller_name << " loaded.");
00220 }
00221 catch (const pluginlib::PluginlibException &ex_nav_core)
00222 {
00223 ROS_FATAL_STREAM("Failed to load the " << controller_type << " controller, are you sure it's properly registered"
00224 << " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what());
00225 }
00226 }
00227 return controller_ptr;
00228 }
00229
00230 bool CostmapNavigationServer::initializeControllerPlugin(
00231 const std::string& name,
00232 const mbf_abstract_core::AbstractController::Ptr& controller_ptr)
00233 {
00234 ROS_DEBUG_STREAM("Initialize controller \"" << name << "\".");
00235
00236 if (!tf_listener_ptr_)
00237 {
00238 ROS_FATAL_STREAM("The tf listener pointer has not been initialized!");
00239 return false;
00240 }
00241
00242 if (!local_costmap_ptr_)
00243 {
00244 ROS_FATAL_STREAM("The costmap pointer has not been initialized!");
00245 return false;
00246 }
00247
00248 mbf_costmap_core::CostmapController::Ptr costmap_controller_ptr
00249 = boost::static_pointer_cast<mbf_costmap_core::CostmapController>(controller_ptr);
00250 costmap_controller_ptr->initialize(name, tf_listener_ptr_.get(), local_costmap_ptr_.get());
00251 ROS_DEBUG_STREAM("Controller plugin \"" << name << "\" initialized.");
00252 return true;
00253 }
00254
00255 mbf_abstract_core::AbstractRecovery::Ptr CostmapNavigationServer::loadRecoveryPlugin(
00256 const std::string& recovery_type)
00257 {
00258 mbf_abstract_core::AbstractRecovery::Ptr recovery_ptr;
00259
00260 try
00261 {
00262 recovery_ptr = boost::static_pointer_cast<mbf_abstract_core::AbstractRecovery>(
00263 recovery_plugin_loader_.createInstance(recovery_type));
00264 std::string recovery_name = recovery_plugin_loader_.getName(recovery_type);
00265 ROS_DEBUG_STREAM("mbf_costmap_core-based recovery behavior plugin " << recovery_name << " loaded.");
00266 }
00267 catch (pluginlib::PluginlibException &ex_mbf_core)
00268 {
00269 ROS_DEBUG_STREAM("Failed to load the " << recovery_type << " recovery behavior as a mbf_costmap_core-based plugin;"
00270 << " Retry to load as a nav_core-based plugin. " << ex_mbf_core.what());
00271 try
00272 {
00273
00274 boost::shared_ptr<nav_core::RecoveryBehavior> nav_core_recovery_ptr =
00275 nav_core_recovery_plugin_loader_.createInstance(recovery_type);
00276
00277 recovery_ptr = boost::make_shared<mbf_nav_core_wrapper::WrapperRecoveryBehavior>(nav_core_recovery_ptr);
00278 std::string recovery_name = recovery_plugin_loader_.getName(recovery_type);
00279 ROS_DEBUG_STREAM("nav_core-based recovery behavior plugin " << recovery_name << " loaded.");
00280
00281 }
00282 catch (const pluginlib::PluginlibException &ex_nav_core)
00283 {
00284 ROS_FATAL_STREAM("Failed to load the " << recovery_type << " recovery behavior, are you sure it's properly registered"
00285 << " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what());
00286 }
00287 }
00288
00289 return recovery_ptr;
00290 }
00291
00292 bool CostmapNavigationServer::initializeRecoveryPlugin(
00293 const std::string& name,
00294 const mbf_abstract_core::AbstractRecovery::Ptr& behavior_ptr)
00295 {
00296 ROS_DEBUG_STREAM("Initialize recovery behavior \"" << name << "\".");
00297
00298 if (!tf_listener_ptr_)
00299 {
00300 ROS_FATAL_STREAM("The tf listener pointer has not been initialized!");
00301 return false;
00302 }
00303
00304 if (!local_costmap_ptr_)
00305 {
00306 ROS_FATAL_STREAM("The local costmap pointer has not been initialized!");
00307 return false;
00308 }
00309
00310 if (!global_costmap_ptr_)
00311 {
00312 ROS_FATAL_STREAM("The global costmap pointer has not been initialized!");
00313 return false;
00314 }
00315
00316 mbf_costmap_core::CostmapRecovery::Ptr behavior =
00317 boost::static_pointer_cast<mbf_costmap_core::CostmapRecovery>(behavior_ptr);
00318 behavior->initialize(name, tf_listener_ptr_.get(), global_costmap_ptr_.get(), local_costmap_ptr_.get());
00319 ROS_DEBUG_STREAM("Recovery behavior plugin \"" << name << "\" initialized.");
00320 return true;
00321 }
00322
00323
00324 void CostmapNavigationServer::stop()
00325 {
00326 AbstractNavigationServer::stop();
00327 ROS_INFO_STREAM_NAMED("mbf_costmap_nav", "Stopping local and global costmap for shutdown");
00328 local_costmap_ptr_->stop();
00329 global_costmap_ptr_->stop();
00330 }
00331
00332
00333 CostmapNavigationServer::~CostmapNavigationServer()
00334 {
00335 }
00336
00337 void CostmapNavigationServer::reconfigure(mbf_costmap_nav::MoveBaseFlexConfig &config, uint32_t level)
00338 {
00339
00340 if (!setup_reconfigure_)
00341 {
00342 default_config_ = config;
00343 setup_reconfigure_ = true;
00344 }
00345
00346 if (config.restore_defaults)
00347 {
00348 config = default_config_;
00349
00350 config.restore_defaults = false;
00351 }
00352
00353
00354 mbf_abstract_nav::MoveBaseFlexConfig abstract_config;
00355 abstract_config.planner_frequency = config.planner_frequency;
00356 abstract_config.planner_patience = config.planner_patience;
00357 abstract_config.planner_max_retries = config.planner_max_retries;
00358 abstract_config.controller_frequency = config.controller_frequency;
00359 abstract_config.controller_patience = config.controller_patience;
00360 abstract_config.controller_max_retries = config.controller_max_retries;
00361 abstract_config.recovery_enabled = config.recovery_enabled;
00362 abstract_config.recovery_patience = config.recovery_patience;
00363 abstract_config.oscillation_timeout = config.oscillation_timeout;
00364 abstract_config.oscillation_distance = config.oscillation_distance;
00365 abstract_config.restore_defaults = config.restore_defaults;
00366 mbf_abstract_nav::AbstractNavigationServer::reconfigure(abstract_config, level);
00367
00368
00369 shutdown_costmaps_delay_ = ros::Duration(config.shutdown_costmaps_delay);
00370 if (shutdown_costmaps_delay_.isZero())
00371 ROS_WARN("Zero shutdown costmaps delay is not recommended, as it forces us to enable costmaps on each action");
00372
00373 if (shutdown_costmaps_ && !config.shutdown_costmaps)
00374 {
00375 checkActivateCostmaps();
00376 shutdown_costmaps_ = config.shutdown_costmaps;
00377 }
00378 if (!shutdown_costmaps_ && config.shutdown_costmaps)
00379 {
00380 shutdown_costmaps_ = config.shutdown_costmaps;
00381 checkDeactivateCostmaps();
00382 }
00383
00384 last_config_ = config;
00385 }
00386
00387 bool CostmapNavigationServer::callServiceCheckPointCost(mbf_msgs::CheckPoint::Request &request,
00388 mbf_msgs::CheckPoint::Response &response)
00389 {
00390
00391 CostmapPtr costmap;
00392 std::string costmap_name;
00393 switch (request.costmap)
00394 {
00395 case mbf_msgs::CheckPoint::Request::LOCAL_COSTMAP:
00396 costmap = local_costmap_ptr_;
00397 costmap_name = "local costmap";
00398 break;
00399 case mbf_msgs::CheckPoint::Request::GLOBAL_COSTMAP:
00400 costmap = global_costmap_ptr_;
00401 costmap_name = "global costmap";
00402 break;
00403 default:
00404 ROS_ERROR_STREAM("No valid costmap provided; options are "
00405 << mbf_msgs::CheckPoint::Request::LOCAL_COSTMAP << ": local costmap, "
00406 << mbf_msgs::CheckPoint::Request::GLOBAL_COSTMAP << ": global costmap");
00407 return false;
00408 }
00409
00410
00411 std::string costmap_frame = costmap->getGlobalFrameID();
00412
00413 geometry_msgs::PointStamped point;
00414 if (! mbf_utility::transformPoint(*tf_listener_ptr_, costmap_frame, request.point.header.stamp,
00415 ros::Duration(0.5), request.point, global_frame_, point))
00416 {
00417 ROS_ERROR_STREAM("Transform target point to " << costmap_name << " frame '" << costmap_frame << "' failed");
00418 return false;
00419 }
00420
00421 double x = point.point.x;
00422 double y = point.point.y;
00423
00424
00425 checkActivateCostmaps();
00426 unsigned int mx, my;
00427 if (!costmap->getCostmap()->worldToMap(x, y, mx, my))
00428 {
00429
00430 response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::OUTSIDE);
00431 ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is outside the map (cost = " << response.cost << ")");
00432 }
00433 else
00434 {
00435
00436 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getCostmap()->getMutex()));
00437
00438
00439 response.cost = costmap->getCostmap()->getCost(mx, my);
00440 switch (response.cost)
00441 {
00442 case costmap_2d::NO_INFORMATION:
00443 response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::UNKNOWN);
00444 ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is in unknown space! (cost = " << response.cost << ")");
00445 break;
00446 case costmap_2d::LETHAL_OBSTACLE:
00447 response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::LETHAL);
00448 ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is in collision! (cost = " << response.cost << ")");
00449 break;
00450 case costmap_2d::INSCRIBED_INFLATED_OBSTACLE:
00451 response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::INSCRIBED);
00452 ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is near an obstacle (cost = " << response.cost << ")");
00453 break;
00454 default:
00455 response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::FREE);
00456 ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is free (cost = " << response.cost << ")");
00457 break;
00458 }
00459 }
00460
00461 checkDeactivateCostmaps();
00462 return true;
00463 }
00464
00465 bool CostmapNavigationServer::callServiceCheckPoseCost(mbf_msgs::CheckPose::Request &request,
00466 mbf_msgs::CheckPose::Response &response)
00467 {
00468
00469 CostmapPtr costmap;
00470 std::string costmap_name;
00471 switch (request.costmap)
00472 {
00473 case mbf_msgs::CheckPose::Request::LOCAL_COSTMAP:
00474 costmap = local_costmap_ptr_;
00475 costmap_name = "local costmap";
00476 break;
00477 case mbf_msgs::CheckPose::Request::GLOBAL_COSTMAP:
00478 costmap = global_costmap_ptr_;
00479 costmap_name = "global costmap";
00480 break;
00481 default:
00482 ROS_ERROR_STREAM("No valid costmap provided; options are "
00483 << mbf_msgs::CheckPose::Request::LOCAL_COSTMAP << ": local costmap, "
00484 << mbf_msgs::CheckPose::Request::GLOBAL_COSTMAP << ": global costmap");
00485 return false;
00486 }
00487
00488
00489 std::string costmap_frame = costmap->getGlobalFrameID();
00490
00491 geometry_msgs::PoseStamped pose;
00492 if (request.current_pose)
00493 {
00494 if (! mbf_utility::getRobotPose(*tf_listener_ptr_, robot_frame_, costmap_frame, ros::Duration(0.5), pose))
00495 {
00496 ROS_ERROR_STREAM("Get robot pose on " << costmap_name << " frame '" << costmap_frame << "' failed");
00497 return false;
00498 }
00499 }
00500 else
00501 {
00502 if (! mbf_utility::transformPose(*tf_listener_ptr_, costmap_frame, request.pose.header.stamp,
00503 ros::Duration(0.5), request.pose, global_frame_, pose))
00504 {
00505 ROS_ERROR_STREAM("Transform target pose to " << costmap_name << " frame '" << costmap_frame << "' failed");
00506 return false;
00507 }
00508 }
00509
00510 double x = pose.pose.position.x;
00511 double y = pose.pose.position.y;
00512 double yaw = tf::getYaw(pose.pose.orientation);
00513
00514
00515 checkActivateCostmaps();
00516
00517
00518 std::vector<geometry_msgs::Point> footprint = costmap->getUnpaddedRobotFootprint();
00519 costmap_2d::padFootprint(footprint, request.safety_dist);
00520
00521
00522 base_local_planner::FootprintHelper fph;
00523 std::vector<base_local_planner::Position2DInt> footprint_cells =
00524 fph.getFootprintCells(Eigen::Vector3f(x, y, yaw), footprint, *costmap->getCostmap(), true);
00525 response.state = mbf_msgs::CheckPose::Response::FREE;
00526 if (footprint_cells.empty())
00527 {
00528
00529 response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::OUTSIDE));
00530 }
00531 else
00532 {
00533
00534 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getCostmap()->getMutex()));
00535
00536
00537 for (int i = 0; i < footprint_cells.size(); ++i)
00538 {
00539 unsigned char cost = costmap->getCostmap()->getCost(footprint_cells[i].x, footprint_cells[i].y);
00540 switch (cost)
00541 {
00542 case costmap_2d::NO_INFORMATION:
00543 response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::UNKNOWN));
00544 response.cost += cost * (request.unknown_cost_mult ? request.unknown_cost_mult : 1.0);
00545 break;
00546 case costmap_2d::LETHAL_OBSTACLE:
00547 response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::LETHAL));
00548 response.cost += cost * (request.lethal_cost_mult ? request.lethal_cost_mult : 1.0);
00549 break;
00550 case costmap_2d::INSCRIBED_INFLATED_OBSTACLE:
00551 response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::INSCRIBED));
00552 response.cost += cost * (request.inscrib_cost_mult ? request.inscrib_cost_mult : 1.0);
00553 break;
00554 default:response.cost += cost;
00555 break;
00556 }
00557 }
00558 }
00559
00560
00561 switch (response.state)
00562 {
00563 case mbf_msgs::CheckPose::Response::OUTSIDE:
00564 ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is outside the map (cost = " << response.cost
00565 << "; safety distance = " << request.safety_dist << ")");
00566 break;
00567 case mbf_msgs::CheckPose::Response::UNKNOWN:
00568 ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is in unknown space! (cost = " << response.cost
00569 << "; safety distance = " << request.safety_dist << ")");
00570 break;
00571 case mbf_msgs::CheckPose::Response::LETHAL:
00572 ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is in collision! (cost = " << response.cost
00573 << "; safety distance = " << request.safety_dist << ")");
00574 break;
00575 case mbf_msgs::CheckPose::Response::INSCRIBED:
00576 ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is near an obstacle (cost = " << response.cost
00577 << "; safety distance = " << request.safety_dist << ")");
00578 break;
00579 case mbf_msgs::CheckPose::Response::FREE:
00580 ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is free (cost = " << response.cost
00581 << "; safety distance = " << request.safety_dist << ")");
00582 break;
00583 }
00584
00585 checkDeactivateCostmaps();
00586 return true;
00587 }
00588
00589 bool CostmapNavigationServer::callServiceCheckPathCost(mbf_msgs::CheckPath::Request &request,
00590 mbf_msgs::CheckPath::Response &response)
00591 {
00592
00593 CostmapPtr costmap;
00594 std::string costmap_name;
00595 switch (request.costmap)
00596 {
00597 case mbf_msgs::CheckPath::Request::LOCAL_COSTMAP:
00598 costmap = local_costmap_ptr_;
00599 costmap_name = "local costmap";
00600 break;
00601 case mbf_msgs::CheckPath::Request::GLOBAL_COSTMAP:
00602 costmap = global_costmap_ptr_;
00603 costmap_name = "global costmap";
00604 break;
00605 default:ROS_ERROR_STREAM("No valid costmap provided; options are "
00606 << mbf_msgs::CheckPath::Request::LOCAL_COSTMAP << ": local costmap, "
00607 << mbf_msgs::CheckPath::Request::GLOBAL_COSTMAP << ": global costmap");
00608 return false;
00609 }
00610
00611
00612 checkActivateCostmaps();
00613
00614
00615 std::string costmap_frame = costmap->getGlobalFrameID();
00616
00617
00618 base_local_planner::FootprintHelper fph;
00619
00620 std::vector<geometry_msgs::Point> footprint;
00621 if (!request.path_cells_only)
00622 {
00623
00624
00625 footprint = costmap->getUnpaddedRobotFootprint();
00626 costmap_2d::padFootprint(footprint, request.safety_dist);
00627 }
00628
00629
00630 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getCostmap()->getMutex()));
00631
00632 geometry_msgs::PoseStamped pose;
00633
00634 response.state = mbf_msgs::CheckPath::Response::FREE;
00635
00636 for (int i = 0; i < request.path.poses.size(); ++i)
00637 {
00638 response.last_checked = i;
00639
00640 if (! mbf_utility::transformPose(*tf_listener_ptr_, costmap_frame, request.path.header.stamp,
00641 ros::Duration(0.5), request.path.poses[i], global_frame_, pose))
00642 {
00643 ROS_ERROR_STREAM("Transform target pose to " << costmap_name << " frame '" << costmap_frame << "' failed");
00644 return false;
00645 }
00646
00647 double x = pose.pose.position.x;
00648 double y = pose.pose.position.y;
00649 double yaw = tf::getYaw(pose.pose.orientation);
00650 std::vector<base_local_planner::Position2DInt> cells_to_check;
00651 if (request.path_cells_only)
00652 {
00653 base_local_planner::Position2DInt cell;
00654 if (costmap->getCostmap()->worldToMap(x, y, (unsigned int&)cell.x, (unsigned int&)cell.y))
00655 cells_to_check.push_back(cell);
00656 }
00657 else
00658 {
00659 cells_to_check = fph.getFootprintCells(Eigen::Vector3f(x, y, yaw), footprint, *costmap->getCostmap(), true);
00660 }
00661
00662 if (cells_to_check.empty())
00663 {
00664
00665
00666 response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPath::Response::OUTSIDE));
00667 }
00668 else
00669 {
00670
00671
00672 for (int j = 0; j < cells_to_check.size(); ++j)
00673 {
00674 unsigned char cost = costmap->getCostmap()->getCost(cells_to_check[j].x, cells_to_check[j].y);
00675 switch (cost)
00676 {
00677 case costmap_2d::NO_INFORMATION:
00678 response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::UNKNOWN));
00679 response.cost += cost * (request.unknown_cost_mult ? request.unknown_cost_mult : 1.0);
00680 break;
00681 case costmap_2d::LETHAL_OBSTACLE:
00682 response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPath::Response::LETHAL));
00683 response.cost += cost * (request.lethal_cost_mult ? request.lethal_cost_mult : 1.0);
00684 break;
00685 case costmap_2d::INSCRIBED_INFLATED_OBSTACLE:
00686 response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPath::Response::INSCRIBED));
00687 response.cost += cost * (request.inscrib_cost_mult ? request.inscrib_cost_mult : 1.0);
00688 break;
00689 default:response.cost += cost;
00690 break;
00691 }
00692 }
00693 }
00694
00695 if (request.return_on && response.state >= request.return_on)
00696 {
00697
00698 switch (response.state)
00699 {
00700 case mbf_msgs::CheckPath::Response::OUTSIDE:
00701 ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes outside the map "
00702 << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
00703 break;
00704 case mbf_msgs::CheckPath::Response::UNKNOWN:
00705 ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes in unknown space! "
00706 << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
00707 break;
00708 case mbf_msgs::CheckPath::Response::LETHAL:
00709 ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes in collision! "
00710 << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
00711 break;
00712 case mbf_msgs::CheckPath::Response::INSCRIBED:
00713 ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes near an obstacle "
00714 << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
00715 break;
00716 case mbf_msgs::CheckPath::Response::FREE:
00717 ROS_DEBUG_STREAM("Path is entirely free (maximum cost = "
00718 << response.cost << "; safety distance = " << request.safety_dist << ")");
00719 break;
00720 }
00721
00722 break;
00723 }
00724
00725 i += request.skip_poses;
00726 }
00727
00728 checkDeactivateCostmaps();
00729 return true;
00730 }
00731
00732 bool CostmapNavigationServer::callServiceClearCostmaps(std_srvs::Empty::Request &request,
00733 std_srvs::Empty::Response &response)
00734 {
00735
00736 local_costmap_ptr_->getCostmap()->getMutex()->lock();
00737 local_costmap_ptr_->resetLayers();
00738 local_costmap_ptr_->getCostmap()->getMutex()->unlock();
00739
00740 global_costmap_ptr_->getCostmap()->getMutex()->lock();
00741 global_costmap_ptr_->resetLayers();
00742 global_costmap_ptr_->getCostmap()->getMutex()->unlock();
00743 return true;
00744 }
00745
00746 void CostmapNavigationServer::checkActivateCostmaps()
00747 {
00748 boost::mutex::scoped_lock sl(check_costmaps_mutex_);
00749
00750 shutdown_costmaps_timer_.stop();
00751
00752
00753
00754 if (shutdown_costmaps_ && !costmaps_users_)
00755 {
00756 local_costmap_ptr_->start();
00757 global_costmap_ptr_->start();
00758 ROS_DEBUG_STREAM("Costmaps activated.");
00759 }
00760 ++costmaps_users_;
00761 }
00762
00763 void CostmapNavigationServer::checkDeactivateCostmaps()
00764 {
00765 --costmaps_users_;
00766 if (shutdown_costmaps_ && !costmaps_users_)
00767 {
00768
00769
00770
00771 shutdown_costmaps_timer_ =
00772 private_nh_.createTimer(shutdown_costmaps_delay_, &CostmapNavigationServer::deactivateCostmaps, this, true);
00773 }
00774 }
00775
00776 void CostmapNavigationServer::deactivateCostmaps(const ros::TimerEvent &event)
00777 {
00778 boost::mutex::scoped_lock sl(check_costmaps_mutex_);
00779
00780 ROS_ASSERT_MSG(!costmaps_users_, "Deactivating costmaps with %u active users!", costmaps_users_);
00781 local_costmap_ptr_->stop();
00782 global_costmap_ptr_->stop();
00783 ROS_DEBUG_STREAM("Costmaps deactivated.");
00784 }
00785
00786 }