Ask Your Question
1

Clearing of obstacle layer in the layered_costmap

asked 2016-01-19 15:30:51 -0500

Naman gravatar image

updated 2016-10-24 09:06:09 -0500

ngrennan gravatar image

Hi all,

I have a layered_costmap with static_layer, obstacle_layer (for Lidar and Kinect) and inflation_layer and there are situations when I need to clear out the obstacle_layers manually in the code. I am working on full coverage and I want to clear out the obstacle layer when the robot reaches the goal and starts going to the next goal so that it can cover more area. What is the best way to clear out the entire obstacle layer and remove all obstacles (which are not in the static_map) manually in the code (maybe in move_base.cpp) ? I don't want to make any changes to the static_layer, just want to clear the obstacle_layer.

Update 1 :
I tried using /move_base/clearCostmapsService for which I added a piece of code in move_base.cpp itself (for now) so that I can test it by giving goals from RVIZ. I added the code in the executeCb function of move_base.cpp which gets called when a goal is received. Here is the code snippet :

void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
      {
        if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){
          as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
          return;
        }


       ///// Added on 01/13/2016
       boost::unique_lock<boost::mutex> lock1(planner_mutex_);
       runPlanner_ = false;

      std_srvs::Empty srv;
      if(clear_costmaps_client.call(srv))
           ROS_INFO("I got the goal and the costmap layers are reset!!!!!");  // It prints this and continues but the costmap is not reset properly, it takes some time after this for the costmap to properly reset

      lock1.unlock();
      /////

        geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);

        //we have a goal so start the planner
        boost::unique_lock<boost::mutex> lock(planner_mutex_);
        planner_goal_ = goal;
        runPlanner_ = true;
        planner_cond_.notify_one();
        lock.unlock();


    current_goal_pub_.publish(goal);
    std::vector<geometry_msgs::PoseStamped> global_plan;

Using the clearCostmapsService, the map is successfully cleared but the problem is that path planning is done before the map can filled with the static_layer (at this time, the map is basically empty and contains no obstacles) so the path is planned through the obstacles in the static_layer which I don't want. I want the costmap should be cleared and then filled again with static_layer and sensor_data and then path planning should be done. Does anyone have any idea what can be the issue in calling clearCostmapService shown in the code snippet above?

Thanks in advance.
Naman

edit retag flag offensive close merge delete

Comments

Did you find a solution?

SH_Developer gravatar imageSH_Developer ( 2017-02-24 10:03:44 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
3

answered 2016-01-20 03:25:37 -0500

mgruhler gravatar image

What about using the move_base/clear_costmaps service of move_base?

You could have a service client in your code and wouldn't need to touch move_base itself.

It clears the full obstacle_layer, which then gets filled again by the static_layer as well as any new sensor data.

edit flag offensive delete link more

Comments

Thanks @mig! I tried what you said and have updated the original question. Please have a look.. TIA!

Naman gravatar imageNaman ( 2016-01-20 08:05:49 -0500 )edit

I think the problem is that you must call the clear_costmaps from another node, for example from the node you send the move_base goal. Like this, move_base will have time to iterate before replanning and so add back static map obstacles into the global costmap.

jorge gravatar imagejorge ( 2016-01-21 11:41:29 -0500 )edit

@jorge, this might actually be the problem. Good idea. I don't know how move_base actually behaves if it is called internally...

mgruhler gravatar imagemgruhler ( 2016-01-22 06:47:17 -0500 )edit

I am also writing a full coverage planner. The move_base clear_costmaps service works well from the terminal. However, the method shown above halts my global plugin code at "clear_costmaps_client.call(srv)" . it doesn't crash, just doesn't continue. Any ideas? i have Ubuntu 16.04 and Kinetic. Thanks

Stephen_Z gravatar imageStephen_Z ( 2017-02-22 21:22:47 -0500 )edit

@Stephen_Z please post a new question with some more details: Ideally, the source code, otherwise some more relevant input, a more descriptive error description, ...

mgruhler gravatar imagemgruhler ( 2017-02-23 01:50:27 -0500 )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-01-19 15:30:51 -0500

Seen: 1,478 times

Last updated: Feb 24 '17