move_base でRecovery 行動に遷移する条件を調べてみた

# はじめに
以前，[Navigation Stack を理解する - 2.2 move_base: ソフトウェア構成をみる/積み状態からの回復](http://qiita.com/MoriKen/items/d5cd6208143d6c40caff#%E7%A9%8D%E3%81%BF%E7%8A%B6%E6%85%8B%E3%81%8B%E3%82%89%E3%81%AE%E5%9B%9E%E5%BE%A9)でRecovery動作に入ることを触れました．

![14_rotate_recovery.png](https://qiita-image-store.s3.amazonaws.com/0/110138/6dbccf9d-c93a-fd9c-d33d-1ede027e9d6e.png)

実際に使ってみると，なかなかこのRecovery動作(上図でいうところの，Clearing Rotation に相当)に入ってくれないことがあります．さっさと諦めたり，諦めずにねばってしまうときもあります．ダメならさっさとRecovery行動をとってほしいのに，じれったくなることがあります^^;

という経緯があり，どんな条件でRecoveryに入るのだろうか？その条件は変更できるのだろうか？という疑問が仲間内で出てきました．

そんなわけで気になってソースコードを読んでみたら，いくつか条件が浮かび上がりました．

[ROS Wiki](http://wiki.ros.org/move_base)にも載っていない情報が抽出できたので，備忘録がてら残しておくことにしました．`move_base`利用者の方にとって，少しでもご参考になれば幸いです．

# 環境
- OS: Ubuntu 14.04
- ROS: Indigo
- モデル：[ros-planning/navigation](https://github.com/ros-planning/navigation/tree/indigo-devel)

# 結論
条件は，パラメータサーバで設定できます．

デフォルトで有効となっている条件は，下記の通りです．

- `controller_patience`秒，停止状態が継続した場合

デフォルトで無効になっている条件は，下記の通りです.


- `oscillation_timeout`秒，振動行動が継続した場合
- `max_planning_retries`回，ローカルプランニング試行しても経路が得られなかった場合
  - この設定はwikiには載っていない…

これらの値をプランナーのyamlファイルで設定してあげればよいです([Navigation Stack のチュートリアル](http://wiki.ros.org/navigation/Tutorials/RobotSetup)で言うところの，base_local_planner_params.yaml に相当)．

パラメータの仕様（`max_planning_retries`以外）は[move_baseのWikiページ](http://wiki.ros.org/move_base)でパラメータ名を検索すると引っかかります．

# ソースコード解析
## 前提: Recovery 状態に遷移する条件
- メンバ変数`state_`が`CLEARING`になると，Recovery 状態に遷移します．
- 以降，この状態遷移が起こる分岐を見ていきます．

```cpp: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秒です．

```cpp:ros-planning/navigation/blob/indigo-devel/move_base/src/move_base.cpp_l.74
    private_nh.param("controller_patience", controller_patience_, 15.0);
```

- 状態遷移条件の設定部

```cpp: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に移りません．

```cpp:ros-planning/navigation/blob/indigo-devel/move_base/src/move_base.cpp_l.77
    private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
```

- 状態遷移条件の設定部

```cpp: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には記載されていないよう…


```cpp: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
```

- 状態遷移条件の設定部

```cpp: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行動に入りやすくなることを確認しました．

# 参考文献
- [ROS Wiki - move_base](http://wiki.ros.org/move_base)
- [ROS Wiki - Setup and Configuration of the Navigation Stack on a Robot](http://wiki.ros.org/navigation/Tutorials/RobotSetup)
- [Github - ros-planning/navigation/move_base](https://github.com/ros-planning/navigation/blob/indigo-devel/move_base)