Ask Your Question
0

clear_costmap service not working.....

asked 2016-06-13 18:19:58 -0600

Dharmateja Kadem gravatar image

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");
            //////////////////////////////////////////////////////////////////////////////
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2016-06-14 13:17:47 -0600

David Lu gravatar image

By default, the clear costmaps recovery clears the obstacles layer. You can specify which layers get cleared with a parameter: https://github.com/ros-planning/navig...

edit flag offensive delete link more

Comments

Thanks for the reply David.

Here I am using clear_costmaps service not the clear costmaps recovery. I am a beginner in ros, so i don't know the difference between these two. Can I use clear costmaps recovery instead of clear costmaps service.

Dharmateja Kadem gravatar imageDharmateja Kadem ( 2016-06-14 13:34:35 -0600 )edit

clear costmap recovery will only clears the local obstacles (correct me if i am wrong), but I want to clear all the obstacles in the map i.e., global costmap.

Dharmateja Kadem gravatar imageDharmateja Kadem ( 2016-06-14 17:45:32 -0600 )edit

Did you find a solution?

SH_Developer gravatar imageSH_Developer ( 2017-05-19 05:25:10 -0600 )edit
0

answered 2016-06-14 22:14:30 -0600

asimay_y gravatar image

I think you should update your local costmap, not clearing costmap. clearing activity is triggered for recovery behavior, ex. in case of robot cannot move, not for avoiding dynamic obstacle.

edit flag offensive delete link more

Comments

To update local costmap i need sensor information. For my robot there is no sensor on the back. Here I want to update the map after it avoiding and moving away from obstacle.

Dharmateja Kadem gravatar imageDharmateja Kadem ( 2016-06-14 22:48:37 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2016-06-13 18:19:58 -0600

Seen: 981 times

Last updated: Jun 14 '16