GPSを使いたくて、色々とネットを調べていると
hector_gazebo_plugin

libgazebo_gps_plugin.so
がヒットするのですが、なかなか ROS2で使える方法がわからず、ようやく見つけることができました。
/opt/ros/foxy/share/gazebo_plugins/worlds/gazebo_ros_gps_sensor_demo.world
にGPSセンサーのサンプルがありました。

https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/worlds/gazebo_ros_gps_sensor_demo.world

これを参考に、次のようにsdfに追記します。

<?xml version='1.0'?>
<sdf version='1.5'>
  <model name="ロボット名">
  ・・・・
    <link name="gps_link">
      <pose>0 0 10 0 0 0</pose>
      <sensor name="my_gps" type="gps">
        <always_on>true</always_on>
        <update_rate>30</update_rate>
        <gps>
          <position_sensing>
            <horizontal>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
              </noise>
            </horizontal>
            <vertical>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
              </noise>
            </vertical>
          </position_sensing>
        </gps>
        <plugin name="gps_plugin" filename="libgazebo_ros_gps_sensor.so">
          <ros>
            <argument>--ros-args --remap ~/out:=gps_data</argument>
          </ros>
        </plugin>
      </sensor>
    </link>

    <joint name="gps_joint" type="fixed">
      <parent>base_link</parent>
      <child>gps_link</child>
    </joint>

  </model>
</sdf>

GPSの位置情報はWorldに書き込みます。私の環境では、北を合わせるために180回転させる必要がありましたので、

<heading_deg>180</heading_deg>

と変更してあります。

<sdf version='1.7'>
  <world name='default'>
    <spherical_coordinates>
      <surface_model>EARTH_WGS84</surface_model>
      <latitude_deg>36.0824</latitude_deg>
      <longitude_deg>140.078</longitude_deg>
      <elevation>0</elevation>
      <heading_deg>180</heading_deg>
    </spherical_coordinates>
・・・・
  </world>
</sdf>

次にIMUです。D455というカメラの場合、カメラ内にIMUが搭載されているのでカメラリンク内に配置しました。

<link name="camera_link">
  <pose>0 0 1.2 0 0.349066 0</pose>
  <inertial>
    ・・・
  </inertial>
  <collision name="collision">
   ・・・・
  </collision>
  <visual name="camera_visual">
    ・・・
  </visual>
  <sensor type="camera" name="camera">
    ・・・
  </sensor>

  <sensor type="depth" name="depth">
    ・・・
  </sensor>

  <sensor name="imu" type="imu">
    <imu>
      <angular_velocity>
        <x>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </noise>
        </x>
        <y>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </noise>
        </y>
        <z>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </noise>
        </z>
      </angular_velocity>
      <linear_acceleration>
        <x>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </x>
        <y>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </y>
        <z>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </z>
      </linear_acceleration>
    </imu>
    <always_on>1</always_on>
    <update_rate>1000</update_rate>
    <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
      <topicName>imu</topicName>
      <bodyName>camera_link</bodyName>
      <updateRateHZ>10.0</updateRateHZ>
      <gaussianNoise>0.0</gaussianNoise>
      <xyzOffset>0 0 0</xyzOffset>
      <rpyOffset>0 0 0</rpyOffset>
      <frameName>camera_link</frameName>
      <initialOrientationAsReference>false</initialOrientationAsReference>
    </plugin>
  </sensor>

</link>

実行するとgps_plugin/outが得られます。

$ ros2 topic list
/camera/camera_info
/camera/image_raw
/clicked_point
/clock
/depth/camera_info
/depth/depth/camera_info
/depth/depth/image_raw
/depth/image_raw
/depth/points
/goal_pose
/gps_plugin/out
/imu_plugin/out
/initialpose
/joint_states
/momo/cmd_vel
/momo/odom
/parameter_events
/performance_metrics
/robot_description
/rosout
/tf
/tf_static

取得できた値を見てみます。

$ ros2 topic echo /gps_plugin/out
header:
stamp:
sec: 5662
nanosec: 578000000
frame_id: gps_link
status:
status: 0
service: 1
latitude: 36.08194676155107
longitude: 140.07913439959185
altitude: 10.403717489913106
position_covariance:

うまくGPS情報を取得できているようです。

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です