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情報を取得できているようです。