環境

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

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

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

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

ロボット仕様

今回も差動2輪型のロボットになります。センサーとしてはIMUと2次元lidarがついています。

キャスターには不安定性がある

差動2輪機構を例として今後実行します。後ろの従輪は実際はキャスターを使うかと思いますが、シミュレーション編と同様にここには摩擦の低い半球を使っています。キャスターには「向き」があり、この向きによって同じcmd_velでも挙動が変わってしまいます。無用のトラブルを避けるためにキャスターは避けましょう。

アクチュエーター

差動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の関係をずらすことで調節します。

名前空間の分離

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

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

が必要です。

ソースコード

urdfファイル

ほぼすべてsim2_lectureで使用したものと同様です。githubのページにあげておきます。

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

diff_drive_controll設定ファイル

差動2輪のコントローラーのコンフィグです。

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ファイル

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>

実行

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

シミュレーターの実行

roslaunch nav_lecture basic.launch

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

移動指示

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"

緑の線はホイールオドメトリ、青い線がgazeboからとってきた絶対位置です。ホイールオドメトリは正しく出るはずなのに少しずれがあることが見えます。
今回はlaunchファイルの設定でnav_gazebo_odomノードのtf_enableがtrueになっているので、gazebo位置でtfが出ています。

参考

ROS navigation package
ROS navigation tutorial

目次ページへのリンク

ROS講座の目次へのリンク

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

環境

概要