ROS2でgnssとimuを利用して、自己位置推定(実装の備忘録）

これ実装済み前提です

IMUドライバ
　https://github.com/technoroad/ADI_IMU_TR_Driver_ROS2
　参考 https://qiita.com/m2_labo/items/db32061bf3cbfd8f721e

GNSS
　https://github.com/KumarRobotics/ublox/tree/galactic-devel
　参考 https://qiita.com/m2_labo/items/1ddf05a03a1871ff4a28

basic roboto localization
　https://automaticaddison.com/how-to-use-gps-with-the-robot-localization-package-ros-2/
　参考　作成中

base_robot_localizationの設定とrobot_localizationの設定を変更

①ros2/src/basic_mobile_robot/basic_mobile_bot_v6.launch.py

変更前

remappings=[('imu', 'imu/data'),
                ('gps/fix', 'gps/fix'), 
                ('gps/filtered', 'gps/filtered'),
                ('odometry/gps', 'odometry/gps'),
                ('odometry/filtered', 'odometry/global')])

変更後

remappings=[('imu', 'imu/data_raw'),
                ('gps/fix', 'fix'), 
                ('gps/filtered', 'gps/filtered'),
                ('odometry/gps', 'odometry/gps'),
                ('odometry/filtered', 'odometry/global')])

②ros2/src/basic_mobile_robot/config/ekf_with_gps.yaml

134、284行目あたり
変更前

imu0: imu/data

変更後

imu0: imu/data_raw

③ros2/src/models/basic_mobile_bot_v1.urdf

変更前

  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="-0.10 0 0.05" rpy="0 0 0"/>
  </joint>

  <link name="imu_link"/>

  <joint name="gps_joint" type="fixed">
    <parent link="base_link"/>
    <child link="gps_link"/>
    <origin xyz="0.10 0 0.05" rpy="0 0 0"/>
  </joint>

  <link name="gps_link"/>

変更後

  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu"/>
    <origin xyz="-0.10 0 0.05" rpy="0 0 0"/>
  </joint>
  <link name="imu"/> 

   <joint name="gps_joint" type="fixed">
    <parent link="base_link"/>
    <child link="gps"/>
    <origin xyz="0.10 0 0.05" rpy="0 0 0"/>
  </joint>

  <link name="gps"/>

④ros2/src/models/basic_mobile_bot_description/model.sdf

変更前

<link name="imu_link">

<link name="gps_link">

変更後

<link name="imu">

<link name="gps">

実行＆確認

f9pとimuをPCに接続

cd ros2
colcon build
source install/setup.bash

以下すべて別のターミナルで実施
gpsとimuを入れ込んで、ekfさせるノードの実行

cd ros2
source install/setup.bash
ros2 launch basic_mobile_robot basic_mobile_bot_v6.launch.py

gpsノードの実行

cd ros2
source install/setup.bash
ros2 launch ublox_gps ublox_gps_node-launch.py

imuノードの実行(ttyACM1は実際のシリアルポートに合わせてください）

cd ros2
source install/setup.bash
ros2 launch adi_imu_tr_driver_ros2 adis_rcv_csv.launch.py mode:=Register device:=/dev/ttyACM1

確認

cd ros2
source install/setup.bash
rqt_graph

以下の画像が表示されればOK

cd ros2
source install/setup.bash
ros2 topic echo /odometry/gps

反応あればOK！（反応なければデータいっていない）

次は出力を実環境に持っていく。