ROS講座60 Navgation用のシミュレーション環境

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 16.04 |
| ROS | Kinetic |
| Gazebo | 7.14.0 |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
ここからはROSの自己位置システムとnavigation packageを使った移動についての解説をしていく予定です。それに使うためのシミュレーション環境を解説していきます。

# ロボット仕様
今回も差動2輪型のロボットになります。センサーとしてはIMUと2次元lidarがついています。
![nav_normal1.png](https://qiita-image-store.s3.amazonaws.com/0/254442/730a163a-5dcb-8c45-12ad-878f5998b6c9.png)

## キャスターには不安定性がある
差動2輪機構を例として今後実行します。後ろの従輪は実際はキャスターを使うかと思いますが、シミュレーション編と同様にここには摩擦の低い半球を使っています。キャスターには「向き」があり、この向きによって同じcmd_velでも挙動が変わってしまいます。無用のトラブルを避けるためにキャスターは避けましょう。
![nav_normal2.png](https://qiita-image-store.s3.amazonaws.com/0/254442/69ea0d3b-d9a7-aab5-8fd0-311e68f16dc8.png)

## アクチュエーター

* 差動2輪機構(`/dtw_robot1/diff_drive_controller/cmd_vel`)  
twistで指示すると回るタイヤの機構です。  

## センサー

* ホイールオドメトリ(`/dtw_robot1/diff_drive_controller/odom`)  
タイヤの回転から計算される位置と速度です。
* 慣性センサー(`/dtw_robot1/imu/data`)  
加速度、角速度センサーと9軸統合をしたIMUの値です。
* 2次元Lidar(`/laser/scan`)  
lidarのscanデータが入っています。
* 絶対位置(`/dtw_robot1/tracking/groundtruth`)  
gazeboからとったロボットの「正しい」位置が入っています。リファレンスとして使います。

## tfの構造
ROSの標準では`map`、`odom`、`base_link`というのが大事なリンクの名前です。mapはワールドの原点（大体は起動した絶対的な位置）、base_linkはロボットの基準位置（差動2輪なら2つのタイヤの中心位置でz=0の場所）です。難しいのがodomでこれは時間とともにドリフトするが連続に動くワールドの基準点となります。
例えばodom->base_linkはホイールオドメトリなど連続的だが時間とともにずれる情報を入れて、そのずれをたまにLidarなどで環境とマッチングしたらmap->base_linkの関係がただしくなるようにmap->odomの関係をずらすことで調節します。

![nav_tf.png](https://qiita-image-store.s3.amazonaws.com/0/254442/799ea155-8199-06cd-1def-db778443a941.png)

## 名前空間の分離
今後1つのシミュレーションで複数のロボットを使うことを考慮して名前空間の分離をします。

* ROSノード、ROSトピックの分離
* tfの分離
* gazeboでの分離

が必要です。

# ソースコード

## urdfファイル
ほぼすべてsim2_lectureで使用したものと同様です。[githubのページ](https://github.com/project-srs/ros_lecture/tree/master/nav_lecture/xacro)にあげておきます。

名前空間の分離で注意があります。
rosコントロールのために記述する`<gazebo>`タグの`<robotNamespace>`タグの値はgazeboに出現させるロボット名と対応品暮れはなりません。またlidarのプラグインではtfの名前空間の分離をデフォルトでサポートしていますが、IMUだけは自分で名前空間を分離しないといけません。

## diff_drive_controll設定ファイル
差動2輪のコントローラーのコンフィグです。

```xml:nav_lecture/config/ros_control/diff_drive_controller.yaml
diff_drive_controller:
  type        : "diff_drive_controller/DiffDriveController"
  left_wheel  : 'left_wheel_joint'
  right_wheel : 'right_wheel_joint'

  publish_rate: 20.0
  cmd_vel_timeout: 1.0
  wheel_separation : 0.20
  wheel_radius : 0.05
  pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
  twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
    
  # tf
  #enable_odom_tf: true
  #base_frame_id: base_link
  #odom_frame_id: odom
    
  # limits
  linear:
    x:
      has_velocity_limits    : true
      max_velocity           :  0.55 # m/s
      min_velocity           : -0.55 # m/s
      has_acceleration_limits: true
      max_acceleration       :  1.0 # m/s^2
      min_acceleration       : -1.0 # m/s^2
  angular:
    z:
      has_velocity_limits    : true
      max_velocity           :  2.5 # rad/s
      min_velocity           : -2.5 # rad/s
      has_acceleration_limits: true
      max_acceleration       :  1.0  # rad/s^2
      min_acceleration       : -1.0 # rad/s^2
```

## launchファイル
```xml:nav_lecture/launch/basic.launch
<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="model" default="$(find nav_lecture)/xacro/dtw_robot.xacro" />
  <arg name="rvizconfig" default="$(find nav_lecture)/rviz/basic.rviz" />  
  <arg name="enable_odom_tf" default="true"/>
  
  <!-- gazebo world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <!-- dtw_robot -->
  <include file="$(find nav_lecture)/launch/dtw_spawn.launch">
    <arg name="robot_name" value="dtw_robot1"/>
    <arg name="enable_odom_tf" default="true"/>
  </include>

  <group ns="dtw_robot1">
    <!-- ground truth -->
    <node name="gazebo_odom_truth" pkg="nav_lecture" type="nav_gazebo_odom"  output="screen">
      <param name="model_name"  value="dtw_robot1" />
      <param name="odom_frame" value="dtw_robot1/odom" />
      <param name="base_frame"  value="dtw_robot1/ground_truth" />
      <param name="publish_rate"  value="20" />
      <param name="noise"  value="0.0" />
      <param name="tf_enable" value="true" />
      <remap from="odom" to="tracking/groundtruth" />
    </node>
  </group>

  <!-- rviz -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
```

# 実行
以下のコマンドで実行します。

```shell:シミュレーターの実行
roslaunch nav_lecture basic.launch
```

また以下のようにtwistをpublishすると動きます。

```shell:移動指示
rostopic pub -r 10 /wheel_robot/diff_drive_controller/cmd_vel geometry_msgs/Twist "linear:
  x: 0.3
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.2" 
```

![nav_normal.gif](https://qiita-image-store.s3.amazonaws.com/0/254442/8eb3bf63-d21d-a366-6793-7deb2771da15.gif)
緑の線はホイールオドメトリ、青い線がgazeboからとってきた絶対位置です。ホイールオドメトリは正しく出るはずなのに少しずれがあることが見えます。
今回はlaunchファイルの設定で`nav_gazebo_odom`ノードの`tf_enable`がtrueになっているので、gazebo位置でtfが出ています。

# 参考
[ROS navigation package](http://wiki.ros.org/ja/navigation)
[ROS navigation tutorial](http://wiki.ros.org/ja/navigation/Tutorials)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)