GPSを使いたくて、色々とネットを調べていると
hector_gazebo_plugin
や
libgazebo_gps_plugin.so
がヒットするのですが、なかなか ROS2で使える方法がわからず、ようやく見つけることができました。
/opt/ros/foxy/share/gazebo_plugins/worlds/gazebo_ros_gps_sensor_demo.world
にGPSセンサーのサンプルがありました。
これを参考に、次のようにｓｄｆに追記します。
<?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情報を取得できているようです。
