clear_costmap service not working.....
I want to clear the dynamic obstacles in the costmap manually for this I am trying to clear all the layers of the costmap except static layer by editing the clear_costmaps service (because if I use clear_costmaps service it will clear the whole costmap and the robot may hit the obstacles.) But that's not working and It is not clearing the obstacles in the costmap. It is calling the service and the program is running fine but the obstacles in the costmap are not cleared. I am using ros indigo , ubuntu 14.04 and I made the following changes in move_base.cpp and costmap_2d_ros.cpp programs. Can someone help how to fix this issue?
In move_base.cpp:
//advertise a service for clearing the costmaps
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
//ros::NodeHandle cnh;
//advertise a service for clearing the costmaps except static layer
clear_costmaps_new_srv_ = private_nh.advertiseService("clear_costmaps_new", &MoveBase::clearCostmapsService_new, this);
----------
bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){
//clear the costmaps
planner_costmap_ros_->resetLayers();
controller_costmap_ros_->resetLayers();
return true;
}
/////new function to clear the costmap layers except static layer/////
bool MoveBase::clearCostmapsService_new(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){
//clear the costmaps
planner_costmap_ros_->resetLayers_new();
controller_costmap_ros_->resetLayers_new();
return true;
}
In costmap_2d_ros.cpp:
void Costmap2DROS::resetLayers()
{
Costmap2D* top = layered_costmap_->getCostmap();
top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());
std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
++plugin)
{
(*plugin)->reset();
}
}
//////new function to reset layers except static layer/////
void Costmap2DROS::resetLayers_new()
{
std::string SL = "global_costmap/static_layer";
Costmap2D* top_n = layered_costmap_->getCostmap();
top_n->resetMap(0, 0, top_n->getSizeInCellsX(), top_n->getSizeInCellsY());
std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
++plugin)
{
if((*plugin)->getName() != SL)
ROS_INFO("resetting layer %s",(*plugin)->getName().c_str());
(*plugin)->reset();
}
}
In my program the service is called as shown below:
///////////////////////////////////////////////////////////////////////////////
std_srvs::Empty ccn;
ros::service::waitForService("clear_costmaps_new",ros::Duration(2.0));
if (clear_costmaps_client.call(ccn))
{
ROS_INFO("every costmap layers are cleared except static layer");
}
else ROS_INFO("failed calling clear_costmaps service");
//////////////////////////////////////////////////////////////////////////////