ROS講座40 車輪ロボット位置を取得・ros_control経由で関節角度を取得

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

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |
| Gazebo | 11.9.0 |

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

# 概要
twistを送ってロボットに指示をするとロボットがgazebo上で動きます。ここまで内容ではgazeboのウィンドウのみが立ち上がり、rvizは使いませんでした。今後高度な可視化をするためにはrvizが必要不可欠です。gazeboで表示されているロボットをrviz上で表示する方法を説明します。
rviz上でのロボットの表示はtfに基づいています。つまりtfにロボットの位置をpublishすればrviz上で表示を行うことができます。このために今回は

* staticな関係をtfに出す。
* jointの回転位置をtfに出す。
* ロボット自体の位置をtfに出す。

の3つの作業を行います。

# 3つのtfの出し方概要

今回のロボットのtfの構成は以下のようになっています。黒はfixedなtf、赤はjointで定義されているtf、青はロボットの自己位置を示しています。

![tf_tree.png](https://qiita-image-store.s3.amazonaws.com/0/254442/297fe6e7-fb07-4164-c8dc-3fa40596cc5c.png)

## staticな関係をtfに出す
これは簡単で`robot_state_publisher`を起動するだけです。これはロボットのurdf(`robot_description`)を読み取ってfixedなjointでつながっているlinkの関係をtfに自動で出してくれます。

## jointの回転位置をtfに出す
これには`joint_state_controller/JointStateController`というros_controlが必要です。これはgazebo上でのjointの角度を`/joint_states`にpublishしてくれます。ここまでくると上記の`robot_state_publisher`が自動でtfに変換してくれます。

## ロボット自体の位置をtfに出す
これには2通りの方法があります。

### wheel odometryを使う方法
ロボットの自己位置の1つとして有名なのがwheel odometryです。これは「ロボットが1m/sで1s進んだからきっと1m前に進んでいるだろう」という推定を繰り返していくものです。連続的な位置が取れますが、時間とともに誤差が積分されてずれてしまいます。

### gazeboワールド上の位置を使う方法
`p3d`というgazeboプラグインを使えばgazeboワールド上での位置を取得することができます。`nav_msgs::Odometry`でROSトピックが出てくるので、これをtfに変換するノードを作って使用します。

# gazeboワールド上の位置の取得ノード

## 変換ノード

Odometry型のROSトピックをsubscribeして、tfを出力します。

```c++:gazebo_urdf_lecture/src/odom_tf_converter.cpp
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <string>
#include <math.h>

class Converter
{
public:
  Converter(void)
  {
    joy_sub_ = nh_.subscribe("/tracker", 10, &Converter::odomCallback, this);
  }
  void odomCallback(const nav_msgs::Odometry& msg)
  {
    tf::Transform transform;
    tf::poseMsgToTF(msg.pose.pose, transform);
    br_.sendTransform(tf::StampedTransform(transform, msg.header.stamp, /*msg.header.frame_id*/ "odom", msg.child_frame_id));
  }
  ros::NodeHandle nh_;
  ros::Subscriber joy_sub_;
  tf::TransformBroadcaster br_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "sim1_odom_tf_converter");
  Converter converter;
  ros::spin();
  return 0;
}
```

ROSトピックの受信もtfのbroadcastも今までに解説した内容です。ただノードの書き方のスタイルが今までと違います。これは`tf::TransformBroadcaster`のライフサイクルの問題です。実は`tf::TransformBroadcaster`はインスタンス化した段階から`/tf`のトピックにたいしてadvertiseを行います。しかしトピックへのadvertiseを`ros::NodeHandle`のインスタンス化の前に行うと実行時エラーになってしまいます。このインスタンス化の順番を守るためにこのようなスタイルになっています。

## CmakeList
以下の3点を追加します。

```cmake:sim_lecture/CMakeLists.txtの一部
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  tf　　　#この行を追加
)

add_executable(sim1_odom_tf_convertersrc/sim1_odom_tf_converter.cpp)

target_link_libraries(sim1_odom_tf_converter
  ${catkin_LIBRARIES}
)
```

## ビルド
```shell
cd ~/catkin_ws
catkin_make
```

# 位置・関節角度の取得の設定

## urdfの追加

以下を`<robot>`タグの中の最後に追加します。

```xml:gazebo_urdf_lecture/xacro/dtw_robot3.xacroの追加
  <gazebo>
    <plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
      <frameName>world</frameName>
      <bodyName>base_link</bodyName>
      <topicName>/tracker</topicName>
      <updateRate>10.0</updateRate>
    </plugin>
  </gazebo>
```

* `frameName`は出力する位置の基準を指定する変数です。worldかmapならgazeboワールド上での位置を、ほかの名前を指定するとgazeboの中のその名前のリンクからの相対位置になります（gazebo上でのリンク名≠urdfでのframe名なので注意）。またこの名前は出力されるOdometryのframe_idにもなります。
* `bodyName`は出力するOdometryトピックのchild_frame_idになります。
* `topicName`は出力するtopic名です。
* `updateRate`は出力周期です。

## launch

```xml:gazebo_urdf_lecture/launch/dtw_gazebo_rviz.launch
<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="position" default="wheel" /> <!-- wheel or tracker -->
  <arg name="model" default="$(find gazebo_urdf_lecture)/xacro/dtw_robot.xacro" />
  <arg name="rvizconfig" default="$(find gazebo_urdf_lecture)/rviz/dtw_gazebo.rviz" />  
  <param name="robot_description" command="$(find xacro)/xacro $(arg model) --inorder"/>

  <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>

  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model dtw_robot" />

  <group if="$(eval position=='wheel')">
    <param name="/dtw_robot/diff_drive_controller/enable_odom_tf" value="true" />
  </group>
  <group unless="$(eval position=='wheel')">
    <param name="/dtw_robot/diff_drive_controller/enable_odom_tf" value="false" />
  </group>

  <group if="$(eval position=='tracker')">
    <node name="odom_tf_converter" pkg="gazebo_urdf_lecture" type="odom_tf_converter">
      <param name="parent_frame_id" value="odom"/>
    </node>
  </group>

  <rosparam command="load"  file="$(find gazebo_urdf_lecture)/config/diff_drive_controller.yaml" ns="/dtw_robot"/>
  <rosparam command="load"  file="$(find gazebo_urdf_lecture)/config/joint_state_controller.yaml" ns="/dtw_robot"/>
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/dtw_robot" 
      args="diff_drive_controller joint_state_controller" />

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" ns="/dtw_robot"/>
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
```

* `position`という名前のargで自己位置の種類を切り替えています。`wheel`ならホイールオドメトリ、`tracker`ならgazenoワールド上に位置を使います。
* ホイールオドメトリ由来のtfを出力するかは`/dtw_robot/diff_drive_controller/enable_odom_tf`というrosparamで決定します。この値によってgazeboプラグインの`diff_drive_controller`の動作が変わります。
* ros_controlの起動の部分で`joint_state_controller`を追加しています。これは動軸の値の情報を`/joint_status`に送信してくれるgazeboプラグインです。これに必要なrosparamの追加情報が以下になります。

```yaml:gazebo_urdf_lecture/config/joint_state_controller.yaml
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50  
```

# 実行
各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

```shell:wheel_odometryを使う場合
roslaunch gazebo_urdf_lecture dtw_gazebo.launch position:=wheel
```

```shell:gazeboの位置を使う場合
roslaunch gazebo_urdf_lecture dtw_gazebo.launch position:=tracker
```

前回と違いはrvizとgazeboの両方の画面が出現します。rvizの画面でも車輪ロボットが移動します。

![sim_move_rviz.gif](https://qiita-image-store.s3.amazonaws.com/0/254442/ca01c0ad-f11a-321a-f357-ebb8769d8b10.gif)

rvizで上記のような表示にするためには以下の作業が必要です。

* Fixed Frameをodomに変更
* tfをadd
* RobotModelをadd

# コメント
wheel odometryの場合は例えばgazeboで強制的に位置を変更してもrviz上での位置は動きません。これは車輪による移動以外の移動はwheel odometryに反映されないからです。一方gazebo位置を使う方法ではこのようなことをするとRviz上の表示も移動します。
一見gazebo位置を使うほうが良い感じがしますが、このように絶対的な位置をロボットが使うためにはモーショントラッカーなどのリッチな環境が必要で、実験室でない限りロボットが動く実環境ではあまりこういうものがあることは望めません。
実環境で使うことを考えるとwheel odometryを使うほうが現実的で、徐々にずれてしまうなどの問題にはLIDARを使ったSLAMと組み合わせる等して対処します。これらの技法は「自己位置推定(Localization)」といってロボットの一大分野となります。

# /gazebo/model_statesについて
`/gazebo/model_state`というROSトピックを見ると実はgazeboワールド上のモデルの位置を出力しています。これを使えばp3dプラグインを使わなくてもgazebo上の位置を取得できます。しかしこのROSトピックはgazenoワールドの更新頻度（デフォルトでは1000Hz）で出ているので、使用するとCPUの負荷の面でよろしくありません。p3dプラグインを使うほうが良いでしょう。

# 参考
[gazeboチュートリアル](http://gazebosim.org/tutorials?tut=ros_comm)
[gazeboをROSに繋ぐ](http://cir-kit.github.io/blog/2015/03/02/gazebo-ros-communication/)
[tfと他の型の変換](http://docs.ros.org/diamondback/api/tf/html/c%2B%2B/namespacetf.html)

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