ROS対応 Car-like ステア機構ロボット CIR-KIT-Unit03 の紹介

# はじめに
メリークリスマスです！[ROS Advent Calendar 2016](http://qiita.com/advent-calendar/2016/ros) 25日目（最終日）ギリギリ間に合いました ^^;

さて，ROSでは`move_base`を基本としたナビゲーションパッケージが整備されており，プラットフォームとしても`Husky`などがあるので気軽に自律移動ロボットの開発に着手できる環境が整っていると思います．

ところが，2016年12月現在，ROSで用意されている自律移動ロボットのプラットフォームは，独立二輪型を想定した情報ばかりが出てくるような気がしませんか？

自動車の自律走行に関する研究が盛んな中，`ROS`にきちんと準拠したCar-like ステア機構対応のプラットフォームが見当たらないのです．多分，皆さんが各自で組んでおられるのだと思います．

でも，それでは`ROS`の意味がなくなってしまうのではないかと思いました．世界中の人たちが使える形に落とし込んで，公開してブラッシュアップしてこそ，オープンソースの真価が発揮されるというものです．

そこで，今回九州工業大学自律移動ロボット作成サークル [CIR-KIT](http://wiki.ros.org/CIR-KIT) が開発したシニアカータウンカート型ロボット`CIR-KIT-Unit03`を題材として，ROSに準拠したステア機構対応のシステムをご紹介します．このロボットは[つくばチャレンジ2016](http://www.tsukubachallenge.jp/tc2016)で2kmの完全自律走行（マイルストーン3）を達成しています．

![cirkit_unit03_banner.jpg](https://qiita-image-store.s3.amazonaws.com/0/110138/71037861-0232-4512-bf7e-6fc3a9f8d799.jpeg)

なお，本記事投稿日現在，既存のパッケージではステア機構対応のプラグイン（`ros_controller`, `RobotHW`, `RobotHWSim`, `RecoveryBehaivior`）が公開されていなかったので，全て自前で作りました．よって，アプリケーションレイヤーを共通にしたまま，実機でもシミュレーションでも動作可能という，いわゆる理想的な`ros_control`の設計思想に準拠したシステムとなっております．

九州のチームということで，現地での実験が殆どできない（今回は本番3日前に現地入り）という不利な状況下でも完走できたのは，学生メンバーの絶え間ない努力はもちろんのこと，`GAZEBO`で効率的にソフトウェア開発ができたことも無視できない影響を与えていると思います．実際，九州からロボットを配送している期間中も `GAZEBO`で既知の問題のデバッグ作業を進めることができ，現地調整の負担を軽減できたという実績があります．

- 実機，つくばチャレンジでのGOALシーン（Facebook 動画）

[![TSUKUBA GOAL](https://qiita-image-store.s3.amazonaws.com/0/110138/230f2684-7c02-33b2-0ec5-3152d76465de.png)](https://www.facebook.com/takeshi.nishida.73/videos/1119692694775498/)

- `Gazebo`デモシーン（Youtube 動画）

[![GAZEBO DEMO](https://i.ytimg.com/vi/6OA_49ULO2A/hqdefault.jpg)](https://youtu.be/6OA_49ULO2A)

さぁ，前置きはこのくらいにして，中身の話をしてみましょう．

# 目次
- ハードウェア構成
  - ロボット本体
  - センサ
- ソフトウェア構成
- 各機能の説明

# コード & ドキュメント
説明はいいからソースを見せろ，という方のために，先にリンクを載せておきます．どうぞご確認下さい．

- **ROS Wiki** 
 * ロボット: [Robots/CIR-KIT-Unit03](http://wiki.ros.org/Robots/CIR-KIT-Unit03)
 * サークル: [CIR-KIT](http://wiki.ros.org/CIR-KIT)
- **GitHub** 
 * ロボット（Organization）：[CIR-KIT-Unit03](https://github.com/CIR-KIT-Unit03/)
 * ロボット（アプリケーションパッケージ）：[CIR-KIT-Unit03/cirkit_unit03_apps](https://github.com/CIR-KIT-Unit03/cirkit_unit03_apps)
 * サークル（ロボットに依存しない汎用機能）: [CIR-KIT](https://github.com/CIR-KIT)

# ハードウェア構成
![thirdrobot_2016ver.png](https://qiita-image-store.s3.amazonaws.com/0/110138/adf38e94-0f55-308b-5cdd-db544e36cafd.png)

## ロボット本体
- シニアカータウンカート ([スズキ社製 TC1A4](http://www.suzuki.co.jp/welfare/tc1a/detail/index.html)) 
  - ステア機構のカートを改造しています．

## 計算機
- Lenovo Laptop
  * ROS用のPCです．
- [iXis Research 社製マイコンボード iMCs01](http://www.ixs.co.jp/products/)
  * 前輪のステア角，後輪のエンコーダ情報を取得します．
  * 後輪駆動を制御します．
- [Arduino Uno](https://www.arduino.cc/en/Main/ArduinoBoardUno)
  * 前輪ステア角制御用のステッピングモータを制御します． 

## センサ
- LRF ([北陽電機製 UTM-30LX)](http://www.hokuyo-aut.co.jp/search/single.php?serial=21))
  - 前方と後方に1つずつ，計2つの装備です．地図作成，障害物検出に利用します．
- LIDAR（[北陽電機製 YVT-X002](http://www.hokuyo-aut.co.jp/search/single.php?serial=150)）
  - 前方に1つの装備です．人物検出に利用します．
- ロータリエンコーダ
  - 前方ステア：[MUTOH 製 UN-125](http://www.mutoheng.com/~deji/rotaryencoder/)：ステアリングの角度を取得します．
  - 後方車輪：セニアカーに装備されていたものを使用します．

# ソフトウェア構成
## 概要
まず，対応バージョンは`indigo`です．（それ以外のverで動かしたいけど動かない，ということがあれば`issue`を挙げて頂ければできる限り対応する所存です．）

![ros_control.png](https://qiita-image-store.s3.amazonaws.com/0/110138/3736bf83-7f4d-2b69-b2d9-ac2bb8457694.png)

さて，冒頭でも述べたように，`ros_control`と`RobotHW`および`RobotHWSim`に対応しているので，アプリケーションレイヤーは（パラメータ以外は）完全に共通となります．

## とりあえずインストールして遊んでみる

下記のコマンドの通りに実行すれば，とりあえず`Gazebo`で遊ぶことくらいはできます．

### 1. Create workspacd
`<catkin_ws>`は任意のワークスペース名です．

```bash
$ mkdir -p <catkin_ws>/src
$ cd <catkin_ws>/src
$ catkin_init_workspace
```

### 2. Clone this repository.
```bash
$ cd <catkin_ws>/src
$ git clone https://github.com/CIR-KIT-Unit03/cirkit_unit03_apps.git
```

### 3. Run install script
```bash
$ cd <catkin_ws>/src/cirkit_unit03_apps
$ sh install.sh
```
- optional arguments:
  * -h: show the help message and exit
  * -i: eneble `rosdep init`
  * -t: enable `catkin_make run_tests`
  * -r: remove existing `src/.rosinstall`

### 4. launch `Gazebo` appliation
```bash
$ roslaunch cirkit_unit03_autorun autorun_gazebo.launch
```

- 初期位置がずれているので，`Rviz`上で`2D Pose Estimate`で現在位置を調整して下さい．
- その後，`2D Nav Goal`でゴールを指定して下さい．`CIR-KIT-Unit03`が動き始めます．

## インストール後の構成
さて，ソフトウェア構成についてもう少し踏み込むと，下図のような構成となります．

![図1.png](https://qiita-image-store.s3.amazonaws.com/0/110138/e93f62c1-f8da-a963-3df7-554c192196d0.png)

依存パッケージと記載しているパッケージは`rosdep`で取得できないため`wstool`を利用してcloneしているものを指しています．

以降，各パッケージの構成について更に踏み込んでいきます．

### cirkit_unit03_pkgs
#### 概要
- `CIR-KIT-Unit03`の動作に最低限必要なパッケージをインストールします．

#### 構成
- ベース側は下記の構成を取ります．
  * [cirkit_unit03_common](https://github.com/CIR-KIT-Unit03/cirkit_unit03_common/tree/indigo-devel) : 実機，シミュレータ共通
      - [cirkit_unit03_contorol](https://github.com/CIR-KIT-Unit03/cirkit_unit03_common/tree/indigo-devel/cirkit_unit03_control/tree/indigo-devel) : コントローラ，センサフィルタ周りの設定
      - [cirkit_unit03_description](https://github.com/CIR-KIT-Unit03/cirkit_unit03_common/tree/indigo-devel/cirkit_unit03_description) : `URDF`
  * [cirkit_unit03_robot](https://github.com/CIR-KIT-Unit03/cirkit_unit03_robot/tree/indigo-devel) : 実機関連
      - [cirkit_unit03_bringup](https://github.com/CIR-KIT-Unit03/cirkit_unit03_robot/tree/indigo-devel/cirkit_unit03_bringup) : ハードウェアセットアップ
      - [cirkit_unit03_base](https://github.com/CIR-KIT-Unit03/cirkit_unit03_robot/tree/indigo-devel/cirkit_unit03_base) : `RobotHW`
  * [cirkit_unit03_simulator](https://github.com/CIR-KIT-Unit03/cirkit_unit03_simulator/tree/indigo-devel) : シミュレータ関連
      - [cirkit_unit03_gazebo](https://github.com/CIR-KIT-Unit03/cirkit_unit03_simulator/tree/indigo-devel/cirkit_unit03_gazebo) : `Gazebo(RobotHWSim)`設定
  * [cirkit_unit03_navigation](https://github.com/CIR-KIT-Unit03/cirkit_unit03_navigation/tree/indigo-devel) : 自律移動関連(`CIR-KIT-Unit03`に特化した設定を格納)
      - [cirkit_unit03_amcl](https://github.com/CIR-KIT-Unit03/cirkit_unit03_navigation/tree/indigo-devel/cirkit_unit03_amcl) : 自己位置推定
      - [cirkit_unit03_gmapping](https://github.com/CIR-KIT-Unit03/cirkit_unit03_navigation/tree/indigo-devel/cirkit_unit03_gmapping) : 地図作成
      - [cirkit_unit03_move_base](https://github.com/CIR-KIT-Unit03/cirkit_unit03_navigation/tree/indigo-devel/cirkit_unit03_move_base) : ナビゲーション
      - [cirkit_unit03_maps](https://github.com/CIR-KIT-Unit03/cirkit_unit03_navigation/tree/indigo-devel/cirkit_unit03_maps) : 地図格納場所

- 依存側は下記の構成を取り，`cirkit_unit03_deps`ディレクトリに入ります．
  * [ira_laser_tools](https://github.com/AriYu/ira_laser_tools/tree/fix-eigen3cmake) : 複数のLRF情報をマージ
  * [steer_drive_ros](https://github.com/CIR-KIT/steer_drive_ros/tree/indigo-devel) : ステア機構用プラグイン
      - [steer_drive_controller](https://github.com/CIR-KIT/steer_drive_ros/tree/indigo-devel/steer_drive_controller) : ステア機構対応 `ros_controller`
      - [stepback_and_steerturn_recovery](https://github.com/CIR-KIT/steer_drive_ros/tree/indigo-devel/stepback_and_steerturn_recovery) : ステアロボット用リカバリプラグイン
      - [steer_bot_hardware_gazebo](https://github.com/CIR-KIT/steer_drive_ros/tree/indigo-devel/steer_bot_hardware_gazebo) : `Gazebo`用4輪ステアロボットモデル（`RobotHWSim`）
  * [lower_step_detector](https://github.com/CIR-KIT/lower_step_detector/tree/indigo-devel) : 下方段差検出（今年は不使用）
  
### cirkit_unit03_apps
#### 概要
* `CIR-KIT-Unit03`基本パッケージ＋アプリケーションパッケージを丸ごとインストールします．
* `launch`叩けばシステムが一発で立ち上がる，的なパッケージを入れるのがココです．
* `rosdep`で取得できない依存パッケージは`wstool`を利用してcloneしてきます．

#### 構成
- ベース側は下記の構成を取ります．
  * [cirkit_unit03_apps](https://github.com/CIR-KIT-Unit03/cirkit_unit03_apps/) : メタパッケージ
      - [cirkit_unit03_autorun](https://github.com/CIR-KIT-Unit03/cirkit_unit03_apps/tree/master/cirkit_unit03_autorun) :  自律走行に必要なノードが全部立ち上がる．
- 依存側は下記の構成を取ります．
  * [cirkit_waypoint_manager](https://github.com/CIR-KIT/cirkit_waypoint_manager/tree/indigo-devel) : `waypoint`管理
      - [cirkit_waypoint_generator](https://github.com/CIR-KIT/cirkit_waypoint_manager/tree/indigo-devel/cirkit_waypoint_generator) : 経路上に`waypoint`を作成
      - [cirkit_waypoint_navigator](https://github.com/CIR-KIT/cirkit_waypoint_manager/tree/indigo-devel/cirkit_waypoint_navigator) : 作成した`waypoint`を順番に切り替え
  * [human_detector](https://github.com/CIR-KIT/human_detector/tree/indigo-devel) : 人物検出関連
      - [target_object_detector](https://github.com/CIR-KIT/human_detector/tree/indigo-devel/target_obejct_detector) : 人物検出コアプログラム
      - [fake_target_detector](https://github.com/CIR-KIT/human_detector/tree/indigo-devel/fake_target_detector) : 仮想みなし検出器（ナビゲーションデバッグ用）
      - [human_model_gazebo](https://github.com/CIR-KIT/human_detector/tree/indigo-devel/human_model_gazebo) : `Gazebo` 用人物モデル．
      - [point_cloud_reducer](https://github.com/CIR-KIT/human_detector/tree/indigo-devel/point_cloud_reducer) : `Gazebo` のKinectプラグインの点群数の削減（計算量低減のため）
   * [timed_roslaunch](https://github.com/MoriKen254/timed_roslaunch/tree/indigo-devel) : `launch`ファイルを遅延起動

インストール後の具体的なフォルダ構成は下記のようになります．

```
<catkin_ws>
│
└ src
   │ 
   ├ cirkit_unit03_apps 
   │   │
   │   ├ cirkit_unit03_apps (Metapackage)
   │   │
   │   ├ cirkit_unit03_autorun
   │   │
   │   ├ human_detector (Cloned via cirkit_unit03_apps.rosinstall)
   │   │
   │   └ timed_roslaunch (Cloned via cirkit_unit03_apps.rosinstall)
   │
   ├ cirkit_unit03_deps (Just a directory, not a metapacage)
   │   |
   |   ├ ira_laser_tools (Cloned via cirkit_unit03_pkgs.rosinstall)
   │   |
   |   ├ lower_step_detector (Cloned via cirkit_unit03_pkgs.rosinstall)
   │   |
   │   └ steer_drive_ros (Cloned via cirkit_unit03_pkgs.rosinstall)
   │
   └ cirkit_unit03_pkgs
       |
       ├ cirkit_unit03_pkgs (Metapackage)
       |
       ├ cirkit_unit03_common (Cloned via cirkit_unit03_pkgs.rosinstall)
       |
       ├ cirkit_unit03_navigation (Cloned via cirkit_unit03_pkgs.rosinstall)
       |
       ├ cirkit_unit03_robot (Cloned via cirkit_unit03_pkgs.rosinstall)
       |
       └ cirkit_unit03_simulator (Cloned via cirkit_unit03_pkgs.rosinstall)
```

このように，様々なパッケージをその機能・役割に応じてメタパッケージに格納する構成となっています．

# ハード＋ソフトの詳細システム構成
以上で説明したハードとソフトを統合したシステム構成を，かの有名な[`ros_control`x`GAZEBO`のソフトウェア構成図](http://gazebosim.org/tutorials/?tut=ros_control)に合わせて図示したものが下図となります．青く**NEW**と書いてあるモジュールが，自前で開発したものとなります．

![図0.png](https://qiita-image-store.s3.amazonaws.com/0/110138/50c846aa-75f5-c1ad-e8bd-bccb76ec3cba.png)

※`Rviz`への矢印は本当はもっとたくさんあるのですが，もはやスペースの問題で描けなくなりました笑．ご了承下さい．

こうして全体を見返すと，今回の取組においては，上位のアプリケーションレイヤーはもちろんのこと，下位のコントローラレイヤーまで幅広く自前のプラグインを作成していることが分かると思います．ここまでしないとステア機構の移動ロボットに対応させることができなかった，というのが現実だったので，じゃあ作ちゃえばいいじゃん♪というノリでココに至ったわけです ^^;

という経緯があったので，特にコントローラレイヤーの開発成果については，できるだけ抽象化した設計をして，より多くの人に使って頂けるような工夫をしてみました．

そろそろ構成の話はお腹いっぱいですよね笑．というわけで，その抽象化あたりの話も含め，これ以降は各機能の説明に移っていこうと思います♪ ^^

# 各機能の説明
ここでは，主要なものについてもう少し説明を加えてゆきます．詳しい仕様については，それぞれ`ROS Wiki`のドキュメントをご参照下さい．

- コントローラ関連
  * `steer_drive_controller` 
  * `steer_bot_hardware_gazebo`
- ナビゲーション関連
  * `waypoint_generator`
  * `waypoint_navigator`
  * `stepback_and_steerturn_recovery`
- 人物検出関連
  * `target_object_detector`
  * `human_model_gazebo`
- 遠隔監視
  * `remote_monitor`

## コントローラ関連
### `steer_drive_controller`

- GitHub: [CIR-KIT/steer_drive_ros/steer_drive_controller - indigo-devel](https://github.com/CIR-KIT/steer_drive_ros)
- ROS Wiki: [steer_drive_controller](http://wiki.ros.org/steer_drive_controller)

ステア機構対応の`ros_controller`です．`Twist`型の速度指令を受け取り，後輪用の`velocity_joint_interface`と前輪ステア用の`position_joint_interface`に分解します．

#### 指令値の仕様
分解する`joint_interface`はそれぞれ1軸ずつです．理由は，ロボットの車輪の構成は多様であるため，その1軸ずつの情報をどのように分解してロボットに指令を与えるかは，`RobotHW`あるいは`RobotHWSim`で吸収したほうが汎用的になると考えたためです．

##### 例1: 後輪が1軸（2輪でディファレンシャルギア），前輪ステアが1軸（単に1輪，あるいは2輪でアッカーマンリンク）

この構成であれば，指令値を分解せずにそのまま使えます．一般的には，この構成を取る4輪ロボットが多いのではないでしょうか．

![example1.png](https://qiita-image-store.s3.amazonaws.com/0/110138/88041f4c-83d6-63f7-8f24-68d24f78161c.png)

##### 例2: 後輪が2軸（2輪が独立），前輪ステアは1軸（単に1輪，あるいは2輪でアッカーマンリンク）

もしこのような構成をとったロボットがあったとすれば，下図のように後輪の速度指令値を分解することになります，

![example2.png](https://qiita-image-store.s3.amazonaws.com/0/110138/b78d13d4-ed28-a63e-68bf-07d3e87b69ac.png)

##### 例3: 後輪が2軸（2輪が独立），前輪ステアが2軸（2輪が独立）

この場合には，下図のように前輪の角度指令値を分解する構成を取ります．

![example3.png](https://qiita-image-store.s3.amazonaws.com/0/110138/017caea8-a16d-a112-04e1-511e9cdfc6b5.png)

こんなまどろっこしい構成を取る必要があるのかと思われるかもしれませんが，実は必要な場合があるのです．例えば，`URDF`は閉リンクに対応していないので，`Gazebo`用の`RobotHWSim`ではこの構成を取って，前輪はパラレルリンク機構あるいはアッカーマンリンク機構を，後輪ではディファレンシャルドライブ機構をソフトウェア的に再現する必要があるのです．

#### 設定
`yaml`ファイルで前後のjoint等を設定すれば良いです．`CIR-KIT-Unit03`では[cirkit_unit03_common/cirkit_unit03_control](https://github.com/CIR-KIT-Unit03/cirkit_unit03_common/tree/indigo-devel/cirkit_unit03_control)に設定ファイルを置いています．

例えば，このような設定になります．各パラメータの仕様は，[ROS Wiki: steer_drive_controller](http://wiki.ros.org/steer_drive_controller)をご参照下さい．

```yaml:cirkit_unit03_steer_drive_controller_gazebo.yaml
steer_bot_hardware_gazebo:
# cirkit_unit03:
# Publish all joint states -----------------------------------
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50

# Wheel & Steer
steer_drive_controller:
  type                          : "steer_drive_controller/SteerDriveController"
  rear_wheel                    : 'rear_wheel_joint'
  front_steer                   : 'front_steer_joint'
  #publish_rate                  : 100

  pose_covariance_diagonal      : [0.00001, 0.00001, 1000000000000.0, 1000000000000.0, 1000000000000.0, 0.001]
  twist_covariance_diagonal     : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  wheel_separation_h_multiplier : 1.7 # calibration parameter for angle odometory
  #wheel_radius_multiplier       : 1.0 # calibration parameter for linear odometory
  steer_pos_multiplier          : 1.5
  cmd_vel_timeout               : 4
  base_frame_id                 : base_link

  enable_odom_tf: true
  #wheel_separation_h            : 0.79 # retriieved from urdf joints
  #wheel_radius                  : 0.28 # retrieved from urdf joint

  #linear:
  #  x:
  #    has_velocity_limits       : true
  #    max_velocity              : 0.9  # m/s
  #    min_velocity              : -0.9 # m/s
  #    has_acceleration_limits   : true
  #    max_acceleration          : 1.7  # m/s^2
  #    min_acceleration          : -0.4 # m/s^2
  #    has_jerk_limits           : true
  #    max_jerk                  : 5.0 # m/s^3
  #angular:
  #  z:
  #    has_velocity_limits       : true
  #    max_velocity              : 0.5  # rad/s
  #    has_acceleration_limits   : true
  #    max_acceleration          : 1.5  # rad/s^2
  #    has_jerk_limits           : true
  #    max_jerk                  : 2.5 # rad/s^3
``` 

#### `RobotHW`と`RobotHWSim`の実装例
実装例は，下記リポジトリを参照下さい．

- `RobotHW` の実装例（前述の例1: 後輪が1軸，前輪ステアが1軸）
  - [CIR-KIT-Unit03/cirkit_unit03_robot/cirkit_unit03_base: indigo-devel](https://github.com/CIR-KIT-Unit03/cirkit_unit03_robot/tree/indigo-devel/cirkit_unit03_base)
  - ただし，後輪のエンコーダは各車輪についているので，コントローラ側に送る現在値は両者の平均を取るようにしています．

- `RobotHWSim` の実装例（後輪が2軸，前輪ステアが2軸）
  - [CIR-KIT/steer_drive_ros/steer_bot_hardware_gazebo: indigo-devel](https://github.com/CIR-KIT-Unit03/cirkit_unit03_robot/tree/indigo-devel/cirkit_unit03_base)
  - `RobotHWSim`は汎用性を高めるために，[cirkit-unit03-simulator/cirkit-unit03-gazebo](https://github.com/CIR-KIT-Unit03/cirkit_unit03_simulator/tree/indigo-devel/cirkit_unit03_gazebo) で直接実装はしていません．
  - `URDF`で必要となる`joint`を登録し，[cirkit-unit03-simulator/cirkit-unit03-gazebo/urdfdescription.gazebo.xacro](https://github.com/CIR-KIT-Unit03/cirkit_unit03_simulator/blob/indigo-devel/cirkit_unit03_gazebo/urdf/description.gazebo.xacro)でこのプラグインを読み込んで，自身のロボットに対応するジョイント名だけをyamlファイルで設定すれば使えるようにしてあります．
  - ただし，4輪ステア型ロボット用のモデルになりますので，ご了承下さい．
  - `yaml`ファイルの設定方法の例としては，[CIR-KIT-Unit03/cirkit-unit03-simulator/cirkit-unit03-gazebo](https://github.com/CIR-KIT-Unit03/cirkit_unit03_simulator/tree/indigo-devel/cirkit_unit03_gazebo)をご参照下さい．

例えば，このような設定になります．

```xml:description.gazebo.xacro
<?xml version="1.0"?>
<robot name="cirkit_unit03_gazebo" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- 省略 -->
  <!-- Gazebo plugin for ROS Control -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/</robotNamespace>
      <robotSimType>steer_bot_hardware_gazebo/SteerBotHardwareGazebo</robotSimType>
    </plugin>
  </gazebo>
<!-- 省略 -->
</robot>
```

```yaml:cirkit_unit03_hardware_gazebo.yaml
steer_bot_hardware_gazebo:
    rear_wheel  : 'rear_wheel_joint'
    front_steer : 'front_steer_joint'
    virtual_rear_wheels  : ['base_to_right_rear_wheel', 'base_to_left_rear_wheel']
    virtual_front_wheels : ['base_to_right_front_wheel', 'base_to_left_front_wheel']
    virtual_front_steers : ['base_to_right_front_steer', 'base_to_left_front_steer']

    # ackermann link mechanism
    enable_ackermann_link: true
    wheel_separation_w : 0.5
    wheel_separation_h : 0.79
```  

また，`Gazebo`に速度指令値を入力する部分でPID制御をしているので，ゲインを設定しています．

```yaml:pids.yaml
gains:
　# 後輪速度制御用PIDゲイン
  base_to_right_rear_wheel  :  {p: 100000.0, d: 10.0, i: 0.50, i_clamp: 3.0}
  base_to_left_rear_wheel   :  {p: 100000.0, d: 10.0, i: 0.50, i_clamp: 3.0}
```

必要な`joint`を`URDF`に登録するというあたりの話は，設計思想についてはROS Wiki，実装についてはGitHubを参照下さい．

- ROS Wiki: [steer_bot_hardware_gazebo](http://wiki.ros.org/steer_bot_hardware_gazebo)
- GitHub: [CIR-KIT/steer_drive_ros/steer_bot_hardware_gazebo: indigo-devel](https://github.com/CIR-KIT/steer_drive_ros/tree/indigo-devel/steer_bot_hardware_gazebo)
 
#### `multi_interface_controller` の継承
一つのコントローラで複数種類（`velocity`, `position`）の`joint_interface`を使用するので，`multi_interface_controller`を継承する必要があります．

この`multi_interface_controller`，実は`jade`以降の対応なので，公式には`indigo`では存在しません．とは言えこちとらずっと`indigo`でやってきた経緯があり，今更`ROS`のバージョンを変えると~~怖い~~アプリケーションが新しい`ROS`のバージョンについてきていない場合があり，使用上のリスクが高いと判斷したわけです．私たちに限らず，まだ`indigo`を使いたいって人も多いのが現実ではないでしょうか？

#### `indigo` 対応
という訳で，`multi_interface_controller`を私が**非公式に改変し**，`indigo`でも使えるようにした（とにかく`catkin_make`が通るようにしただけのお粗末な）ものを[CIR-KIT/steer_drive_ros: indigo-devel](https://github.com/CIR-KIT/steer_drive_ros/tree/indigo-devel)に入れています．お行儀が悪いのですが，ひとまず問題なく使えている，という状況です ^^;

という背景もあり，近く私は本家にこのコントローラのプルリクを出す予定ですが，`indigo`ではサポートされないと思われます．よって，`indigo`で使用したい場合は下記リポジトリをローカルのワークスペースにcloneして`catkin_make`して頂ければ，ご利用頂くことができます．

- GitHub: [CIR-KIT/steer_drive_ros: indigo-devel](https://github.com/CIR-KIT/steer_drive_ros/tree/indigo-devel)

## ナビゲーション関連
ナビゲーションのベースとしては[move_base](http://wiki.ros.org/move_base)を利用していますが，`move_base`は単一のゴールを設定する仕様となっているので，長距離移動の経路を細かく設定する仕組みは自前で搭載する必要があります．私たちは，経路上に多数の`waypoint`を設定して，ロボットの現在位置と現在の`waypoint`間の距離が閾値以下に達した時に，次の`waypoint`を`move_base`の`actionlib`サーバに送るような仕様を採用しています．

![図5.png](https://qiita-image-store.s3.amazonaws.com/0/110138/cdac8b75-a983-6c32-a245-1ebb4c4cda09.png)

そのような機能を，先に紹介した下記リポジトリに格納しています．

- GitHub リポジトリ: [cirkit_waypoint_manager](https://github.com/CIR-KIT/cirkit_waypoint_manager/tree/indigo-devel)

なお，現在つくばチャレンジに特化している部分が大きいのでその辺は改善する予定です．

内部は下記のような構成となります．

### `cirkit_waypoint_generator` パッケージ

- GitHub: [cirkit_waypoint_generator](https://github.com/CIR-KIT/cirkit_waypoint_manager/tree/indigo-devel/cirkit_waypoint_generator)
- ROS Wiki: [cirkit_waypoint_generator](http://wiki.ros.org/cirkit_waypoint_generator)

前述したナビゲーションをしようにも，まずは`waypoint`を作成しないことには話が始まりません．本パッケージは，経路上への`waypoint`作成関連のノードが下記のように格納されています．

* `cirkit_waypoint_generator`
  - `rviz`からクリックで`waypoint`を追加できます．自己位置推定ができていればロボットの走行中でも，
  - あるいは，ロボットが実際に走行している経路上において，一定間隔で`waypoint`を自動で設定することができます．ゲームコントローラで操縦しながら`waypoint`群を自動で生成するような使い方が可能です．
  - さらに，一度置いた`waypoint`を微調整をしたい場合でも，`rviz`上でのドラッグ操作で対応可能です．
* `cirkit_waypoint_saver`
  - `cirkit_waypoint_generator`でクリックした位置をCSVファイルに保存します．
* `cirkit_waypoint_server`
  -  `cirkit_waypoint_saver`で保存したCSVファイルを読み込み，`waypoint`を地図上に表示します．保存した`waypoint`が正しく生成されているかを確認するために使います．

下図は，クリックして作成した`waypointの`例です．

![図8.png](https://qiita-image-store.s3.amazonaws.com/0/110138/31421d1a-cc3f-ab9d-8e74-4188650ca81d.png)

次の図は，ロボットの走行経路上に一定間隔で`waypoint`を自動的に作成した例です．黄色い矢印の部分が，人物探索フラグが立ち上がっている`waypoint`を指しています．

![waypoint_navigator.png](https://qiita-image-store.s3.amazonaws.com/0/110138/c9cbb353-d7fa-3de4-8e3f-1cf6d4650086.png)

### `cirkit_waypoint_navigator` パッケージ

- GitHub: [cirkit_waypoint_navigator](https://github.com/CIR-KIT/cirkit_waypoint_manager/tree/indigo-devel/cirkit_waypoint_navigator)
- ROS Wiki: [cirkit_waypoint_navigator](http://wiki.ros.org/cirkit_waypoint_navigator)

`waypoint`の自動切り替え機能が実装されています．

* `cirkit_waypoint_navigator`
  - `move_base`に`actionlib`を介してwaypointで指定された箇所にゴールを設定します．
  - ある程度ゴールの近くに到達すると新たにゴールを設定し，次の`waypoint`を目指します．これを次々と実行していくことで自律走行を実現します．
  - `move_base`の`abort`状態検知および`waypoint`再設定機能も搭載しています．
     * `move_base`はスタックするとリカバリ動作を行いますが，全てのリカバリ動作を行っても進むことが出来ない場合，`abort`してしまいます．
     * しかし，`move_base`には自身の`abort`状態を通知する仕組みが存在しないため，`waypoint_navigator`側で一定時間自己位置が変化していないことを検出したときに`waypoint`を新たに設定する仕様としています．
  - つくばチャレンジでは人物探索およびアプローチの課題があるため，この機能も搭載しています．
     * `waypoint_saver`で保存したCSVファイルには，`waypoint`が探索エリアかどうかを設定するフラグが設定できるようにしてあります．
     * `waypoint_navigator`では，このフラグ読み取ることで，次の`waypoint`が探索エリアであることを判定します．
     * 次の`waypoint`が探索エリアである場合には，後述する人物検出器である`target_object_detector`から探索結果を受け取り，人物検出位置に`waypoint`を設定して，人物へのアプローチを試みます．
     * ただし，すでにアプローチ済みの位置周辺である場合には，検出候補位置に`waypoint`を設定しません．重複検出による冗長なアプローチを回避するためです．

![図7.png](https://qiita-image-store.s3.amazonaws.com/0/110138/245422b4-22a2-ba86-2e91-a57835e43157.png)

### `stepback_and_steerturn_recovery` パッケージ

- GitHub: [stepback_and_steerturn_recovery](https://github.com/CIR-KIT/steer_drive_ros/tree/indigo-devel/stepback_and_steerturn_recovery)
- ROS Wiki: [stepback_and_steerturn_recovery](http://wiki.ros.org/stepback_and_steerturn_recovery)

`move_base`を利用してスタックしたときのリカバリ行動としては，[rotate_recovery](http://wiki.ros.org/rotate_recovery)がよく使用されていると思います．しかし，ステア機構のロボットではこのリカバリプラグインを使えません．だって，**その場回転ができません**から ^^;

という訳で，ステア機構ロボットでも使えるリカバリプラグイン，`stepback_and_steerturn`を開発しました．[steer_drive_ros: indigo-devel](https://github.com/CIR-KIT/steer_drive_ros/tree/indigo-devel)に`steer_drive_controller`と一緒に入っています．

#### 仕様
以下が基本行動です．

1. Stuckする
2. 後述する旋回行動の進路を確保するため，後退する．
2. 前方のコストマップを確認し，最も近い位置にある障害物の位置を探索する．
3. それが左半分にあれば右前方に進む．設定次第で，その後前進＋左前方に進む行動も追加できる．
4. 右半分にあれば，3.と同様で向きが反対の行動を取る．

![図9.png](https://qiita-image-store.s3.amazonaws.com/0/110138/0dd10512-1b2c-ec42-9669-97c36e372545.png)

下記はデモ動画です．まだ検証段階で撮影した動画なので，独立二輪の[Husky](http://wiki.ros.org/Robots/Husky)を使わせてもらっていますが，指令値的にはステア型のロボットでも使用可能です．

[![RECOVERY DEMO](https://qiita-image-store.s3.amazonaws.com/0/110138/0fc712ae-1ade-d958-c620-0f67396e42af.jpeg)](https://youtu.be/j6GID9XuiiU)

同動画の12秒あたりで想定外の障害物を検知してStuckしています．その後前述した回避行動を取ることで，新たなローカルパスを取得してナビゲーションが再開している様子が確認できると思います．

`CIR-KIT-Unit03`でも，このプラグインを導入してからはStuckして復帰できないという状況が発生する機会が格段に減り，確実な走行の実現に大きく寄与しました．

#### 設定
`move_base`のプラグインですので，[cirkit_unit03_navigation/cirkit_unit03_move_base](https://github.com/CIR-KIT-Unit03/cirkit_unit03_navigation/tree/indigo-devel/cirkit_unit03_move_base)に設定ファイルを置いています．

抜粋したパラメータを下記に示します．

```yaml:config/gazebo/base_local_planner_params.yaml
# 省略

recovery_behaviors:
  - {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}
  - {name: stepback_and_steerturn_recovery, type: stepback_and_steerturn_recovery/StepBackAndSteerTurnRecovery}
  - {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}

stepback_and_steerturn_recovery:
    # 最初の一回だけ旋回したい場合にtrue
    only_single_steering: true
    # リカバリ行動の試行回数[回]
    trial_times         : 3
    # 障害物までの許容距離[m]．
    #-- 移動中に，移動方向に対して最も近い障害物がこの距離以内に出現したら停止する．
    obstacle_patience   : 0.4
    #-- 移動中に，障害物を確認する頻度[回/sec]
    obstacle_check_frequency: 5.0
    # 障害物探索時の角度の分解能[rad] costmapアクセス数を低減したいときに調整する．
    sim_angle_resolution: 0.1
    # back(初回後退時の速度[m/s], 移動距離[m], タイムアウト[sec])
    linear_vel_back     : -1.5
    step_back_length    : 1.5
    step_back_timeout   : 10.0
    # steer(旋回時の直進速度[m/s], 回転速さ(環境に寄って±が変わる)[rad/s], 目標回転角度[rad], タイムアウト[sec])
    linear_vel_steer    : 0.3
    angular_speed_steer : 1.0
    turn_angle          : 0.78
    steering_timeout    : 10.0
    # forward(旋回→直進→旋回の直進時の速度[m/s], 目標移動距離[m])
    linear_vel_forward  : 0.3
    step_forward_length : 1.0
    step_forward_timeout: 15.0

# 省略
```

## 人物検出関連
## `target_object_detector` パッケージ

- GitHub リポジトリ: [human_detector/target_obejct_detector](https://github.com/CIR-KIT/human_detector/tree/indigo-devel/target_obejct_detector)
- ROS Wiki: [target_object_detector](http://wiki.ros.org/target_object_detector)

### 概要
今回，人物探索には`3DLIDAR`から得られた点群をSVMで識別する方法を用いました．実装には[`LibSVM`](https://www.google.co.jp/search?q=LibSVM&oq=LibSVM&aqs=chrome..69i57j0l5.1046j0j4&sourceid=chrome&ie=UTF-8)を用いました．`SVM`を用いた理由は，今回のタスクにおいては探索対象かそうでないかの二値分類をすれば十分であったこと，C++で容易に実装ができるという点が挙げられます．

学習の際には，つくばチャレンジで想定される人物の形状を再現するべく，実際につくばチャレンジで使用されるものと同一の看板とジャンパーを購入して学内で実験を行いました．我々のチームは拠点が会場であるつくばから遠方に位置する九州であり，なかなか実験走行に参加することができないので，このような方法を取っています．

![図10.jpg](https://qiita-image-store.s3.amazonaws.com/0/110138/b0a58087-abb7-36ae-5a31-b368bed766b1.jpeg)

### 前処理
地面点群の除去をしたのちに，ユークリディアンクラスタリングを行います．単純にユークリッド距離が近い点群をクラスタとみなします．以降の特徴量の計算は，クラスタリング後の点群を元に行います．

### 特徴量
識別に利用する点群の特徴量は，下記論文のものを参考にしました．

- Liu, Kun, and Jan Boehm. "Classification of Big Point Cloud Data Using Cloud Computing." The International Archives of Photogrammetry, Remote Sensing and Spatial Information Sciences 40.3 (2015): 553.

特徴量$f_{i}$は6次元からなる$f_{i1}$と7次元からなる$f_{i2}$の2つの特徴ベクトルを並べた13次元の特徴量です．
まず，$f_{i1}$は点群の3次元共分散行列の要素であり次式で示されます．

```math
\Sigma = \frac{1}{n-1}\sum_{\boldsymbol{x}_{k}(i)\in C_{i}}\boldsymbol{x}_{k}^{(i)}\boldsymbol{x}_{k}^{(i)\mathrm{T}} ... (1)\\

\mathrm{where　} \boldsymbol{x}_k^{(i)} \triangleq {[x_{k}^{(i)} y_{k}^{(i)} z_{k}^{(i)}]}. ... (2)
```

ここで，$\boldsymbol{x}_{k}^{(i)}$ はクラスタ内に含まれる点群のクラスタ重心を原点とする座標です．

この行列は対称行列であるため，行列要素のうち重複を除いた6要素を特徴量として，

```math
\boldsymbol{f}_{i1} = [f_{i11}, \cdots, f_{i16}]^{T} ... (3)
```

とします．

式(1)に示す3次元共分散行列の固有値を$\lambda_{1}, \lambda_{2}, \lambda_{3}$とします．ただし，$\lambda_{1} < \lambda_{2} < \lambda_{3}$です．この固有値を元に下表に示す特徴量を計算しこれを$\boldsymbol{f}_{i2}$としました．

![図10.png](https://qiita-image-store.s3.amazonaws.com/0/110138/da4ae82c-4ea9-c8a5-5b37-c53478f408ca.png)

要は，データの分布（特に散らばりの方向性に重点をおいて）をいろいろな指標で比較している，ということです．ただ，点群の密度がセンサからの距離に応じて変化するので，絶対値で評価すると学習時とピッタリ同じ距離にある人物し認識しないというかなり厳しい条件で識別することになるでしょう．

実際，作成されたモデルを利用する場合には特徴量の正規化をちゃんと行わないと精度が出ませんでした．`LibSVM`で訓練を行ったときにスケーリングパラメータファイルも出力されるのでこれを読み込み，正規化を行いました．

### より具体的な学習手順
ロボットを移動させながら`3DLIDAR`からの点群をbagファイルに記録します．`bag`ファイルを再生しながら点群をクラスタリングし一つづつ`pcd`ファイルとして保存します．クラスタリングなどは`PCL`の関数を利用しました．
保存された`pcd`ファイルを一つずつ探索対象かそうでないかの2つに分類し，特徴量を計算しCSVファイルに保存します．全ての分類が終わったら`LibSVM`を用いてモデルを作成します．この辺りはいろいろな記事があるためそちらを参照下さい．

### 大学内での実験

大学で事前検証を行った時の様子を以下の動画で示します．

[![DETECTION DEMO](https://qiita-image-store.s3.amazonaws.com/0/110138/5dd0b068-78b2-fd4a-de39-841bc44b122d.png)](https://youtu.be/_AiFR_FApjs)

最後まで残っているボックスが見つかった探索対象です．一見上手く行っているように見えますが，残念ながらつくばチャレンジの環境では誤検出が多くなってしまいました．学習に使ったデータが300個ほどしか無かったため，十分に訓練が出来ていなかったものと推察されます．


### シミュレーション空間での実験

- GitHub リポジトリ: [human_detector/human_model_gazebo](https://github.com/CIR-KIT/human_detector/tree/indigo-devel/human_model_gazebo)
- ROS Wiki: [human_model_gazebo](http://wiki.ros.org/human_model_gazebo)

ナビゲーション＋人物探索の挙動を机上で検証できるように，`Gazebo`用の人物モデルを作成しました．

`Gazebo`上に実際に表示された様子を，下図に示します．
![human_detection_gazebo.png](https://qiita-image-store.s3.amazonaws.com/0/110138/35f6fbc7-2f00-93d3-693d-e3dccf5bc0fb.png)

`Rviz`上で人物位置にCandidateとしてボックスが表示されていることが確認できます．

![human_detection.png](https://qiita-image-store.s3.amazonaws.com/0/110138/ece74bd8-967a-a18a-a264-e11b3a52a42e.png)

これにより，現地で走らせることなくナビゲーションのデバッグ作業を進めることが出来ました．今回については本番で人物検出の機能をOFFにしましたが，ナビゲーション動作自体の完成度を上げることができました．

なお，この人物モデルは`URDF`で作成しました．中身はごちゃごちゃですが笑．

## remote_monitor
- GitHubリポジトリ: [remote_monitor](https://github.com/CIR-KIT/remote_monitor/tree/indigo-devel)
- ROS Wiki: [remote_monitor](http://wiki.ros.org/remote_monitor)

つくばチャレンジでは，必須ではないですが遠隔モニタリングが強く推奨されています．そこで私たちは，自前でOpenVPNでROSネットワークを繋いで，九州からつくばのロボットの位置をリアルタイムに監視することに成功しました．

![図6.png](https://qiita-image-store.s3.amazonaws.com/0/110138/2ecf1024-7ec9-67c3-0588-dc8e5e647b57.png)

下図は，本番走行におけるロボット位置の履歴を表示した地図です．少し感覚が短かったようですが，ぐるっと回って完走している様子を確認できます．

プロトタイプの段階のものを，ローカルPCで動作確認したものが次の動画です．遠隔モニタリングも同一の挙動を示します．

[![REMOTE MONITORING DEMO](https://qiita-image-store.s3.amazonaws.com/0/110138/4bb88bb4-c625-e24e-6fb4-6cf18ab04a57.jpeg)](https://youtu.be/hpAnaV-bylQ)

この後改良を加え，人物検出＋アプローチ時に，地図上の検出位置に棒人間を表示させるようにしました．
![monitor.png](https://qiita-image-store.s3.amazonaws.com/0/110138/4b29c231-22d8-3eac-16f6-4b6834b7b94e.png)

コレに関連する記事は，下記にございますので，ご興味のある方はご参照下さい．

* [ROS で OpenVPN を使う](http://qiita.com/MoriKen/items/00c2006e0c7ae914a9ba)
* 図解！UbuntuでOpenVPNを使う：目次
  1. [準備](http://qiita.com/MoriKen/items/7fe17fda8442adb599d9)
  2. [証明書と認証鍵の作成](http://qiita.com/MoriKen/items/50d226e8c6395487c0a6)
  3. [設定](http://qiita.com/MoriKen/items/75587fd862b3ce93e4eb)
  4. [接続確認](http://qiita.com/MoriKen/items/682d31496fd0d2476af7)

これで，今回実装した主要な機能はカバーしました^^

# おわりに
今回，つくばチャレンジへの取組として`CIR-KIT`サークルが取り組んだロボットの紹介を行いました．

Car-like ステア機構に`ros_control`レベルで対応したプラットフォームは初ではないでしょうか．近年盛んに行われている自動車の自律走行技術の研究開発に，少しでも貢献できれば幸いでございます．

まだドキュメントの整備が完全に終わっていなかったり，色々やり残していることがありまだまだ至らぬ点が多いのが現状ではありますが，今回の投稿を期に更にブラッシュアップしてゆく所存ですので，どうぞご利用頂けたり，議論ができたらと思います．

今後ともよろしくお願いいたします．

# 謝辞

本ロボットの公開にあたり，`CIR-KIT`のメンバーにご協力をいただきました．特に，

- [AriYu](https://github.com/AriYu) さん
- [RyodoTanaka](https://github.com/RyodoTanaka) さん
- [forno](https://github.com/forno) さん

には多大なるご協力をいただきました．この場をお借りして心より感謝申し上げます．