costmap_navigation_server.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  *
11  * 2. Redistributions in binary form must reproduce the above
12  * copyright notice, this list of conditions and the following
13  * disclaimer in the documentation and/or other materials provided
14  * with the distribution.
15  *
16  * 3. Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived
18  * from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  * costmap_navigation_server.cpp
34  *
35  * authors:
36  * Sebastian Pütz <spuetz@uni-osnabrueck.de>
37  * Jorge Santos Simón <santos@magazino.eu>
38  *
39  */
40 
41 #include <tf/tf.h>
42 #include <nav_msgs/Path.h>
43 #include <geometry_msgs/PoseArray.h>
44 #include <mbf_msgs/MoveBaseAction.h>
45 #include <mbf_abstract_nav/MoveBaseFlexConfig.h>
50 #include <xmlrpcpp/XmlRpc.h>
51 
54 
55 namespace mbf_costmap_nav
56 {
59 static std::string getStringElement(const XmlRpc::XmlRpcValue& value, const std::string& tag)
60 {
61  // We have to check manually, since XmlRpc would just return tag if its
62  // missing...
63  if (!value.hasMember(tag))
64  throw XmlRpc::XmlRpcException(tag + " not found");
65 
66  return static_cast<std::string>(value[tag]);
67 }
68 
77 StringToMap loadStringToMapsImpl(const std::string& resource, const ros::NodeHandle& nh,
78  const CostmapWrapper::Ptr& global_costmap_ptr,
79  const CostmapWrapper::Ptr& local_costmap_ptr)
80 {
81  using namespace XmlRpc;
82  XmlRpcValue raw;
83 
84  // Load the data from the param server
85  if (!nh.getParam(resource, raw))
86  throw std::runtime_error("No parameter " + resource);
87 
88  // We expect that resource defines an array
89  if (raw.getType() != XmlRpcValue::TypeArray)
90  throw std::runtime_error(resource + " must be an XmlRpcValue::TypeArray");
91 
92  StringToMap output, mapping;
93 
94  // We support only 'local' or 'global' names for the costmap tag.
95  mapping["global"] = global_costmap_ptr;
96  mapping["local"] = local_costmap_ptr;
97 
98  const int size = raw.size();
99  for (int ii = 0; ii != size; ++ii)
100  {
101  const XmlRpcValue& element = raw[ii];
102 
103  // The element must be of the type struct.
104  if (element.getType() != XmlRpcValue::TypeStruct)
105  continue;
106 
107  // Check if the tags name and costmap are defined and ignore the element if not.
108  if (!element.hasMember("name") || !element.hasMember("costmap"))
109  continue;
110 
111  try
112  {
113  // Try to convert the tags to strings. If the elements cannot be
114  // converted, we will throw.
115  const std::string name = getStringElement(element, "name");
116  const std::string costmap = getStringElement(element, "costmap");
117 
118  // If the costmap tag is not 'local' or 'global', we will throw.
119  output[name] = mapping.at(costmap);
120  }
121  catch (const XmlRpcException& ex)
122  {
123  ROS_ERROR_STREAM("Failed to read the tags 'name' and 'costmap': " << ex.getMessage());
124  }
125  catch (const std::out_of_range& _ex)
126  {
127  ROS_ERROR_STREAM("Unknown costmap name. It must be either 'local' or 'global'");
128  }
129  }
130  return output;
131 }
132 
137 StringToMap loadStringToMaps(const std::string& resource, const ros::NodeHandle& nh,
138  const CostmapWrapper::Ptr& global_costmap_ptr,
139  const CostmapWrapper::Ptr& local_costmap_ptr)
140 {
141  try
142  {
143  return loadStringToMapsImpl(resource, nh, global_costmap_ptr, local_costmap_ptr);
144  }
145  catch (const XmlRpc::XmlRpcException& _ex)
146  {
147  ROS_ERROR_STREAM("Failed to load the mapping: " << _ex.getMessage());
148  }
149  catch (const std::exception& _ex)
150  {
151  ROS_ERROR_STREAM("Failed to load the mapping: " << _ex.what());
152  }
153  return StringToMap();
154 }
155 
157  AbstractNavigationServer(tf_listener_ptr),
158  recovery_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapRecovery"),
159  nav_core_recovery_plugin_loader_("nav_core", "nav_core::RecoveryBehavior"),
160  controller_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapController"),
161  nav_core_controller_plugin_loader_("nav_core", "nav_core::BaseLocalPlanner"),
162  planner_plugin_loader_("mbf_costmap_core", "mbf_costmap_core::CostmapPlanner"),
163  nav_core_planner_plugin_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
164  global_costmap_ptr_(new CostmapWrapper("global_costmap", tf_listener_ptr_)),
165  local_costmap_ptr_(new CostmapWrapper("local_costmap", tf_listener_ptr_)),
166  setup_reconfigure_(false)
167 {
168  // advertise services and current goal topic
177 
178  // dynamic reconfigure server for mbf_costmap_nav configuration; also include abstract server parameters
179  dsrv_costmap_ = boost::make_shared<dynamic_reconfigure::Server<mbf_costmap_nav::MoveBaseFlexConfig> >(private_nh_);
180  dsrv_costmap_->setCallback(boost::bind(&CostmapNavigationServer::reconfigure, this, _1, _2));
181 
182  // Load the optional mapping from planner/controller name to the costmap.
186 
187  // initialize all plugins
189 
190  // start all action servers
192 }
193 
195 {
196  // remove every plugin before its classLoader goes out of scope.
200 
205 }
206 
207 template <typename Key, typename Value>
208 const Value& findWithDefault(const boost::unordered_map<Key, Value>& map, const Key& key, const Value& default_value)
209 {
210  typedef boost::unordered_map<Key, Value> MapType;
211  typename MapType::const_iterator iter = map.find(key);
212  if (iter == map.end())
213  return default_value;
214  return iter->second;
215 }
216 
218  const std::string &plugin_name,
219  const mbf_abstract_core::AbstractPlanner::Ptr &plugin_ptr)
220 {
221  const CostmapWrapper::Ptr& costmap_ptr =
223  return boost::make_shared<mbf_costmap_nav::CostmapPlannerExecution>(
224  plugin_name, boost::static_pointer_cast<mbf_costmap_core::CostmapPlanner>(plugin_ptr), tf_listener_ptr_,
225  costmap_ptr, last_config_);
226 }
227 
229  const std::string &plugin_name,
231 {
232  const CostmapWrapper::Ptr& costmap_ptr =
234  return boost::make_shared<mbf_costmap_nav::CostmapControllerExecution>(
235  plugin_name, boost::static_pointer_cast<mbf_costmap_core::CostmapController>(plugin_ptr), vel_pub_, goal_pub_,
236  tf_listener_ptr_, costmap_ptr, last_config_);
237 }
238 
240  const std::string &plugin_name,
242 {
243  return boost::make_shared<mbf_costmap_nav::CostmapRecoveryExecution>(
244  plugin_name,
245  boost::static_pointer_cast<mbf_costmap_core::CostmapRecovery>(plugin_ptr),
249  last_config_);
250 }
251 
253 {
255  try
256  {
257  planner_ptr = boost::static_pointer_cast<mbf_abstract_core::AbstractPlanner>(
258  planner_plugin_loader_.createInstance(planner_type));
259  std::string planner_name = planner_plugin_loader_.getName(planner_type);
260  ROS_DEBUG_STREAM("mbf_costmap_core-based planner plugin " << planner_name << " loaded.");
261  }
262  catch (const pluginlib::PluginlibException &ex_mbf_core)
263  {
264  ROS_DEBUG_STREAM("Failed to load the " << planner_type << " planner as a mbf_costmap_core-based plugin."
265  << " Try to load as a nav_core-based plugin. " << ex_mbf_core.what());
266  try
267  {
268  // For plugins still based on old nav_core API, we load them and pass to a new MBF API that will act as wrapper
270  planner_ptr = boost::make_shared<mbf_nav_core_wrapper::WrapperGlobalPlanner>(nav_core_planner_ptr);
271  std::string planner_name = nav_core_planner_plugin_loader_.getName(planner_type);
272  ROS_DEBUG_STREAM("nav_core-based planner plugin " << planner_name << " loaded");
273  }
274  catch (const pluginlib::PluginlibException &ex_nav_core)
275  {
276  ROS_FATAL_STREAM("Failed to load the " << planner_type << " planner, are you sure it's properly registered"
277  << " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what());
278  }
279  }
280 
281  return planner_ptr;
282 }
283 
285  const std::string &name,
286  const mbf_abstract_core::AbstractPlanner::Ptr &planner_ptr
287 )
288 {
289  mbf_costmap_core::CostmapPlanner::Ptr costmap_planner_ptr
290  = boost::static_pointer_cast<mbf_costmap_core::CostmapPlanner>(planner_ptr);
291  ROS_DEBUG_STREAM("Initialize planner \"" << name << "\".");
292 
294 
295  if (!costmap_ptr)
296  {
297  ROS_FATAL_STREAM("The costmap pointer has not been initialized!");
298  return false;
299  }
300 
301  costmap_planner_ptr->initialize(name, costmap_ptr.get());
302  return true;
303 }
304 
305 
307 {
309  try
310  {
311  controller_ptr = controller_plugin_loader_.createInstance(controller_type);
312  std::string controller_name = controller_plugin_loader_.getName(controller_type);
313  ROS_DEBUG_STREAM("mbf_costmap_core-based controller plugin " << controller_name << " loaded.");
314  }
315  catch (const pluginlib::PluginlibException &ex_mbf_core)
316  {
317  ROS_DEBUG_STREAM("Failed to load the " << controller_type << " controller as a mbf_costmap_core-based plugin;"
318  << " we will retry to load as a nav_core-based plugin. " << ex_mbf_core.what());
319  try
320  {
321  // For plugins still based on old nav_core API, we load them and pass to a new MBF API that will act as wrapper
322  boost::shared_ptr<nav_core::BaseLocalPlanner> nav_core_controller_ptr
324  controller_ptr = boost::make_shared<mbf_nav_core_wrapper::WrapperLocalPlanner>(nav_core_controller_ptr);
325  std::string controller_name = nav_core_controller_plugin_loader_.getName(controller_type);
326  ROS_DEBUG_STREAM("nav_core-based controller plugin " << controller_name << " loaded.");
327  }
328  catch (const pluginlib::PluginlibException &ex_nav_core)
329  {
330  ROS_FATAL_STREAM("Failed to load the " << controller_type << " controller, are you sure it's properly registered"
331  << " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what());
332  }
333  }
334  return controller_ptr;
335 }
336 
338  const std::string &name,
339  const mbf_abstract_core::AbstractController::Ptr &controller_ptr)
340 {
341  ROS_DEBUG_STREAM("Initialize controller \"" << name << "\".");
342 
343  if (!tf_listener_ptr_)
344  {
345  ROS_FATAL_STREAM("The tf listener pointer has not been initialized!");
346  return false;
347  }
348 
350 
351  if (!costmap_ptr)
352  {
353  ROS_FATAL_STREAM("The costmap pointer has not been initialized!");
354  return false;
355  }
356 
357  mbf_costmap_core::CostmapController::Ptr costmap_controller_ptr
358  = boost::static_pointer_cast<mbf_costmap_core::CostmapController>(controller_ptr);
359  costmap_controller_ptr->initialize(name, tf_listener_ptr_.get(), costmap_ptr.get());
360  ROS_DEBUG_STREAM("Controller plugin \"" << name << "\" initialized.");
361  return true;
362 }
363 
365  const std::string &recovery_type)
366 {
368 
369  try
370  {
371  recovery_ptr = boost::static_pointer_cast<mbf_abstract_core::AbstractRecovery>(
372  recovery_plugin_loader_.createInstance(recovery_type));
373  std::string recovery_name = recovery_plugin_loader_.getName(recovery_type);
374  ROS_DEBUG_STREAM("mbf_costmap_core-based recovery behavior plugin " << recovery_name << " loaded.");
375  }
376  catch (pluginlib::PluginlibException &ex_mbf_core)
377  {
378  ROS_DEBUG_STREAM("Failed to load the " << recovery_type << " recovery behavior as a mbf_costmap_core-based plugin;"
379  << " Retry to load as a nav_core-based plugin. " << ex_mbf_core.what());
380  try
381  {
382  // For plugins still based on old nav_core API, we load them and pass to a new MBF API that will act as wrapper
383  boost::shared_ptr<nav_core::RecoveryBehavior> nav_core_recovery_ptr =
385 
386  recovery_ptr = boost::make_shared<mbf_nav_core_wrapper::WrapperRecoveryBehavior>(nav_core_recovery_ptr);
387  std::string recovery_name = recovery_plugin_loader_.getName(recovery_type);
388  ROS_DEBUG_STREAM("nav_core-based recovery behavior plugin " << recovery_name << " loaded.");
389 
390  }
391  catch (const pluginlib::PluginlibException &ex_nav_core)
392  {
393  ROS_FATAL_STREAM("Failed to load the " << recovery_type << " recovery behavior, are you sure it's properly registered"
394  << " and that the containing library is built? " << ex_mbf_core.what() << " " << ex_nav_core.what());
395  }
396  }
397 
398  return recovery_ptr;
399 }
400 
402  const std::string &name,
403  const mbf_abstract_core::AbstractRecovery::Ptr &behavior_ptr)
404 {
405  ROS_DEBUG_STREAM("Initialize recovery behavior \"" << name << "\".");
406 
407  if (!tf_listener_ptr_)
408  {
409  ROS_FATAL_STREAM("The tf listener pointer has not been initialized!");
410  return false;
411  }
412 
413  if (!local_costmap_ptr_)
414  {
415  ROS_FATAL_STREAM("The local costmap pointer has not been initialized!");
416  return false;
417  }
418 
419  if (!global_costmap_ptr_)
420  {
421  ROS_FATAL_STREAM("The global costmap pointer has not been initialized!");
422  return false;
423  }
424 
426  boost::static_pointer_cast<mbf_costmap_core::CostmapRecovery>(behavior_ptr);
427  behavior->initialize(name, tf_listener_ptr_.get(), global_costmap_ptr_.get(), local_costmap_ptr_.get());
428  ROS_DEBUG_STREAM("Recovery behavior plugin \"" << name << "\" initialized.");
429  return true;
430 }
431 
432 
434 {
435  AbstractNavigationServer::stop();
436  ROS_INFO_STREAM_NAMED("mbf_costmap_nav", "Stopping local and global costmap for shutdown");
437  local_costmap_ptr_->stop();
438  global_costmap_ptr_->stop();
439 }
440 
441 void CostmapNavigationServer::reconfigure(mbf_costmap_nav::MoveBaseFlexConfig &config, uint32_t level)
442 {
443  // Make sure we have the original configuration the first time we're called, so we can restore it if needed
444  if (!setup_reconfigure_)
445  {
446  default_config_ = config;
447  setup_reconfigure_ = true;
448  }
449 
450  if (config.restore_defaults)
451  {
452  config = default_config_;
453  // if someone sets restore defaults on the parameter server, prevent looping
454  config.restore_defaults = false;
455  }
456 
457  // fill the abstract configuration common to all MBF-based navigation
458  mbf_abstract_nav::MoveBaseFlexConfig abstract_config;
459  abstract_config.planner_frequency = config.planner_frequency;
460  abstract_config.planner_patience = config.planner_patience;
461  abstract_config.planner_max_retries = config.planner_max_retries;
462  abstract_config.controller_frequency = config.controller_frequency;
463  abstract_config.controller_patience = config.controller_patience;
464  abstract_config.controller_max_retries = config.controller_max_retries;
465  abstract_config.recovery_enabled = config.recovery_enabled;
466  abstract_config.recovery_patience = config.recovery_patience;
467  abstract_config.oscillation_timeout = config.oscillation_timeout;
468  abstract_config.oscillation_distance = config.oscillation_distance;
469  abstract_config.restore_defaults = config.restore_defaults;
471 
472  // also reconfigure costmaps
473  local_costmap_ptr_->reconfigure(config.shutdown_costmaps, config.shutdown_costmaps_delay);
474  global_costmap_ptr_->reconfigure(config.shutdown_costmaps, config.shutdown_costmaps_delay);
475 
476  last_config_ = config;
477 }
478 
479 bool CostmapNavigationServer::callServiceCheckPointCost(mbf_msgs::CheckPoint::Request &request,
480  mbf_msgs::CheckPoint::Response &response)
481 {
482  // selecting the requested costmap
483  CostmapWrapper::Ptr costmap;
484  std::string costmap_name;
485  switch (request.costmap)
486  {
487  case mbf_msgs::CheckPoint::Request::LOCAL_COSTMAP:
488  costmap = local_costmap_ptr_;
489  costmap_name = "local costmap";
490  break;
491  case mbf_msgs::CheckPoint::Request::GLOBAL_COSTMAP:
492  costmap = global_costmap_ptr_;
493  costmap_name = "global costmap";
494  break;
495  default:
496  ROS_ERROR_STREAM("No valid costmap provided; options are "
497  << mbf_msgs::CheckPoint::Request::LOCAL_COSTMAP << ": local costmap, "
498  << mbf_msgs::CheckPoint::Request::GLOBAL_COSTMAP << ": global costmap");
499  return false;
500  }
501 
502  // get target point as x, y coordinates
503  std::string costmap_frame = costmap->getGlobalFrameID();
504 
505  geometry_msgs::PointStamped point;
506  if (!mbf_utility::transformPoint(*tf_listener_ptr_, costmap_frame, ros::Duration(0.5), request.point, point))
507  {
508  ROS_ERROR_STREAM("Transform target point to " << costmap_name << " frame '" << costmap_frame << "' failed");
509  return false;
510  }
511 
512  double x = point.point.x;
513  double y = point.point.y;
514 
515  // ensure costmap is active so cost reflects latest sensor readings
516  costmap->checkActivate();
517  unsigned int mx, my;
518  if (!costmap->getCostmap()->worldToMap(x, y, mx, my))
519  {
520  // point is outside of the map
521  response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::OUTSIDE);
522  ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is outside the map (cost = " << response.cost << ")");
523  }
524  else
525  {
526  // lock costmap so content doesn't change while checking cell costs
527  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getCostmap()->getMutex()));
528 
529  // get cost of cell under point and classify as one of the states: UNKNOWN, LETHAL, INSCRIBED, FREE
530  response.cost = costmap->getCostmap()->getCost(mx, my);
531  switch (response.cost)
532  {
534  response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::UNKNOWN);
535  ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is in unknown space! (cost = " << response.cost << ")");
536  break;
538  response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::LETHAL);
539  ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is in collision! (cost = " << response.cost << ")");
540  break;
542  response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::INSCRIBED);
543  ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is near an obstacle (cost = " << response.cost << ")");
544  break;
545  default:
546  response.state = static_cast<uint8_t>(mbf_msgs::CheckPoint::Response::FREE);
547  ROS_DEBUG_STREAM("Point [" << x << ", " << y << "] is free (cost = " << response.cost << ")");
548  break;
549  }
550  }
551 
552  costmap->checkDeactivate();
553  return true;
554 }
555 
556 bool CostmapNavigationServer::callServiceCheckPoseCost(mbf_msgs::CheckPose::Request &request,
557  mbf_msgs::CheckPose::Response &response)
558 {
559  // selecting the requested costmap
560  CostmapWrapper::Ptr costmap;
561  std::string costmap_name;
562  switch (request.costmap)
563  {
564  case mbf_msgs::CheckPose::Request::LOCAL_COSTMAP:
565  costmap = local_costmap_ptr_;
566  costmap_name = "local costmap";
567  break;
568  case mbf_msgs::CheckPose::Request::GLOBAL_COSTMAP:
569  costmap = global_costmap_ptr_;
570  costmap_name = "global costmap";
571  break;
572  default:
573  ROS_ERROR_STREAM("No valid costmap provided; options are "
574  << mbf_msgs::CheckPose::Request::LOCAL_COSTMAP << ": local costmap, "
575  << mbf_msgs::CheckPose::Request::GLOBAL_COSTMAP << ": global costmap");
576  return false;
577  }
578 
579  // get target pose or current robot pose as x, y, yaw coordinates
580  std::string costmap_frame = costmap->getGlobalFrameID();
581 
582  geometry_msgs::PoseStamped pose;
583  if (request.current_pose)
584  {
585  if (!mbf_utility::getRobotPose(*tf_listener_ptr_, robot_frame_, costmap_frame, ros::Duration(0.5), pose))
586  {
587  ROS_ERROR_STREAM("Get robot pose on " << costmap_name << " frame '" << costmap_frame << "' failed");
588  return false;
589  }
590  }
591  else
592  {
593  if (!mbf_utility::transformPose(*tf_listener_ptr_, costmap_frame, ros::Duration(0.5), request.pose, pose))
594  {
595  ROS_ERROR_STREAM("Transform target pose to " << costmap_name << " frame '" << costmap_frame << "' failed");
596  return false;
597  }
598  }
599 
600  double x = pose.pose.position.x;
601  double y = pose.pose.position.y;
602  double yaw = tf::getYaw(pose.pose.orientation);
603 
604  // ensure costmap is active so cost reflects latest sensor readings
605  costmap->checkActivate();
606 
607  // pad raw footprint to the requested safety distance; note that we discard footprint_padding parameter effect
608  std::vector<geometry_msgs::Point> footprint = costmap->getUnpaddedRobotFootprint();
609  costmap_2d::padFootprint(footprint, request.safety_dist);
610 
611  // use footprint helper to get all the cells totally or partially within footprint polygon
612  std::vector<Cell> footprint_cells =
613  FootprintHelper::getFootprintCells(x, y, yaw, footprint, *costmap->getCostmap(), true);
614  response.state = mbf_msgs::CheckPose::Response::FREE;
615  if (footprint_cells.empty())
616  {
617  // no cells within footprint polygon must mean that robot is at least partly outside of the map
618  response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::OUTSIDE));
619  }
620  else
621  {
622  // lock costmap so content doesn't change while adding cell costs
623  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getCostmap()->getMutex()));
624 
625  // integrate the cost of all cells; state value precedence is UNKNOWN > LETHAL > INSCRIBED > FREE
626  for (int i = 0; i < footprint_cells.size(); ++i)
627  {
628  unsigned char cost = costmap->getCostmap()->getCost(footprint_cells[i].x, footprint_cells[i].y);
629  switch (cost)
630  {
632  response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::UNKNOWN));
633  response.cost += cost * (request.unknown_cost_mult ? request.unknown_cost_mult : 1.0);
634  break;
636  response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::LETHAL));
637  response.cost += cost * (request.lethal_cost_mult ? request.lethal_cost_mult : 1.0);
638  break;
640  response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::INSCRIBED));
641  response.cost += cost * (request.inscrib_cost_mult ? request.inscrib_cost_mult : 1.0);
642  break;
643  default:
644  response.cost += cost;
645  break;
646  }
647  }
648  }
649 
650  // Provide some details of the outcome
651  switch (response.state)
652  {
653  case mbf_msgs::CheckPose::Response::OUTSIDE:
654  ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is outside the map (cost = " << response.cost
655  << "; safety distance = " << request.safety_dist << ")");
656  break;
657  case mbf_msgs::CheckPose::Response::UNKNOWN:
658  ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is in unknown space! (cost = " << response.cost
659  << "; safety distance = " << request.safety_dist << ")");
660  break;
661  case mbf_msgs::CheckPose::Response::LETHAL:
662  ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is in collision! (cost = " << response.cost
663  << "; safety distance = " << request.safety_dist << ")");
664  break;
665  case mbf_msgs::CheckPose::Response::INSCRIBED:
666  ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is near an obstacle (cost = " << response.cost
667  << "; safety distance = " << request.safety_dist << ")");
668  break;
669  case mbf_msgs::CheckPose::Response::FREE:
670  ROS_DEBUG_STREAM("Pose [" << x << ", " << y << ", " << yaw << "] is free (cost = " << response.cost
671  << "; safety distance = " << request.safety_dist << ")");
672  break;
673  }
674 
675  costmap->checkDeactivate();
676  return true;
677 }
678 
679 bool CostmapNavigationServer::callServiceCheckPathCost(mbf_msgs::CheckPath::Request &request,
680  mbf_msgs::CheckPath::Response &response)
681 {
682  // selecting the requested costmap
683  CostmapWrapper::Ptr costmap;
684  std::string costmap_name;
685  switch (request.costmap)
686  {
687  case mbf_msgs::CheckPath::Request::LOCAL_COSTMAP:
688  costmap = local_costmap_ptr_;
689  costmap_name = "local costmap";
690  break;
691  case mbf_msgs::CheckPath::Request::GLOBAL_COSTMAP:
692  costmap = global_costmap_ptr_;
693  costmap_name = "global costmap";
694  break;
695  default:
696  ROS_ERROR_STREAM("No valid costmap provided; options are "
697  << mbf_msgs::CheckPath::Request::LOCAL_COSTMAP << ": local costmap, "
698  << mbf_msgs::CheckPath::Request::GLOBAL_COSTMAP << ": global costmap");
699  return false;
700  }
701 
702  // ensure costmap is active so cost reflects latest sensor readings
703  costmap->checkActivate();
704 
705  // get target pose or current robot pose as x, y, yaw coordinates
706  std::string costmap_frame = costmap->getGlobalFrameID();
707 
708  std::vector<geometry_msgs::Point> footprint;
709  if (!request.path_cells_only)
710  {
711  // unless we want to check just the cells directly traversed by the path, pad raw footprint
712  // to the requested safety distance; note that we discard footprint_padding parameter effect
713  footprint = costmap->getUnpaddedRobotFootprint();
714  costmap_2d::padFootprint(footprint, request.safety_dist);
715  }
716 
717  // lock costmap so content doesn't change while adding cell costs
718  boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getCostmap()->getMutex()));
719 
720  geometry_msgs::PoseStamped pose;
721 
722  response.state = mbf_msgs::CheckPath::Response::FREE;
723 
724  for (int i = 0; i < request.path.poses.size(); ++i)
725  {
726  response.last_checked = i;
727 
728  if (!mbf_utility::transformPose(*tf_listener_ptr_, costmap_frame, ros::Duration(0.5), request.path.poses[i], pose))
729  {
730  ROS_ERROR_STREAM("Transform target pose to " << costmap_name << " frame '" << costmap_frame << "' failed");
731  return false;
732  }
733 
734  double x = pose.pose.position.x;
735  double y = pose.pose.position.y;
736  double yaw = tf::getYaw(pose.pose.orientation);
737  std::vector<Cell> cells_to_check;
738  if (request.path_cells_only)
739  {
740  Cell cell;
741  if (costmap->getCostmap()->worldToMap(x, y, cell.x, cell.y))
742  {
743  cells_to_check.push_back(cell); // out of map if false; cells_to_check will be empty
744  }
745  }
746  else
747  {
748  // use footprint helper to get all the cells totally or partially within footprint polygon
749  cells_to_check = FootprintHelper::getFootprintCells(x, y, yaw, footprint, *costmap->getCostmap(), true);
750  }
751 
752  if (cells_to_check.empty())
753  {
754  // if path_cells_only is true, this means that current path's pose is outside the map
755  // if not, no cells within footprint polygon means that robot is at least partly outside of the map
756  response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPath::Response::OUTSIDE));
757  }
758  else
759  {
760  // integrate the cost of all cells; state value precedence is UNKNOWN > LETHAL > INSCRIBED > FREE
761  // we apply the requested cost multipliers if different from zero (default value)
762  for (int j = 0; j < cells_to_check.size(); ++j)
763  {
764  unsigned char cost = costmap->getCostmap()->getCost(cells_to_check[j].x, cells_to_check[j].y);
765  switch (cost)
766  {
768  response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPose::Response::UNKNOWN));
769  response.cost += cost * (request.unknown_cost_mult ? request.unknown_cost_mult : 1.0);
770  break;
772  response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPath::Response::LETHAL));
773  response.cost += cost * (request.lethal_cost_mult ? request.lethal_cost_mult : 1.0);
774  break;
776  response.state = std::max(response.state, static_cast<uint8_t>(mbf_msgs::CheckPath::Response::INSCRIBED));
777  response.cost += cost * (request.inscrib_cost_mult ? request.inscrib_cost_mult : 1.0);
778  break;
779  default:response.cost += cost;
780  break;
781  }
782  }
783  }
784 
785  if (request.return_on && response.state >= request.return_on)
786  {
787  // i-th pose state is bad enough for the client, so provide some details of the outcome and abort checking
788  switch (response.state)
789  {
790  case mbf_msgs::CheckPath::Response::OUTSIDE:
791  ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes outside the map "
792  << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
793  break;
794  case mbf_msgs::CheckPath::Response::UNKNOWN:
795  ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes in unknown space! "
796  << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
797  break;
798  case mbf_msgs::CheckPath::Response::LETHAL:
799  ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes in collision! "
800  << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
801  break;
802  case mbf_msgs::CheckPath::Response::INSCRIBED:
803  ROS_DEBUG_STREAM("At pose " << i << " [" << x << ", " << y << ", " << yaw << "] path goes near an obstacle "
804  << "(cost = " << response.cost << "; safety distance = " << request.safety_dist << ")");
805  break;
806  case mbf_msgs::CheckPath::Response::FREE:
807  ROS_DEBUG_STREAM("Path is entirely free (maximum cost = "
808  << response.cost << "; safety distance = " << request.safety_dist << ")");
809  break;
810  }
811 
812  break;
813  }
814 
815  i += request.skip_poses; // skip some poses to speedup processing (disabled by default)
816  }
817 
818  costmap->checkDeactivate();
819  return true;
820 }
821 
822 bool CostmapNavigationServer::callServiceClearCostmaps(std_srvs::Empty::Request &request,
823  std_srvs::Empty::Response &response)
824 {
825  // clear both costmaps
826  local_costmap_ptr_->clear();
827  global_costmap_ptr_->clear();
828  return true;
829 }
830 
831 } /* namespace mbf_costmap_nav */
mbf_costmap_nav::CostmapNavigationServer::nav_core_controller_plugin_loader_
pluginlib::ClassLoader< nav_core::BaseLocalPlanner > nav_core_controller_plugin_loader_
Definition: costmap_navigation_server.h:239
XmlRpc::XmlRpcValue::size
int size() const
response
const std::string response
mbf_costmap_nav::CostmapNavigationServer::loadControllerPlugin
virtual mbf_abstract_core::AbstractController::Ptr loadControllerPlugin(const std::string &controller_type)
Loads the plugin associated with the given controller type parameter.
Definition: costmap_navigation_server.cpp:306
wrapper_global_planner.h
mbf_costmap_nav::CostmapNavigationServer::nav_core_recovery_plugin_loader_
pluginlib::ClassLoader< nav_core::RecoveryBehavior > nav_core_recovery_plugin_loader_
Definition: costmap_navigation_server.h:237
mbf_costmap_nav::CostmapNavigationServer::controller_plugin_loader_
pluginlib::ClassLoader< mbf_costmap_core::CostmapController > controller_plugin_loader_
Definition: costmap_navigation_server.h:238
mbf_costmap_nav::CostmapNavigationServer::newControllerExecution
virtual mbf_abstract_nav::AbstractControllerExecution::Ptr newControllerExecution(const std::string &plugin_name, const mbf_abstract_core::AbstractController::Ptr &plugin_ptr)
Create a new controller execution.
Definition: costmap_navigation_server.cpp:228
mbf_costmap_nav::CostmapNavigationServer::nav_core_planner_plugin_loader_
pluginlib::ClassLoader< nav_core::BaseGlobalPlanner > nav_core_planner_plugin_loader_
Definition: costmap_navigation_server.h:241
costmap_2d::padFootprint
void padFootprint(std::vector< geometry_msgs::Point > &footprint, double padding)
mbf_abstract_nav::AbstractNavigationServer::action_server_exe_path_ptr_
ActionServerExePathPtr action_server_exe_path_ptr_
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
mbf_costmap_nav::loadStringToMapsImpl
StringToMap loadStringToMapsImpl(const std::string &resource, const ros::NodeHandle &nh, const CostmapWrapper::Ptr &global_costmap_ptr, const CostmapWrapper::Ptr &local_costmap_ptr)
Returns the mapping from 'names' to 'costmaps' for the defined resource.
Definition: costmap_navigation_server.cpp:77
boost::shared_ptr
mbf_abstract_nav::AbstractNavigationServer::planner_plugin_manager_
AbstractPluginManager< mbf_abstract_core::AbstractPlanner > planner_plugin_manager_
mbf_costmap_nav::CostmapNavigationServer::loadRecoveryPlugin
virtual mbf_abstract_core::AbstractRecovery::Ptr loadRecoveryPlugin(const std::string &recovery_type)
Loads a Recovery plugin associated with given recovery type parameter.
Definition: costmap_navigation_server.cpp:364
XmlRpc::XmlRpcException::getMessage
const std::string & getMessage() const
mbf_abstract_nav::AbstractNavigationServer::recovery_plugin_manager_
AbstractPluginManager< mbf_abstract_core::AbstractRecovery > recovery_plugin_manager_
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
mbf_abstract_nav::AbstractNavigationServer::initializeServerComponents
virtual void initializeServerComponents()
mbf_abstract_nav::AbstractNavigationServer::action_server_get_path_ptr_
ActionServerGetPathPtr action_server_get_path_ptr_
ROS_FATAL_STREAM
#define ROS_FATAL_STREAM(args)
mbf_costmap_nav::CostmapNavigationServer::stop
virtual void stop()
Definition: costmap_navigation_server.cpp:433
footprint_helper.h
costmap_2d::INSCRIBED_INFLATED_OBSTACLE
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
mbf_costmap_nav::CostmapNavigationServer::check_point_cost_srv_
ros::ServiceServer check_point_cost_srv_
Service Server for the check_point_cost service.
Definition: costmap_navigation_server.h:268
mbf_costmap_nav::Cell::y
unsigned int y
Definition: footprint_helper.h:121
mbf_costmap_nav::CostmapNavigationServer::last_config_
mbf_costmap_nav::MoveBaseFlexConfig last_config_
last configuration save
Definition: costmap_navigation_server.h:247
costmap_2d::NO_INFORMATION
static const unsigned char NO_INFORMATION
mbf_abstract_nav::AbstractNavigationServer::private_nh_
ros::NodeHandle private_nh_
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
mbf_costmap_nav::CostmapNavigationServer::recovery_plugin_loader_
pluginlib::ClassLoader< mbf_costmap_core::CostmapRecovery > recovery_plugin_loader_
Definition: costmap_navigation_server.h:236
mbf_costmap_nav::Cell::x
unsigned int x
Definition: footprint_helper.h:121
XmlRpc
mbf_costmap_nav::CostmapNavigationServer::dsrv_costmap_
DynamicReconfigureServerCostmapNav dsrv_costmap_
Dynamic reconfigure server for the mbf_costmap2d_specific part.
Definition: costmap_navigation_server.h:244
mbf_costmap_nav::getStringElement
static std::string getStringElement(const XmlRpc::XmlRpcValue &value, const std::string &tag)
Returns a string element with the tag from value.
Definition: costmap_navigation_server.cpp:59
mbf_costmap_nav::CostmapNavigationServer::initializePlannerPlugin
virtual bool initializePlannerPlugin(const std::string &name, const mbf_abstract_core::AbstractPlanner::Ptr &planner_ptr)
Initializes the controller plugin with its name and pointer to the costmap.
Definition: costmap_navigation_server.cpp:284
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
mbf_costmap_nav::CostmapNavigationServer::callServiceCheckPoseCost
bool callServiceCheckPoseCost(mbf_msgs::CheckPose::Request &request, mbf_msgs::CheckPose::Response &response)
Callback method for the check_pose_cost service.
Definition: costmap_navigation_server.cpp:556
mbf_costmap_nav::CostmapNavigationServer::planner_name_to_costmap_ptr_
StringToMap planner_name_to_costmap_ptr_
Maps planner names to the costmap ptr.
Definition: costmap_navigation_server.h:262
costmap_navigation_server.h
mbf_abstract_nav::AbstractNavigationServer::vel_pub_
ros::Publisher vel_pub_
mbf_abstract_nav::AbstractPluginManager::clearPlugins
void clearPlugins()
pluginlib::PluginlibException
tf::getYaw
static double getYaw(const geometry_msgs::Quaternion &msg_q)
mbf_abstract_nav::AbstractNavigationServer::action_server_move_base_ptr_
ActionServerMoveBasePtr action_server_move_base_ptr_
mbf_costmap_nav::loadStringToMaps
StringToMap loadStringToMaps(const std::string &resource, const ros::NodeHandle &nh, const CostmapWrapper::Ptr &global_costmap_ptr, const CostmapWrapper::Ptr &local_costmap_ptr)
Non-throwing version of loadStringToMapsImpl.
Definition: costmap_navigation_server.cpp:137
mbf_abstract_nav::AbstractNavigationServer::tf_listener_ptr_
const TFPtr tf_listener_ptr_
mbf_costmap_nav::CostmapNavigationServer::setup_reconfigure_
bool setup_reconfigure_
true, if the dynamic reconfigure has been setup
Definition: costmap_navigation_server.h:253
mbf_costmap_nav::CostmapNavigationServer::default_config_
mbf_costmap_nav::MoveBaseFlexConfig default_config_
the default parameter configuration save
Definition: costmap_navigation_server.h:250
mbf_costmap_nav::CostmapNavigationServer::~CostmapNavigationServer
virtual ~CostmapNavigationServer()
Destructor.
Definition: costmap_navigation_server.cpp:194
simple_action_client.h
mbf_utility::transformPose
bool transformPose(const TF &tf, const std::string &target_frame, const ros::Duration &timeout, const geometry_msgs::PoseStamped &in, geometry_msgs::PoseStamped &out)
mbf_costmap_nav::CostmapNavigationServer::check_path_cost_srv_
ros::ServiceServer check_path_cost_srv_
Service Server for the check_path_cost service.
Definition: costmap_navigation_server.h:274
mbf_abstract_nav::AbstractNavigationServer::robot_frame_
std::string robot_frame_
mbf_costmap_nav::CostmapNavigationServer::local_costmap_ptr_
const CostmapWrapper::Ptr local_costmap_ptr_
Shared pointer to the common local costmap.
Definition: costmap_navigation_server.h:256
mbf_costmap_nav
Definition: costmap_controller_execution.h:51
mbf_abstract_nav::AbstractNavigationServer::reconfigure
virtual void reconfigure(mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
mbf_costmap_nav::CostmapNavigationServer::planner_plugin_loader_
pluginlib::ClassLoader< mbf_costmap_core::CostmapPlanner > planner_plugin_loader_
Definition: costmap_navigation_server.h:240
XmlRpc.h
mbf_abstract_nav::AbstractNavigationServer::startActionServers
virtual void startActionServers()
costmap_2d::LETHAL_OBSTACLE
static const unsigned char LETHAL_OBSTACLE
mbf_costmap_nav::CostmapNavigationServer::initializeRecoveryPlugin
virtual bool initializeRecoveryPlugin(const std::string &name, const mbf_abstract_core::AbstractRecovery::Ptr &behavior_ptr)
Initializes a recovery behavior plugin with its name and pointers to the global and local costmaps.
Definition: costmap_navigation_server.cpp:401
wrapper_local_planner.h
mbf_utility::transformPoint
bool transformPoint(const TF &tf, const std::string &target_frame, const ros::Duration &timeout, const geometry_msgs::PointStamped &in, geometry_msgs::PointStamped &out)
mbf_costmap_nav::CostmapNavigationServer::check_pose_cost_srv_
ros::ServiceServer check_pose_cost_srv_
Service Server for the check_pose_cost service.
Definition: costmap_navigation_server.h:271
XmlRpc::XmlRpcValue::getType
const Type & getType() const
mbf_costmap_nav::CostmapNavigationServer::CostmapNavigationServer
CostmapNavigationServer(const TFPtr &tf_listener_ptr)
Constructor.
Definition: costmap_navigation_server.cpp:156
mbf_costmap_nav::CostmapNavigationServer::newRecoveryExecution
virtual mbf_abstract_nav::AbstractRecoveryExecution::Ptr newRecoveryExecution(const std::string &plugin_name, const mbf_abstract_core::AbstractRecovery::Ptr &plugin_ptr)
Create a new recovery behavior execution.
Definition: costmap_navigation_server.cpp:239
XmlRpc::XmlRpcException
tf.h
XmlRpc::XmlRpcValue::hasMember
bool hasMember(const std::string &name) const
mbf_costmap_nav::CostmapNavigationServer::callServiceCheckPathCost
bool callServiceCheckPathCost(mbf_msgs::CheckPath::Request &request, mbf_msgs::CheckPath::Response &response)
Callback method for the check_path_cost service.
Definition: costmap_navigation_server.cpp:679
mbf_costmap_nav::findWithDefault
const Value & findWithDefault(const boost::unordered_map< Key, Value > &map, const Key &key, const Value &default_value)
Definition: costmap_navigation_server.cpp:208
pluginlib::ClassLoader::createInstance
boost::shared_ptr< T > createInstance(const std::string &lookup_name)
mbf_utility::getRobotPose
bool getRobotPose(const TF &tf, const std::string &robot_frame, const std::string &global_frame, const ros::Duration &timeout, geometry_msgs::PoseStamped &robot_pose)
mbf_costmap_nav::CostmapNavigationServer::newPlannerExecution
virtual mbf_abstract_nav::AbstractPlannerExecution::Ptr newPlannerExecution(const std::string &plugin_name, const mbf_abstract_core::AbstractPlanner::Ptr &plugin_ptr)
Create a new planner execution.
Definition: costmap_navigation_server.cpp:217
pluginlib::ClassLoader::getName
virtual std::string getName(const std::string &lookup_name)
mbf_costmap_nav::Cell
Definition: footprint_helper.h:84
mbf_abstract_nav::AbstractNavigationServer::action_server_recovery_ptr_
ActionServerRecoveryPtr action_server_recovery_ptr_
default_value
def default_value(type_)
mbf_abstract_nav::AbstractNavigationServer::controller_plugin_manager_
AbstractPluginManager< mbf_abstract_core::AbstractController > controller_plugin_manager_
ROS_INFO_STREAM_NAMED
#define ROS_INFO_STREAM_NAMED(name, args)
mbf_costmap_nav::FootprintHelper::getFootprintCells
static std::vector< Cell > getFootprintCells(double x, double y, double theta, const std::vector< geometry_msgs::Point > &footprint_spec, const costmap_2d::Costmap2D &, bool fill)
Used to get the cells that make up the footprint of the robot.
Definition: footprint_helper.cpp:216
mbf_costmap_nav::CostmapNavigationServer::reconfigure
void reconfigure(mbf_costmap_nav::MoveBaseFlexConfig &config, uint32_t level)
Reconfiguration method called by dynamic reconfigure.
Definition: costmap_navigation_server.cpp:441
mbf_costmap_nav::CostmapWrapper
The CostmapWrapper class manages access to a costmap by locking/unlocking its mutex and handles (de)a...
Definition: costmap_wrapper.h:63
mbf_abstract_nav::AbstractNavigationServer::goal_pub_
ros::Publisher goal_pub_
mbf_costmap_nav::CostmapNavigationServer::initializeControllerPlugin
virtual bool initializeControllerPlugin(const std::string &name, const mbf_abstract_core::AbstractController::Ptr &controller_ptr)
Initializes the controller plugin with its name, a pointer to the TransformListener and pointer to th...
Definition: costmap_navigation_server.cpp:337
mbf_costmap_nav::CostmapNavigationServer::loadPlannerPlugin
virtual mbf_abstract_core::AbstractPlanner::Ptr loadPlannerPlugin(const std::string &planner_type)
Loads the plugin associated with the given planner_type parameter.
Definition: costmap_navigation_server.cpp:252
wrapper_recovery_behavior.h
mbf_costmap_nav::CostmapNavigationServer::callServiceCheckPointCost
bool callServiceCheckPointCost(mbf_msgs::CheckPoint::Request &request, mbf_msgs::CheckPoint::Response &response)
Callback method for the check_point_cost service.
Definition: costmap_navigation_server.cpp:479
ros::Duration
mbf_costmap_nav::StringToMap
boost::unordered_map< std::string, CostmapWrapper::Ptr > StringToMap
A mapping from a string to a map-ptr.
Definition: costmap_navigation_server.h:78
mbf_costmap_nav::CostmapNavigationServer::global_costmap_ptr_
const CostmapWrapper::Ptr global_costmap_ptr_
Shared pointer to the common global costmap.
Definition: costmap_navigation_server.h:259
mbf_costmap_nav::CostmapNavigationServer::controller_name_to_costmap_ptr_
StringToMap controller_name_to_costmap_ptr_
Maps the controller names to the costmap ptr.
Definition: costmap_navigation_server.h:265
XmlRpc::XmlRpcValue
mbf_costmap_nav::CostmapNavigationServer::clear_costmaps_srv_
ros::ServiceServer clear_costmaps_srv_
Service Server for the clear_costmap service.
Definition: costmap_navigation_server.h:277
ros::NodeHandle
mbf_costmap_nav::CostmapNavigationServer::callServiceClearCostmaps
bool callServiceClearCostmaps(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
Callback method for the make_plan service.
Definition: costmap_navigation_server.cpp:822


mbf_costmap_nav
Author(s): Sebastian Pütz
autogenerated on Wed Mar 2 2022 00:33:55