沉默杀手
actionlib的状态机报错 Your executeCallback did not set the goal to a terminal status
|路径规划move_base分析|
Word count: 400|Reading time: 1 min

报错 Your executeCallback did not set the goal to a terminal status. This is a bug in your ActionServer implementation. Fix your code !

此时的move_base/status消息是 This goal was aborted by the simple action server. The user should have set a terminal status on this goal and did not

源码在actionlib的simple_action_server_imp.h的 SimpleActionServer<ActionSpec>::executeLoop()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
// executeLoop函数在不断地轮转状态，以检测新的目标到来
template <class ActionSpec>
void SimpleActionServer<ActionSpec>::executeLoop()
{
    ros::Duration loop_duration = ros::Duration().fromSec(.1);

    while (n_.ok())
    {
      {
        boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
        if (need_to_terminate_)  // 只在 shutdown() 函数中为 true
          break;
      }
      boost::recursive_mutex::scoped_lock lock(lock_);
      if (isActive())
        ROS_ERROR_NAMED("actionlib", "Should never reach this code with an active goal");

      // new_goal_ 标志被置为true后，即 goalcallback 函数被执行完毕后
      else if (isNewGoalAvailable()  )
      {
        GoalConstPtr goal = acceptNewGoal();

        ROS_FATAL_COND(!execute_callback_, "execute_callback_ must exist. This is a bug in SimpleActionServer");

        // Make sure we're not locked when we call execute
        // 重新以新的goal执行executecallback函数
        lock.unlock();
        execute_callback_(goal);
        lock.lock();

        if (isActive())
        {
          ROS_WARN_NAMED("actionlib", "Your executeCallback did not set the goal to a terminal status.\n"
                   "This is a bug in your ActionServer implementation. Fix your code!\n"
                   "For now, the ActionServer will set this goal to aborted");
          setAborted(Result(), "This goal was aborted by the simple action server. The user should have set a terminal status on this goal and did not");
        }
      }
      // 如果没有新目标，通过条件变量将线程阻塞
      else   
        execute_condition_.timed_wait(lock, boost::posix_time::milliseconds(loop_duration.toSec() * 1000.0f));
    }
  }

move_base接收到新目标后，会进入回调函数execute_callback_，执行完之后会判断是否还是Active，如果是那说明服务端没有给最终状态，也就是没有执行setSucceeded或者setAborted函数，说明move_base的流程没有运行到最终状态的逻辑那里。