what is the best way to follow a moving target? [closed]

edit

Hello Procópio!

Hope this late answer can be of some help. We have been confronted to the same issue, and we ended up writing our own local planner.

It is a very simple node subscribing to a moving goal topic. It gets the free space description by subscribing to the local costmap topic. It makes a search of the best (v, w) speed couple within the free space.

An illustration is available here . The somewhat documented CPP code is here .

Hope it can be of any use, Arnaud

As Dimitri said, move_base uses three states PLANNING, CONTROLLING, and CLEARING. In the planning state, move_base invokes the global planner to get a valid plan, and in the controlling state, it passes the global plan to local planners to actuate controllers.

In my opinion there is no reason why the local planner can't use the last valid global plan to continue local motion while the global planner makes new global plan which is already happening continuously in another thread (controlled by planner_frequency). The mutual exclusion of state seems to be a self-imposed constraint.

I had the same problem, and my solution is this one-line hack in move_base.cpp:

//the move_base state machine, handles the control logic for navigation
switch(state_){
  //if we are in a planning state, then we'll attempt to make a plan
  case PLANNING:
    {
      boost::mutex::scoped_lock lock(planner_mutex_);
      runPlanner_ = true;
      planner_cond_.notify_one();
    }
    ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state.");
    break;

  //if we're controlling, we'll attempt to find valid velocity commands
  case CONTROLLING:

Remove break;. This let the switch fall through to controlling when it's nominally in the planning state. In the controlling block, it executes according to the last global plan, which is what you need.

I tested this patch with Navfn and Carrot. This is definitely not an elegant solution. It is probably not even correct on the whole. But it works, and might be a good start to properly patch move_base to make the global and local planner truly work in parallel.

Btw, you can reduce the number of goals you send to move_base by thresholding with distance and time from last goal.