Eagleyeを使ってみる

# はじめに

7/2 ~ 11/9で[自動運転AIチャレンジ2024](https://www.jsae.or.jp/jaaic/2024ver/)に参加させていただいておりました。

コンペティション自体は終了しましたので、開催期間中に試せなかったアイディアを消化していきます（ここからは来年度以降の参考になるように…と言いつつ完全自己満足で記事をまとめていきます）。

いくつか試したいアイディアはあるのですが、今回は、**GNSS / IMU / 車輪速ベース（LiDAR / ポイントクラウドなし）自己位置推定OSSの`Eagleye`** を使ってみます。

本記事では`Eagleye`のインストール、および2024大会デフォルト設定における処理結果との比較を行います。`Autoware`との結合も可能なのですが、今回はスコープ外としていますのでご了承ください。

::: note
結論を先に。
- `Eagleye`はAIチャレンジ環境にも容易にインストール可能。
- 本記事の設定だと、`Twist`は遅延あり。
- `Pose`は、出力を得るために変換が必要で、適切な変換設定が見つけられておりません。
:::

# Eagleyeとは

マップフォー社が開発している、GNSS / IMUベースのローカライザです。
LiDARとポイントクラウドを使用する方式に比べコスト的に優位。

> Eagleye is an open-source GNSS/IMU-based localizer initially developed by MAP IV. Inc. It provides a cost-effective alternative to LiDAR and point cloud-based localization by using low-cost GNSS and IMU sensors to provide vehicle position, orientation, and altitude information.

Autowareローカライザスタックでも利用可能です。

> Eagleye can be utilized in the Autoware localization stack in two ways:
> 1. Feed only twist into the EKF localizer.
> 1. Feed both twist and pose from Eagleye into the EKF localizer (twist can also be used with regular gyro_odometry).

https://github.com/AutomotiveAIChallenge/autoware-documentation/blob/main/docs/how-to-guides/integrating-autoware/launch-autoware/localization-methods/eagleye-guide.md

2024大会のアーキテクチャで言えば、拡張カルマンフィルタによる自己位置推定への入力（`/localization/imu_gnss_poser/pose_with_covariance`, `/localization/twist_estimator/twist_with_covariance`）を、`Eagleye`出力（`eagleye/pose_with_covariance`, `/eagleye/twist_with_covariance`）に代替する形で利用できそうです。のちほど、ここの比較を行います。

https://automotiveaichallenge.github.io/aichallenge-documentation-2024/development/main-module.html

※ なお既に`Eagleye`に関するQiita記事は存在しているのですが、ROS版で少し古いため、今回ROS 2版でアップデートします。

https://qiita.com/KHMY/items/238132dd0508bd241633


# 使ってみる

::: note
開発環境は、自動運転AIチャレンジ2024環境（ROS 2 Humble）を前提とします。
以降のコマンドはすべて上記の開発コンテナ内で実行してください。
:::

`Eagleye` の ROS2ブランチ に記載されている手順に従います。

https://github.com/MapIV/eagleye/tree/main-ros2?tab=readme-ov-file#how-to-install

## インストール

```bash:terminal 1
sudo apt-get install gfortran
cd $HOME
git clone -b rtklib_ros_bridge_b34 https://github.com/MapIV/RTKLIB.git
cd $HOME/RTKLIB/lib/iers/gcc/
make
cd $HOME/RTKLIB/app/consapp
make 
```

```bash:terminal 1
cd /aichallenge/workspace/src/
git clone https://github.com/MapIV/eagleye.git -b main-ros2 --recursive
git clone https://github.com/MapIV/rtklib_ros_bridge.git -b ros2-v0.1.0
git clone https://github.com/MapIV/llh_converter.git -b ros2
git clone https://github.com/MapIV/nmea_ros_bridge.git -b ros2-v0.1.0
git clone https://github.com/MapIV/gnss_compass_ros.git -b main-ros2
sudo apt-get install -y libgeographic-dev geographiclib-tools geographiclib-doc
sudo geographiclib-get-geoids best
sudo mkdir /usr/share/GSIGEO
sudo cp llh_converter/data/gsigeo2011_ver2_1.asc /usr/share/GSIGEO/
cd ..
rosdep update
rosdep install --from-paths src --ignore-src -r -y
cd ..
./build_autoware.bash clean
```

※ もしビルドできない場合は、以下のようにビルドスクリプトを変更してください。

```diff_shell:build_autoware.bash
#!/bin/bash

if [[ ${1} == "clean" ]]; then
    echo "clean build"
    rm -r ./workspace/build/* ./workspace/install/*
fi

cd ./workspace || exit
- colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+ MAKEFLAGS="-j4" colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers 1
```

https://autowarefoundation.github.io/autoware-documentation/main/support/troubleshooting/#insufficient-memory

## 単体実行

`Eagleye`単体で動作確認します。

```bash:terminal 1
source /aichallenge/workspace/install/setup.bash
sudo ip link set multicast on lo
sudo sysctl -w net.core.rmem_max=2147483647 >/dev/null
ros2 launch eagleye_rt eagleye_rt.launch.xml
```

問題なく動きました。ちなみにノード図はこんな感じ…すごい…

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/062d4584-0c64-93a1-72c7-5dcae1ef0da5.png)

## 比較

`Eagleye`自体は動くようになったので、既存ノード（`gyro_odometer` / `imu_gnss_poser`）の出力と比較していきます。具体的には、以下を比較します。
- `/eagleye/twist_with_covariance`↔`/localization/twist_estimator/twist_with_covariance`
- `eagleye/pose_with_covariance`↔`/localization/imu_gnss_poser/pose_with_covariance`

今回は実車bagファイルからGNSS / IMU / 車輪速のトピックを`Eagleye`へ流し込み、比較してみます。以下2ステップが必要になるので、追加実行します。

- 入出力トピックやパラメータの設定変更
- 一部ソースコードの変更

### 入出力トピックやパラメータの設定変更


まずは`/aichallenge/workspace/src/eagleye/eagleye_rt/config/eagleye_config.yaml`を以下のように変更します。主に入出力トピックの変更になります。

```diff_yaml:eagleye_config.yaml
/**:                                               #GNSS cycle 5Hz, IMU cycle 50Hz.
  ros__parameters:
    # Estimate mode
    use_gnss_mode: RTKLIB
    use_can_less_mode: false

    # Topic
    twist:
-     twist_type: 0 # TwistStamped : 0, TwistWithCovarianceStamped: 1
-     twist_topic: /can_twist
+     twist_type: 1 # TwistStamped : 0, TwistWithCovarianceStamped: 1
+     twist_topic: /sensing/vehicle_velocity_converter/twist_with_covariance
-   imu_topic: /imu/data_raw
+   imu_topic: /sensing/imu/imu_raw
    gnss:
-     velocity_source_type: 0 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3
-     velocity_source_topic: /rtklib_nav
-     llh_source_type: 0 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2
-     llh_source_topic:  /rtklib_nav
+     velocity_source_type: 3 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3
+     velocity_source_topic: /sensing/vehicle_velocity_converter/twist_with_covariance
+     llh_source_type: 2 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2
+     llh_source_topic:  /sensing/gnss/nav_sat_fix

    sub_gnss:
-     llh_source_type: 1 # nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2
-     llh_source_topic:  /sensing/sub_gnss/nmea_sentence
+     llh_source_type: 2 # nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2
+     llh_source_topic:  /sensing/gnss/nav_sat_fix

    # TF
    tf_gnss_frame:
      parent: "base_link"
      child: "gnss"

    # Origin of GNSS coordinates (ECEF to ENU)
    ecef_base_pos:
      x : 0.0
      y : 0.0
      z : 0.0
      use_ecef_base_position : false

    # Eagleye Navigation Parameters
    # Basic Navigation Functions
    common:
-     imu_rate: 50
-     gnss_rate: 5
+     imu_rate: 15
+     gnss_rate: 15
      stop_judgment_threshold: 0.01
      slow_judgment_threshold: 0.278
      moving_judgment_threshold: 2.78
...(以降変更なしなので省略)
```

`/aichallenge/workspace/src/eagleye/eagleye_util/geo_pose_converter/launch/geo_pose_converter.launch.xml`を以下のように変更します。こちらは`pose/position`の変換パラメータの変更になります。座標系番号を愛知から東京へ変更、MGRSを使用するように変更。

```diff_xml:geo_pose_converter.launch.xml
<?xml version="1.0" encoding="UTF-8"?>

<launch>
  <arg name="output_pose_with_cov_name" default="/localization/pose_estimator/pose_with_covariance"/>

  <node pkg="eagleye_geo_pose_converter" name="geo_pose_converter_node" exec="geo_pose_converter" output="screen" >
    <remap from="eagleye/pose_with_covariance" to="$(var output_pose_with_cov_name)"/>

    <!-- plane rectangular coordinate number -->
-   <param name="plane" value="7"/>
+   <param name="plane" value="9"/>
    <!-- 1 : plane rectangular coordinate  2 : MGRS -->
-   <param name="tf_num" value="1"/>
+   <param name="tf_num" value="2"/>
    <!-- 0 : No convert  1 : ellipsoid -> altitude  2 : altitude -> ellipsoid -->
    <param name="convert_height_num" value="0"/>
    <!-- 0 : EGM2008-1  1 : GSIGEO2011 Ver2.1 -->
    <param name="geoid_type" value="0"/>

    <param name="parent_frame_id" value="map"/>
    <param name="child_frame_id" value="eagleye_base_link"/>
    <param name="base_link_frame_id" value="base_link"/>
    <param name="gnss_frame_id" value="gnss_link"/>
  </node>

</launch>

```


### 一部ソースコードの変更

MGRSコードを追加します。`/aichallenge/workspace/src/eagleye/eagleye_util/geo_pose_converter/src/geo_pose_converter.cpp`の197行目辺りを以下のように変更します。

https://github.com/MapIV/eagleye/blob/main-ros2/eagleye_util/geo_pose_converter/src/geo_pose_converter.cpp#L197

```diff_cpp:geo_pose_converter.cpp
...
  if (tf_num == 1)
  {
    _llh_param.use_mgrs = false;
    _llh_param.plane_num = plane;  
  }
  else if (tf_num == 2)
  {
    _llh_param.use_mgrs = true;
+   _llh_param.mgrs_code = "54SUE896431";
  }
  else
...
```

※ `54SUE896431`は、`lanelet2_map.osm`から引っ張ってきています。

https://github.com/AutomotiveAIChallenge/aichallenge-2024/commit/5e78954d97e56eac076e71cca219d9c61ccf32ed

ソースコードを変更したので、再ビルドしておきます。

```bash:terminal 1
cd /aichallenge/workspace/src/
./build_autoware.bash
```

### 再実行

やっと準備が整いました、ターミナルを4つ立ち上げて以下をそれぞれ実行します。

```bash:terminal 1
source /aichallenge/workspace/install/setup.bash
sudo ip link set multicast on lo
sudo sysctl -w net.core.rmem_max=2147483647 >/dev/null
ros2 launch eagleye_rt eagleye_rt.launch.xml
```

```bash:terminal 2
source /aichallenge/workspace/install/setup.bash
sudo ip link set multicast on lo
sudo sysctl -w net.core.rmem_max=2147483647 >/dev/null
ros2 launch eagleye_geo_pose_fusion geo_pose_fusion.launch.xml 
```

```bash:terminal 3
source /aichallenge/workspace/install/setup.bash
sudo ip link set multicast on lo
sudo sysctl -w net.core.rmem_max=2147483647 >/dev/null
ros2 launch eagleye_geo_pose_converter geo_pose_converter.launch.xml output_pose_with_cov_name:=/eagleye/pose_with_covariance
```

※ デフォルトだと出力トピックが`/localization/pose_estimator/pose_with_covariance`になっているので変更しています。

https://github.com/MapIV/eagleye/blob/main-ros2/eagleye_util/geo_pose_converter/launch/geo_pose_converter.launch.xml#L4

```bash:terminal 4
source /aichallenge/workspace/install/setup.bash
sudo ip link set multicast on lo
sudo sysctl -w net.core.rmem_max=2147483647 >/dev/null
ros2 bag play YOUR_ROSBAG_FILE.db3 --clock
```

`terminal 1`に以下のような表示が出ます。`Eagleye`の出力モニターのようです。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/7a25967b-8eac-575c-bfcc-1554c1ba6236.png)

`terminal 1`の出力モニターにおける`status enable`が徐々に`True`に変化していきます（~1,2分）。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/b39009dd-bc82-087f-0ceb-18d55b2c457a.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/a4178e5e-6d3a-1062-3239-ab2f9285fbb5.png)

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/007c22a4-df3d-f4d0-f769-623ad1451174.png)

※ 初期化手順に関する情報が以下にあります、5秒静止→ 30秒直進です。

https://github.com/AutomotiveAIChallenge/autoware-documentation/blob/main/docs/how-to-guides/integrating-autoware/launch-autoware/localization-methods/eagleye-guide.md#notes-on-initialization


※ `Eagleye`の出力トピック一覧は以下でした。

```
/eagleye/acc_x_offset
/eagleye/acc_x_scale_factor
/eagleye/angular_velocity_offset_stop
/eagleye/distance
/eagleye/eagleye/fix
/eagleye/eagleye/geo_pose_with_covariance
/eagleye/eagleye/heading_interpolate_3rd
/eagleye/eagleye/pitching
/eagleye/eagleye/pose
/eagleye/eagleye/rolling
/eagleye/enu_absolute_pos
/eagleye/enu_absolute_pos_interpolate
/eagleye/enu_relative_pos
/eagleye/enu_vel
/eagleye/fix
/eagleye/geo_pose_with_covariance
/eagleye/gnss/fix
/eagleye/gnss/gga
/eagleye/gnss/rmc
/eagleye/gnss/rtklib_nav
/eagleye/gnss_compass_pose
/eagleye/gnss_smooth_pos_enu
/eagleye/heading_1st
/eagleye/heading_2nd
/eagleye/heading_3rd
/eagleye/heading_interpolate_1st
/eagleye/heading_interpolate_2nd
/eagleye/heading_interpolate_3rd
/eagleye/height
/eagleye/imu/data_corrected
/eagleye/imu/data_tf_converted
/eagleye/navsat/reliability_gga
/eagleye/pitching
/eagleye/pose
/eagleye/rolling
/eagleye/rtklib/fix
/eagleye/slip_angle
/eagleye/twist
/eagleye/twist_with_covariance
/eagleye/vehicle/twist
/eagleye/velocity
/eagleye/velocity_scale_factor
/eagleye/velocity_status
/eagleye/yaw_rate_offset_1st
/eagleye/yaw_rate_offset_2nd
/eagleye/yaw_rate_offset_stop
```


### 出力の比較

`Eagleye`出力と既存ノード（`gyro_odometer`, `imu_gnss_poser`）出力を比較します。

#### `gyro_odometer` v.s. `Eagleye`（Twistの比較）
`gyro_odometer`に比べて`Eagleye`の出力は、angularは完全一致しますが、linearに遅延が見られます。この結果を見ると、特段`Eagleye`を使うと嬉しいといったことは無さそうです。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/eb3be693-052f-60b4-2339-790d32fd9625.png)

#### `imu_gnss_poser` v.s. `Eagleye`（Poseの比較）
`Eagleye`の変換設定が不適切なようで、以下のように大きく外れた値が出力されてしまい、比較になりませんでした。変換設定の見直しが必要です。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/8df25bc9-9e09-e6b7-ff69-f7880a462ba8.png)

※ 動画だとこんな感じです。

![ezgif-1-8e94190436.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3628135/faf5941c-f7da-9db3-372e-009212af87ed.gif)


# おわりに

`Eagleye`は、ROS 2(Humble)でも容易にインストールできました！

ただ設定が探り探りになってしまいました、これは筆者の知識不足が原因です。
とりあえずエイヤで使ってみたところ、
`Twist`は、`gyro_odometer`とほぼ同じ出力となりましたが、遅延が確認されました。
`Pose`は、`imu_gnss_poser`に比べて、明らかに不適切な値を出力しており、キャリブレーション手順の厳密な遵守や、変換設定の見直しが必要そうです。
時間があるときに、以下などを参照して見直そうと思います。

https://qiita.com/toki-1441/items/615d781e3a20edf22cda


中途半端な記事で申し訳ありませんが、来年度以降の自動運転AIチャレンジ参加者の何かしらのきっかけになれば幸いです。

この記事は、AI文章校正ツール「ちゅらいと」で校正されています。