はじめに
以前，Navigation Stack を理解する - 2.2 move_base: ソフトウェア構成をみる/積み状態からの回復でRecovery動作に入ることを触れました．
実際に使ってみると，なかなかこのRecovery動作(上図でいうところの，Clearing Rotation に相当)に入ってくれないことがあります．さっさと諦めたり，諦めずにねばってしまうときもあります．ダメならさっさとRecovery行動をとってほしいのに，じれったくなることがあります^^;
という経緯があり，どんな条件でRecoveryに入るのだろうか？その条件は変更できるのだろうか？という疑問が仲間内で出てきました．
そんなわけで気になってソースコードを読んでみたら，いくつか条件が浮かび上がりました．
ROS Wikiにも載っていない情報が抽出できたので，備忘録がてら残しておくことにしました．move_base利用者の方にとって，少しでもご参考になれば幸いです．
環境
- OS: Ubuntu 14.04
- ROS: Indigo
- モデル：ros-planning/navigation
結論
条件は，パラメータサーバで設定できます．
デフォルトで有効となっている条件は，下記の通りです．
-
controller_patience秒，停止状態が継続した場合
デフォルトで無効になっている条件は，下記の通りです.
-
oscillation_timeout秒，振動行動が継続した場合 -
max_planning_retries回，ローカルプランニング試行しても経路が得られなかった場合- この設定はwikiには載っていない…
これらの値をプランナーのyamlファイルで設定してあげればよいです(Navigation Stack のチュートリアルで言うところの，base_local_planner_params.yaml に相当)．
パラメータの仕様（max_planning_retries以外）はmove_baseのWikiページでパラメータ名を検索すると引っかかります．
ソースコード解析
前提: Recovery 状態に遷移する条件
- メンバ変数
state_がCLEARINGになると，Recovery 状態に遷移します． - 以降，この状態遷移が起こる分岐を見ていきます．
ros-planning/navigation/blob/indigo-devel/move_base/src/move_base.cpp_l.986
//we'll try to clear out space with any user-provided recovery behaviors
case CLEARING:
ROS_DEBUG_NAMED("move_base","In clearing/recovery state");
//we'll invoke whatever recovery behavior we're currently on if they're enabled
if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){
ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_, recovery_behaviors_.size());
recovery_behaviors_[recovery_index_]->runBehavior();
-
runBehavior()がリカバリ行動メソッドです．
1. controller_patience秒，停止状態が継続した場合
- デフォルトは15秒です．
ros-planning/navigation/blob/indigo-devel/move_base/src/move_base.cpp_l.74
private_nh.param("controller_patience", controller_patience_, 15.0);
- 状態遷移条件の設定部
ros-planning/navigation/blob/indigo-devel/move_base/src/move_base.cpp_l.958
ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan.");
ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_);
//check if we've tried to find a valid control for longer than our time limit
if(ros::Time::now() > attempt_end){
//we'll move into our obstacle clearing mode
publishZeroVelocity();
state_ = CLEARING;
recovery_trigger_ = CONTROLLING_R;
}
2. oscillation_timeout秒，振動行動が継続した場合
- デフォルト0です．これだと振動してても一生Recoveryに移りません．
ros-planning/navigation/blob/indigo-devel/move_base/src/move_base.cpp_l.77
private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
- 状態遷移条件の設定部
ros-planning/navigation/blob/indigo-devel/move_base/src/move_base.cpp_l.936
//check for an oscillation condition
if(oscillation_timeout_ > 0.0 &&
last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
{
publishZeroVelocity();
state_ = CLEARING;
recovery_trigger_ = OSCILLATION_R;
}
3. max_planning_retries回，ローカルプランニング試行しても経路が得られなかった場合
- デフォルトだと-1です．これだとRecovery状態への遷移はプランニング回数に依存しないことになります．
- ROS Wikiには記載されていないよう…
ros-planning/navigation/blob/indigo-devel/move_base/src/move_base.cpp_l.75
private_nh.param("max_planning_retries", max_planning_retries_, -1); // disabled by default
- 状態遷移条件の設定部
ros-planning/navigation/blob/indigo-devel/move_base/src/move_base.cpp_l.655
ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
//check if we've tried to make a plan for over our time limit or our maximum number of retries
//issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_
//is negative (the default), it is just ignored and we have the same behavior as ever
lock.lock();
if(runPlanner_ &&
(ros::Time::now() > attempt_end || ++planning_retries_ > uint32_t(max_planning_retries_))){
//we'll move into our obstacle clearing mode
state_ = CLEARING;
publishZeroVelocity();
recovery_trigger_ = PLANNING_R;
}
おわりに
こんな感じです．上記パラメータで遷移条件がゆるくなるように調整したところ，確かにRecovery行動に入りやすくなることを確認しました．