Revision history [back]

Clearing the whole cost map seems like overkill if all you really want to do is clear your own layer. One option is to disable your layer using dynamic reconfigure, like in the Creating a New Layer tutorial:

#include<simple_layers/simple_layer.h>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::SimpleLayer, costmap_2d::Layer)

using costmap_2d::LETHAL_OBSTACLE;

namespace simple_layer_namespace
{

SimpleLayer::SimpleLayer() {}

void SimpleLayer::onInitialize()
{
  ros::NodeHandle nh("~/" + name_);
  current_ = true;

  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
      &SimpleLayer::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);
}


void SimpleLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
  enabled_ = config.enabled;
}

void SimpleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                           double* min_y, double* max_x, double* max_y)
{
  if (!enabled_)
    return;

  mark_x_ = robot_x + cos(robot_yaw);
  mark_y_ = robot_y + sin(robot_yaw);

  *min_x = std::min(*min_x, mark_x_);
  *min_y = std::min(*min_y, mark_y_);
  *max_x = std::max(*max_x, mark_x_);
  *max_y = std::max(*max_y, mark_y_);
}

void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
                                          int max_j)
{
  if (!enabled_)
    return;
  unsigned int mx;
  unsigned int my;
  if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){
    master_grid.setCost(mx, my, LETHAL_OBSTACLE);
  }
}

} // end namespace

Notice that the enabled_ flag is set in reconfigureCB() and referenced in updateBounds() and updateCosts(). When enabled_ is false, the layer does not modify the costmap.

To actually set it to false, you would use a dynamic_reconfigure client in some separate code/custom node:

#include <ros/ros.h>
#include <dynamic_reconfigure/client.h>
#include <simple_layer/SimpleLayerConfig.h>

layer_client_local_ = new dynamic_reconfigure::Client<SimpleLayerConfig>("move_base/local_costmap/simple_layer");
SimpleLayerConfig layer_config;
layer_client_local_->getCurrentConfiguration(layer_config, ros::Duration(1.0));
layer_config.enabled = false;
layer_client_local_->setConfiguration(layer_config);

This code is untested, but I hope it gives you an idea of how to proceed.