在CONTROLLING
状态下,对振荡进行判断:1
2
3
4
5
6
7if(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
9if(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
5else 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
有时会出现进行一次恢复行为后,机器人走了一点,但是又陷入振荡,然后又进行恢复行为