7.4 オドメトリのキャリブレーション

実機ロボットのオドメトリをより正確なものにするために、キャリブレーションを行ないます。
7.4.1 並進方向のキャリブレーションおよび7.4.2 回転方向のキャリブレーションを参考にキャリブレーション用のパラメータを求めたら、以下のように設定して下さい。

床の材質によってキャリブレーションの値は大きく変化します。場所ごとにキャリブレーションを行って、以下のlaunchファイルを複数用意しておくのがいいでしょう。

TurtleBotの場合

turtlebot_carpet_create.launch を編集して、以下のように数値を変更して下さい。

<launch>
  <param name="/use_sim_time" value="false" />
  <!-- Calibration parameters after calibration of an original Create TurtleBot on low-ply carpet -->
  <param name="turtlebot_node/gyro_measurement_range" value="150"/>
  <param name="turtlebot_node/gyro_scale_correction" value="1.43"/>
  <param name="turtlebot_node/odom_angular_scale_correction" value="1.0 "/>
  <param name="turtlebot_node/odom_linear_scale_correction" value="0.955"/>

  <include file="$(find rbx1_bringup)/launch/turtlebot_minimal_create.launch" />

</launch>

以降は、キャリブレーション済みの launchファイルを使って下さい。

$ roslaunch rbx1_bringup turtlebot_carpet_create.launch

Kobukiの場合

turtlebot_carpet_create.launch をコピーして、turtlebot_carpet_kobuki.launch を作成し、以下のように変更して下さい。

<launch>
  <param name="/use_sim_time" value="false" />
  <!-- Calibration parameters after calibration of an original Create TurtleBot on low-ply carpet -->
  <param name="turtlebot_node/gyro_measurement_range" value="150"/>
  <param name="turtlebot_node/gyro_scale_correction" value="1.43"/>
  <param name="turtlebot_node/odom_angular_scale_correction" value="1.0 "/>
  <param name="turtlebot_node/odom_linear_scale_correction" value="0.955"/>

  <include file="$(find rbx1_bringup)/launch/turtlebot_minimal_kobuki.launch" />

以降は、キャリブレーション済みの launchファイルを使って下さい。

$ roslaunch rbx1_bringup turtlebot_carpet_kobuki.launch

7.3.2 RVizによるロボットの可視化

RVizは強力な可視化ツールです。
ここでは、シミュレータを使い、Twistメッセージによりロボットが動く様子を可視化してみます。

シミュレータでTurtlebotを動かして

$roslaunch rbx1_bringup fake_turtlebot.launch

別のターミナルウィンドウで、rvizを動かします。

$rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz

さらに別のターミナルウィンドウで以下のTwistメッセージを配信します

$rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: -0.5}}'

以下の図のように反時計方向に0.1ラジアン毎秒で回転しながら、0.1メートル毎秒で直進します。

rostopic の -r 10 オプションは10Hz（毎秒10回）の間隔でTwistメッセージを /cmd_velに発行することを意味します。

ROSでは「安全第一の設計」がされていて、Twistメッセージを送り続けないとロボットは動きません。

ロボットの動きを止めたい時は、以下のように空のTwistメッセージ発行してください。

$rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}'

7.3 Twistメッセージによるロボットの移動

ROSでは base controller へのモーションコマンドとして Twist型のメッセージを使います。
頻繁に見かけるのは、以下の様な例です。
「”command velocities"と呼ばれる/cmd_vel ノードにTwistが型のメッセージを配信する」


7.3.1 Twist型メッセージの例

以下のコマンドでTwist型メッセージの詳細が表示されます

$ rosmsg show geometry_msgs/Twist

geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z

geometry_msgs/Twist 型は二つのサブメッセージから成り、それぞれ float64型が3つからなるベクター（配列）型です。

geometry_msgs/Vector3 linear  は並進方向の速度を意味し、それぞれ x, y, z 方向の速度をメートル毎秒で与えます。

また、geometry_msgs/Vector3 angular  は回転方向の速度を意味し、それぞれ x, y, z 軸回りの回転速度をラジアン毎秒で与えます。

例えば、ロボットを0.1メートル/秒で前進させたいときには、

'{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'

※ここで、サブメッセージ内の3つの値のデリミタは「コロン＋スペース」なのを注意して下さい。

x: 0.1 (x + : +  スペース＋数値）です。：の後にスペースを忘れないように。

同様に、ロボットをその場で反時計方向に回転させるには以下のようにします。

'{linear: {x: 0.0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.1}}'

実際にROSのコマンドでロボットを移動させるには以下の様に入力します。

$rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: -0.5}}'

時計方向に0.5ラジアン毎秒で回転しながら、0.1メートル毎秒で移動します。

7.2 移動制御の抽象化のレベル

7.2 モーションコントロール
ROSでは「与えらえたゴールに移動する」というタスクに関して、モーターへの指示コマンド（抽象度が低い）から移動先のゴール（位置と姿勢）を与える（抽象度が高い）までを幾つかの抽象度に分解して実装しています。
以下の節では、抽象度の低い順に紹介されています。

7.2.1 モータ, 車輪, エンコーダ

移動ロボットの制御においては、オドメトリ（内部センサによる移動量の推定）が重要になり、エンコーダから得られるモータの回転数だけでなく、加速度計などの移動量を測定できるセンサを使うこともあります。

例えば、Turtlebotで使っているRoombaは2つのモータの回転数に加え、一軸のジャイロにより姿勢の推定精度を向上させています。

7.2.2 モータのコントローラとドライバ
移動ロボットの一番低レベルの制御は各モータのスピードを制御することです。
車輪のモータで速度を決めだけで無く、ステアリングのモータを制御して進行方向を変えたり、パンチルト台のモータを制御してカメラが見ている方向を変えることなども含まれます。このレベルは個々のロボット毎に大きく異なります。
モータの数や車輪の大きさはモータ毎に違うので、このレベルにおいてはロボット毎に異なるドライバを実装するのが普通です。

7.2.3 ロボット台車の制御
このレベルでは上位レベルから指示された通りに台車を制御することが要求されます。
例えば、「xxメータ/秒で動かす」「xxラジアン/秒で回転させる」などをいかに正確に実現するかが求められます。
指示された制御量にスムーズに追随するため、PD制御やPID制御と言われるドライバが実装されています。
例えば、あなたのロボットが静止状態にあり、「0.5m/sec（秒速50㎝）で前に進め」という指示をしたいとします。
その時、静止状態のロボットにいきなり0.5m/secの速度指定コマンドを送るのは現実的ではありません。急発進して、場合によっては倒れてしまうかもしれません。
そこで、ROSのBase Controllerノードでは、PID制御のような手法を使って、「スムーズに」指定された速度になるまでロボットの速度コマンドを細分化します。
例えば、以下のように一定時間間隔で徐々に速度を上げていきます。　
0.0m/sec ->0.05m/sec -> 0.1m/sec -> 0.15m/sec ..... -> 0.5m/sec

この base controller ノードと呼ばれるノードからは自己位置（オドメトリ）が /odom トピックとして配信され、/cmd_vel  ノードでは上位レベルからの移動速度を待っています。
同時に、コントローラノードは、一般的に（常にではないが）ベースフレーム（/ base_linkまたは/ base_footprintのいずれか）へ/odmを変換しています。

TurtleBotのようないくつかのロボットでは、ロボットの位置・姿勢のより正確な推定値を取得するために、車輪のオドメトリとジャイロデータを結合するrobot_pose_ekfパッケージを使用します。この場合には、robot_pose_ekfノードが/odom から/ base_footprintに変換しています。（ robot_pose_ekfパッケージは拡張カルマンフィルタにより実装されています。）

このように、自分のロボット専用の base controller ノードを作ってしまえば、後は、上位レベルのプログラミングに専念することが出来ます。

7.2.4 move_base ROS パッケージによるFrame-Base Motion
move_baseパッケージは目的地（一般的な移動ロボットでは移動先の位置・姿勢）を与えられたときに現在地から目的地まで、障害物を回避しながら経路を生成し、移動する機能を提供します。
move_baseパッケージは、非常に洗練された経路計画であり、ロボットが移動するためのパスを選択する際に、ローカルおよびグローバルなコストマップの両方とオドメトリデータを組み合わせを利用します。
7.2.5 gmapping と amcl パッケージによるSLAM
gmappingパッケージにより地図を作成します。一般に、SLAMで地図を作るにはレーザレンジファインのような正確な距離センサが必要ですが、Turtlebotに搭載のKinectやXtionでも十分な精度の地図を得ることが可能です。

地図が作れたら、amcl (adaptive Monte Carlo localization) パッケージを使い正確な自己位置・姿勢を得ることができます。
7.2.6 セマンティックゴール　Semantic Goals
抽象化の最上位のこのレベルでは、
「キッチンに行って、ビールを取ってきて」

とか、もっと曖昧に
「ビールを取ってきて」
のように、実現してもらいたいGoalをロボットに指示します。
7.2.7 まとめ

[Goal]
↓
[AMCL]
↓
[Path Planner]
↓
[move_base]
↓
[/cmd_vel + /odom]
↓
[Base Controller]
↓
[Motor Speeds]

7.1 単位系と座標系

7. 移動ロボットを動かす
7.1 単位系および座標系

座標系

ROSではいわゆる「右手座標系」が使われています。

z座標の回転は「反時計回りが正」、「時計回りが負」の値になります。

単位系

直進方向の速度は　メートル・毎秒[ m / sec]

回転方向の速度は　ラジアン・毎秒[ rad / sec]

6.3 独自ロボットをシミュレータで試す

手元にはKobukiがベースのTurtlebot2があったので、Arbotixシミュレータで動かしてみました。
これです→

~/catkin_ws/src/rbx1/rbx1_bringup/launch ディレクトリにある、fake_turtlebot.launchをTurtlebot2 用に修正します。

$roscd rbx1_bringup/launch
あるいは
$cd ~/catkin_ws/src/rbx1/rbx1_bringup/launch
どちらでも同じです。
$cp fake_turtlebot.launch fake_turtlebot2.launch

ここで、fake_turtlebot2.launch を以下のように修正します。

<launch>

  <param name="/use_sim_time" value="false" />

  <!-- Load the URDF/Xacro model of our robot -->

  <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find turtlebot_description)/robots/kobuki_hexagons_kinect.urdf.xacro'" />

  <param name="robot_description" command="$(arg urdf_file)" />

  <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">

      <rosparam command="delete" param="/arbotix" />

      <rosparam file="$(find rbx1_bringup)/config/fake_turtlebot2_arbotix.yaml" command="load" />

      <param name="sim" value="true"/>

  </node>

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">

      <param name="publish_frequency" type="double" value="20.0" />

  </node>

</launch>

先々の為に、

<rosparam file="$(find rbx1_bringup)/config/fake_turtlebot_arbotix.yaml" command="load" />

↓

<rosparam file="$(find rbx1_bringup)/config/fake_turtlebot2_arbotix.yaml" command="load" />

このように修正して。

cp $(find rbx1_bringup)/config/turtlebot_arbotix.yaml $(find rbx1_bringup)/config/fake_turtlebot2_arbotix.yaml

のようにコピーしておく。いまのところ中身はそのままです。

これでTurtlebot2がシミュレータで使えます。

$roslaunch rbx1_bringup fake_turtlebot2.launch

可視化ツールのrvizで表示します

$rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz

$rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'

同じように、これで動きます。

真上からなので解りにくいですけど、Turtlebot2になりました。

6.2 Arbotixシミュレータのテスト

いよいよシミュレータでロボットを動かします。
ROS by ExampleのサンプルはTurtlebotです。

一つ目のTerminalを開いて、roscore を実行します。この本では、 各章のサンプルでコマンドをコピペすれば動くのですが、roscoreを実行しなければいけないことを説明していないので注意して下さい。

（注意）Terminalを開いたら、以下のコマンドを忘れないで下さい。
ターミナルウィンドウは Ctrl+Alt+t キーで開きます。

$ cd ~/catkin_ws
$ source devel/setup.bash

 一つ目のTerminalで

$ roscore

二つ目のTerminalで

$roslaunch rbx1_bringup fake_turtlebot.launch

もし別のロボット（Pi Robot）で実行したいなら、二つ目のTerminalで

$roslaunch rbx1_bringup fake_pi_robot.launch

可視化ツールのrvizで表示します

$rosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz

ここまでで、Arbotixシミュレータが起動し、指定したロボットが rvizで表示されていると思います。

次に、シミュレータ上のロボットを動かします。
三つ目のTerminalで以下のコマンドを入力して下さい。

$rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'

少し長いコマンドですが、スペースの部分でタブキーを押すと以下の候補が表示されるので、入力が楽になります。

ロボットを止めるには以下のコマンドを入力します。

$rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}'

ここでロボットを動かした rostopic コマンドは、指定したトピックにメッセージをパブリッシュするコマンドで頻繁に使うコマンドです。

詳しくは、後ほど説明しようと思います。

ここでは、「/cmd_vel  というトピックに geometry_msgs/Twist  という型のメッセージを送るとロボットが動く」、と覚えておいてください。