沉默杀手
振荡行为
|路径规划move_base分析|
Word count: 570|Reading time: 2 min

在CONTROLLING状态下，对振荡进行判断:

1
2
3
4
5
6
7
if(oscillation_timeout_ > 0.0 &&  ros::Time::now() - last_oscillation_reset_ > ros::Duration(oscillation_timeout_)  )
{
  publishZeroVelocity();
  state_ = CLEARING;
  recovery_trigger_ = OSCILLATION_R;
  movebase_log.error("oscillate in a small area for a long time !");
}

oscillation_timeout参数默认是0，我设置为10。这个时间是从到达振荡点和判定确实是振荡经过的时间，但整个恢复行为花的时间会有几十秒

判断last_oscillation_reset_的赋值是在

1
2
3
4
5
6
7
8
9
if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
{
  last_oscillation_reset_ = ros::Time::now();
  oscillation_pose_ = current_position;

  //if our last recovery was caused by oscillation, we want to reset the recovery index 
  if(recovery_trigger_ == OSCILLATION_R)
    recovery_index_ = 0;
}

最后彻底失败，会abort:

1
2
3
4
5
else if(recovery_trigger_ == OSCILLATION_R)
{
	movebase_log.error("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
	as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
}

出现振荡的日志如下:

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
[10:21:19.968 - INFO] (): robot is oscillating, and also recently_oscillated

# move_base  CONTROLLING
[10:21:20.262 - ERROR] (): move_base CONTROLLING: oscillate in a small area for a long time !
# 进入CLEARING状态，OSCILLATION_R，准备进入第1个恢复行为
[10:21:20.262 - INFO] (): robot is oscillating, and also recently_oscillated

# 执行第1个恢复行为，结束后，状态变为PLANNING
[10:21:20.301 - INFO] (): move_base_recovery,state machine is CLEARING, Executing behavior 0 of 2
[10:21:20.301 - WARN] (): Clearing costmap to unstuck robot (3.000000m).

[10:21:20.628 - INFO] (): robot is oscillating, and also recently_oscillated
[10:21:33.657 - INFO] (): robot is oscillating, and also recently_oscillated
[10:21:33.724 - INFO] (): robot is oscillating, and also recently_oscillated

# 第一个恢复行为失败，仍然进入CLEARING状态, OSCILLATION_R
[10:21:33.846 - ERROR] (): move_base CONTROLLING: oscillate in a small area for a long time !

[10:21:33.848 - INFO] (): robot is oscillating, and also recently_oscillated

# 执行第2个恢复行为
[10:21:33.900 - INFO] (): move_base_recovery,state machine is CLEARING, Executing behavior 1 of 2
[10:21:33.900 - WARN] (): Clearing costmap to unstuck robot (0.000000m).


[10:21:37.559 - INFO] (): robot is oscillating, and also recently_oscillated
[10:21:37.617 - INFO] (): robot is oscillating, and also recently_oscillated
[10:21:37.805 - INFO] (): robot is oscillating, and also recently_oscillated
[10:21:37.863 - INFO] (): robot is oscillating, and also recently_oscillated

						# 省略一段时间

[10:21:48.917 - ERROR] (): move_base CONTROLLING: oscillate in a small area for a long time !
# 两个恢复行为都失败，状态又变为CLEARING    OSCILLATION_R
[10:21:48.919 - INFO] (): robot is oscillating, and also recently_oscillated

# 彻底失败，Abort
[10:21:49.103 - ERROR] (): Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors

有时会出现进行一次恢复行为后，机器人走了一点，但是又陷入振荡，然后又进行恢复行为