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 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 lock(planner_mutex_);
planner_goal_ = goal;
runPlanner_ = true;
planner_cond_.notify_one();
lock.unlock();
current_goal_pub_.publish(goal);
std::vector 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
↧
