環境

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

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

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

概要

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、青はロボットの自己位置を示しています。

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を出力します。

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点を追加します。

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

ビルド

cd ~/catkin_ws
catkin_make

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

urdfの追加

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

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

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の追加情報が以下になります。

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を実行する必要があります。

wheel_odometryを使う場合

roslaunch gazebo_urdf_lecture dtw_gazebo.launch position:=wheel

gazeboの位置を使う場合

roslaunch gazebo_urdf_lecture dtw_gazebo.launch position:=tracker

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

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チュートリアル
 gazeboをROSに繋ぐ
 tfと他の型の変換

目次ページへのリンク

ROS講座の目次へのリンク

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

環境

概要