Gazeboシミュレート環境でtopicの確認・取得・処理方法(ROS2-Foxy)

Python
2022.10.24
スポンサーリンク

スポンサーリンク

はじめに

前回は Gazebo 上で IMU, LIDAR をシミュレートしました。

GazeboでIMU, LIDARをシミュレートする(ROS2-Foxy)
GazeboでIMU, LIDARをシミュレートする方法を説明します。 さらに、細かい設定方法と、Gazebo上で障害物を設置する方法を説明します。
nutritionfoodtech.com
2022.10.18

細かい設定が多すぎて、理解しにくいとは思いますが、実際に手動で変更する設定は少ないです。

今回はいよいよ作成したロボットを Gazebo 上で動かしていきます。
Gazebo と ROS2-Foxyの間でカメラ、IMU、LIDAR、ディファレンシャルドライブの topic を扱う予定です。

前提条件

前提条件

Gazebo 上のロボットに色を付ける

まずは Gazebo 上のロボットに色を付けます。

first_robot.urdf の /robot より上に、以下のコードを追記してください。

<!-- color settings -->
<gazebo reference="wheel_front_left">
  <material>Gazebo/Black</material>
</gazebo>

<gazebo reference="wheel_front_right">
  <material>Gazebo/Black</material>
</gazebo>

<gazebo reference="front_caster">
  <material>Gazebo/Green</material>
</gazebo>

コードの説明をします。

<gazebo reference="wheel_front_left">
    <material>Gazebo/Black</material>
</gazebo>

これは、wheel_front_left の link に Gazebo/Black の色を適用します。
選択可能な色は
“/usr/share/gazebo-11/media/materials/scripts/gazebo.material”
に記述されているので、nano エディタや VSCode で確認してみてください。

わからない場合は

cd /usr/share/gazebo-11/media/materials/scripts
code .

で、VSCodeを開き、gazebo.material を選択してください。

上記が完了しましたら、
前回の記事で作成した first_gazebo.launch.py を起動します。

cd ~/ros_ws
colcon build --packages-select ros2_first_test
source ~/.bashrc
ros2 launch ros2_first_test first_gazebo.launch.py

Rviz2 と Gazebo が表示されるはずです。

色は好みで設定してみてください。

カメラの topic を取得

Gazebo と Rviz2 を表示した状態でもう一つコンソールを開いてください。

ros2 topic list

と、入力すると以下のように出力されるはずです。

/camera1/camera_info
/camera1/image_raw
/clicked_point
/clock
/first_robot/cmd_vel
/first_robot/imu
/first_robot/odom
/goal_pose
/initialpose
/joint_states
/parameter_events
/robot_description
/rosout
/scan
/tf
/tf_static

このうち、

  • /camera1/image_raw
  • /camera1/camera_info
  • /first_robot/cmd_vel
  • /first_robot/imu
  • /first_robot/odom
  • /scan

が、以前の記事で追加した Gazebo のプラグインとなります。

/camera1/image_raw トピック

上から順にみていきます。/camera1/image_raw を確認します。

ros2 topic info /camera1/image_raw

上記を実行すると、以下のような出力が得られます。

Type: sensor_msgs/msg/Image
Publisher count: 1
Subscription count: 0

これは、sensor_msgs/msg/Image という型に則って、Publisher が画像データを出力しています。ということを表しています。

次に、/camera1/image_raw の中身を確認します。

ros2 topic echo /camera1/image_raw

上記を実行すると、以下のような出力が得られます。

header:
  stamp:
    sec: 28
    nanosec: 763000000
  frame_id: base_footprint
height: 800
width: 800
encoding: rgb8
is_bigendian: 0
step: 2400
data:
- 176
- 176
- 176
- 178
- 178
- 178

上記出力の詳細はROS公式サイトに掲載されていますが、
header は timestamp と frame_id で構成されており、今回の場合、frame_id は ロボットの親リンクである base_footprint となります。

height, width, encoding はカメラの画角とエンコード方式を表します。
URDF で指定した設定になっているか確認してください。

is_bigendian はビッグエンディアンかどうかを表します。PLC との通信くらいでしか意識したことないので、気にしなくても大丈夫です。

step は一行あたりの (columns x RGB) = 800 x 3 = 2400 となります。
これに width をかけると 1フレーム あたりの画素数になります。

data は実際の画像データとなります。

/camera1/camera_info トピック

/camera1/camera_info トピックを同様に確認していきます。

ros2 topic info /camera1/camera_info

上記を実行すると、以下の出力が得られます。

Type: sensor_msgs/msg/CameraInfo
Publisher count: 1
Subscription count: 0

sensor_msgs/msg/CameraInfo の内容はROS公式サイトに掲載されています。
実際に内容を見てみましょう。

ros2 topic echo /camera1/camera_info

上記を実行すると、以下の出力が得られます。

header:
  stamp:
    sec: 113
    nanosec: 476000000
  frame_id: base_footprint
height: 800
width: 800
distortion_model: plumb_bob
d:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
k:
- 476.7030836014194
- 0.0
- 400.5
- 0.0
- 476.7030836014194
- 400.5
- 0.0
- 0.0
- 1.0
r:
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
- 0.0
- 0.0
- 1.0
p:
- 476.7030836014194
- 0.0
- 400.5
- -0.0
- 0.0
- 476.7030836014194
- 400.5
- 0.0
- 0.0
- 0.0
- 1.0
- 0.0
binning_x: 0
binning_y: 0
roi:
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: false

distortion_model: plumb_bob はカメラの歪みを表します。ほとんどの場合、plumb_bob で問題ありません。
d: は distortion_model のパラメータを表します。
k: はカメラの固有行列を表します。
r: はステレオカメラを使用している場合の回転行列を表します。
p: は処理済みのカメラ固有行列を表します。

binning_x, binning_y はビニングを表します。
カメラに詳しくありませんが、感度を上昇させるパラメータのようです。

roi: は関心領域(RegionOfInterest)を表します。ディープラーニングでしか意識したことありません・・・。分かり次第追記します。

/first_robot/cmd_vel トピック

次に、/first_robot/cmd_vel について見ていきます。

ros2 topic info /first_robot/cmd_vel

上記コマンドを実行すると、以下の出力が得られます。

Type: geometry_msgs/msg/Twist
Publisher count: 0
Subscription count: 1

geometry_msgs/msg/Twist の内容はROSの公式サイトにあります。
中身は linear と angular で構成されています。それぞれ x, y, z 方向の、直進速度と角速度で構成されています。

また、今回初めて Subscription count: 1 が表示されています。
これは、受信側なので、/first_robot/cmd_vel に Twist の topic を送信すると、first_robot の車輪が回り始めます。

なので、

ros2 topic info /first_robot/cmd_vel

としても、Publisher が存在しないのでターミナルには何も表示されません。

/first_robot/imu トピック

/first_robot/imu トピックを確認していきます。

ros2 topic info /first_robot/imu

上記のコードを実行すると、以下の出力が得られます。

Type: sensor_msgs/msg/Imu
Publisher count: 1
Subscription count: 0

sensor_msgs/msg/Imu の内容はROS公式サイトにあります。

続いて Publisher count: 1 なので、出力内容を確認していきます。

ros2 topic echo /first_robot/imu

上記を実行すると、以下の出力が得られます。

header:
  stamp:
    sec: 902
    nanosec: 435000000
  frame_id: imu_link
orientation:
  x: -1.2019606649624688e-05
  y: -2.7075212462926695e-05
  z: -0.41458978897343357
  w: 0.9100084098520331
orientation_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
  x: -0.00011046231613901636
  y: -0.011477558389507396
  z: -0.00022275977790307537
angular_velocity_covariance:
- 4.0e-08
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
linear_acceleration:
  x: -0.16586039446302425
  y: -0.013969244199935556
  z: 10.002344470986605
linear_acceleration_covariance:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0

orientation は、方向を表します。
orientation_covariance は、方向の共分散を表します。
共分散が 0 の場合は、共分散が未知であると仮定され、共分散を使用しません。

angular_velocity は、角速度を表します。
linear_acceleration は直進加速度を表します。

/first_robot/odom トピック

/first_robot/odom を確認していきます。

ros2 topic info /first_robot/odom

上記を実行すると、以下の出力が得られます。

Type: nav_msgs/msg/Odometry
Publisher count: 1
Subscription count: 0

nav_msgs/msg/Odometry は、ROS公式サイトに説明があります。

実際に、中身を見てみましょう。

header:
  stamp:
    sec: 1545
    nanosec: 247000000
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x: 0.10429921379223925
      y: -0.5799168917627142
      z: 1.0613214183663455e-05
    orientation:
      x: -1.9963809292826467e-05
      y: -2.9212859914936493e-05
      z: -0.5648722525331772
      w: 0.8251783668190471
  covariance:
  - 1.0e-05
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1.0e-05
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001
twist:
  twist:
    linear:
      x: 0.000705745339546816
      y: -0.0003713427433124029
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: -0.000650926606319553
  covariance:
  - 1.0e-05
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1.0e-05
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1000000000000.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001

child_frame_id は、オドメトリの出力内容が child_frame_id を基準として出力されています、ということを表しています。
pose は PoseWithCovariance を表しており、位置、回転角、共分散が出力されます。
twist は TwistWithCovariance を表しており、直進速度、回転速度、共分散が出力されます。

注意点としては、オドメトリはディファレンシャルドライブのプラグインより算出された値なので、
あくまで推定値です、当然、ズレます。

/scan トピック

最後に、/scan の内容を確認していきます。

ros2 topic info /scan

上記を実行すると、以下の出力が得られます。

Type: sensor_msgs/msg/LaserScan
Publisher count: 1
Subscription count: 1

sensor_msgs/msg/LaserScan は、ROS公式サイトに内容が記載されています。

ここで初めて、
Publisher count: 1
Subscription count: 1
と表示されました。
これは、Gazebo 上の LIDAR が /scan topic を発行し、Rviz2 が /scan topic を取得しています。

続いて、内容を確認していきます。

header:
  stamp:
    sec: 3536
    nanosec: 420000000
  frame_id: lidar_link
angle_min: 0.0
angle_max: 3.140000104904175
angle_increment: 0.008746517822146416
time_increment: 0.0
scan_time: 0.0
range_min: 0.15000000596046448
range_max: 6.0
ranges:
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- .inf
- '...'
intensities:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0

出力は少し省略してあります。

angle_min, angle_max は URDF で指定した値になっていることを確認してください。
angle_increment は測定間の角度の増加を表しています。LIDAR もしくはロボットが時間とともに動いています。
time_increment は angle_increment が大きい場合、time_increment を使用して補間を行います。
scan_time はスキャン間の時間を表します。

range_min, range_max は URDF で指定した値になっているか確認してください。

ranges は、inf と表示されていますが、LIDAR の検知範囲内に障害物がない場合、infとなります。
障害物がある場合、(m) で出力されます。
intensities は、LIDAR が受信したレーザーの強度を表します。今回は設定していないので、障害物が存在しても 0.0 となります。

おわりに

今回は topic の中身の確認を中心に説明しました。
Python での取得まで説明したかったのですが、また、長くなりましたので次回にします。

次回こそ、Python で topic を扱っていきます。

Gazeboシミュレート上で IMU 等の topic を Python プログラムで扱う(ROS2-foxy)
Gazeboシミュレート上で IMU, LIDAR, カメラ, ディファレンシャルドライブの topic を Python プログラムで扱います(ROS2-foxy) また、Python から Gazebo 上へ topic を送信し、シミュレートするところまで説明します。
nutritionfoodtech.com
2022.10.29
スポンサーリンク

コメント

タイトルとURLをコピーしました