clearing costmap using move_base/clearCostmap service

edit

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 costmap manually in the code. I am using move_base/clearCostmapService service to clear the costmap. Whenever I get a new goal, I call this service to clear the costmap and then the planner generates a new plan. AFAIK, when I call move_base/clearCostmap service, it clears the costmap and then fills it with static_map immediately meaning that obstacles which are not in the map are cleared. The problem I am having right now is that sometimes, planner generates a path through obstacles in the static_map (maybe because, costmap is cleared but has not been filled with static_map yet). I am calling the service when I receive a new goal and the planner plans a path after the service is called. Can someone suggest what can be the problem and how to fix it permanently? Code snippet from move_base.cpp ::

    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 12/23/2015
       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;

Update :
It looks like the planner thread plans the path before the clearing (or resetting) Costmap thread is finished. Correct me if I am wrong. How can I make sure to run the planner thread only when the clearing Costmap thread is finished?

Any suggestions will be appreciated Let me know if you need more information from me.

Thanks in advance.
Naman Kumar