振荡行为

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

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