Zenn
岐阜大学アレックス研究室Publicationへの投稿
🦔

AutowareでWHILL(電動車椅子)を自動運転

hayato
2024/11/25に公開
Autoware
whill
tech

岐阜大学アレックス研究室です。
今回は、自動運転ソフトウェア「Autoware」を使ってWHILLを自律走行させるまでの取り組みをご紹介します。同じようなプロジェクトに挑戦している方々に、少しでもヒントや役立つ情報をお届けできたら嬉しいです。

環境

  1. 車両
    • WHILL Model C2
  2. センサー
    • LiDAR : Velodyne VLP-16
    • IMU : MTi600-series Development Kit
  3. コンピュータ構成
    • CPU : Intel Core i7-13700HX
    • GPU : NVIDIA GeForce RTX 4060 Laptop GPU
    • メモリ : 16GB
    • Linux OS : Ubuntu 22.04
    • ROS2 : humble

1. センサーの取り付け

LiDAR

LiDARを車体に取り付けるために、部品をCADを使用して設計・作成しました。

作成した3Dオブジェクト

  • フレームとLiDARを固定するL字ブラケット

  • LiDARのマウント台

IMU

振動によるズレを防ぐため、リンクゲルを使用して固定しました。

全体像

最終的な取り付けの全体像は以下の通りです。

2. 地図作成

地図の作成には、LIO-SAMを使用しました。
自己位置推定が外れて地図作成がうまくいかない場合には、以下のような対策を行いました：

  • 走行環境やセンサーの状態に応じてパラメータを調整
  • センサーデータが安定するように速度を落として走行
パラメータ
/**:
  ros__parameters:
    # Topics
    pointCloudTopic: "/velodyne_points"                   # Point cloud data
    imuTopic: "/imu/data"                        # IMU data
    odomTopic: "odometry/imu"                    # IMU pre-preintegration odometry, same frequency as IMU
    gpsTopic: "odometry/gpsz"                    # GPS odometry topic from navsat, see module_navsat.launch file
    
    # Frames
    lidarFrame: "velodyne"
    baselinkFrame: "base_link"
    odometryFrame: "odom"
    mapFrame: "map"
    
    # GPS Settings
    useImuHeadingInitialization: false           # if using GPS data, set to "true"
    useGpsElevation: false                       # if GPS elevation is bad, set to "false"
    gpsCovThreshold: 2.0                         # m^2, threshold for using GPS data
    poseCovThreshold: 25.0                       # m^2, threshold for using GPS data
    
    # Export settings
    savePCD: false                               # https://github.com/TixiaoShan/LIO-SAM/issues/3
    savePCDDirectory: "/Downloads/LOAM/"         # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
    
    # Sensor Settings
    sensor: velodyne                               # lidar sensor type, either 'velodyne', 'ouster' or 'livox'
    N_SCAN: 16                                   # number of lidar channels (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
    Horizon_SCAN: 1800                            # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
    downsampleRate: 1                          # default: 1. Downsample your data if too many
    
    # points. i.e., 16 = 64 / 4, 16 = 16 / 1
    lidarMinRange: 0.5                      # default: 1.0, minimum lidar range to be used
    lidarMaxRange: 150.0                        # default: 1000.0, maximum lidar range to be used
    
    # IMU Settings
    imuAccNoise: 3.9939570888238808e-03
    imuGyrNoise: 1.5636343949698187e-03
    imuAccBiasN: 6.4356659353532566e-05
    imuGyrBiasN: 3.5640318696367613e-05
    imuGravity: 9.80511
    imuRPYWeight: 0.01
    extrinsicTrans:  [ 0.0,  0.0,  0.0 ]
    extrinsicRot:    [ 1.0,  0.0,  0.0,
                       0.0,  1.0,  0.0,
                       0.0,  0.0,  1.0 ]
    extrinsicRPY: [ 1.0,  0.0,  0.0,
                    0.0,  1.0,  0.0,
                    0.0,  0.0,  1.0 ]
                    
    # LOAM feature threshold
    edgeThreshold: 1.0
    surfThreshold: 0.1
    edgeFeatureMinValidNum: 10
    surfFeatureMinValidNum: 100
    
    # voxel filter paprams
    odometrySurfLeafSize: 0.08                     # default: 0.4 - outdoor, 0.2 - indoor
    mappingCornerLeafSize: 0.08                    # default: 0.2 - outdoor, 0.1 - indoor
    mappingSurfLeafSize: 0.08                      # default: 0.4 - outdoor, 0.2 - indoor
    
    # robot motion constraint (in case you are using a 2D robot)
    z_tollerance: 1000.0                          # meters
    rotation_tollerance: 1000.0                   # radians
    
    # CPU Params
    numberOfCores: 4                              # number of cores for mapping optimization
    mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
    
    # Surrounding map
    surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
    surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
    surroundingKeyframeDensity: 1.0               # meters, downsample surrounding keyframe poses
    surroundingKeyframeSearchRadius: 10.0         # meters, within n meters scan-to-map optimization
    
    # (when loop closure disabled)
    # Loop closure
    loopClosureEnableFlag: true
    loopClosureFrequency: 3.0                     # Hz, regulate loop closure constraint add frequency
    surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
    historyKeyframeSearchRadius: 10.0             # meters, key frame that is within n meters from
    
    # current pose will be considerd for loop closure
    historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be
    
    # considered for loop closure
    historyKeyframeSearchNum: 25                  # number of hostory key frames will be fused into a 
    
    # submap for loop closure
    historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment
    
    # Visualization
    globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
    globalMapVisualizationPoseDensity: 10.0       # meters, global map visualization keyframe density
    globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density

Laneletデータの作成

地図生成後、Vector Map Builder を使用して Lanelet データを作成しました。

作成方法については、以下の公式ガイドを参考にしました：
https://docs.web.auto/user-manuals/vector-map-builder/how-to-use/edit-maps

作成後

作成した点群データとLaneletデータは、それぞれ以下の名前で同じディレクトリに配置してください：

  • pointcloud_map.pcd
  • lanelet2_map.osm

3. リポジトリの準備

Autowareの更新によって、作成したインターフェースが正常に動作しなくなる場合があります。そこで、リポジトリをForkし、安定した環境で作業を進めることにしました。

リポジトリのFork方法については、以下の公式ドキュメントを参照してください：
https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-your-autoware-repositories/creating-autoware-repositories/

4. whill_sensor_kit_launch

sample_sensor_kit_launch をforkし、whill_sensor_kit_launch を作成しました。

whill_sensor_kit_description

whill_sensor_kit_descriptionでは、configフォルダとurdfフォルダのファイルを修正し、センサーの位置を設定します。この設定を基に適切なTF（座標変換情報）が発行され、センサーの位置や姿勢が他のノードと連携して利用されます。

whill_sensor_kit_launch

whill_sensor_kit_launchは、センサードライバーを起動するためのLaunchファイルです。デフォルトで用意されていないドライバーが必要な場合は、sensor_component/external にリポジトリを追加して対応します。

5. whill_vehicle_launch

sample_vehicle_launchをforkし、whill_vehicle_launchを作成しました。

whill_vehicle_description

whill_vehicle_descriptionでは、vehicle_info.param.yamlとmirror.param.yamlを修正し、車両の情報を設定します。
WHILLはオムニホイールを搭載しており、その場で360度回転が可能ですが、今回は簡略化のため最大ステアリング角度を0.5[rad]（約28.65°）に設定しています。

参考リンク：
https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/vehicle-dimensions/
https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-vehicle-and-sensor-model/creating-vehicle-model/#mirrorparamyaml

whill_vehicle_launch

whill_vehicle_launchは、車両の速度やステアリング情報をAutowareに送信し、Autowareから受け取ったcontrol_cmdを基にWHILLを制御するためのLaunchファイルです。

車両インターフェースについては、以下の公式ドキュメントを参照してください：
https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-vehicle-interface-package/creating-vehicle-interface/

AutowareからのデータをSub

Autowareからのデータは、/control/command/control_cmdで受け取ります。データの形式はControl.msgです。

WHILLへの入力は、Joy.msg形式で行います。このメッセージのaxesの0番目がステアリング角度、1番目が速度を表します。これらの値は範囲が-1から1の間で、Autowareから受け取ったデータをWHILLが適切に扱える範囲に変換する必要があります。

具体的には、WHILLに「1」の入力を与えたときの走行結果を基に、5メートル走行するのにかかる時間を計測しました。その結果、以下のような所要時間が得られました：

  • 2.91秒
  • 2.95秒
  • 2.98秒
  • 3.01秒
  • 2.94秒

これらのデータを元に、WHILLのmax_speedを1.666 m/s（約6km/h）に設定しました。ステアリングについても同様の方法で調整を行います。

コード
void WhillController::autoware_callback(const Control::SharedPtr msg)
{
  if (manual_flag_) {
    return;
  }

  float speed = msg->longitudinal.velocity;
  float angle = msg->lateral.steering_tire_angle;

  if (speed < min_speed_) {
      speed = min_speed_;
  } else if (speed > max_speed_) {
      speed = max_speed_;
  }

  if (angle < min_angle_) {
      angle = min_angle_;
  } else if (angle > max_angle_) {
      angle = max_angle_;
  }

  float normalized_speed = speed / max_speed_;
  float normalized_angle = angle * normalizer_;

  sensor_msgs::msg::Joy pub_msg;
  pub_msg.header.stamp = this->now();
  pub_msg.header.frame_id = "joy"; 

  pub_msg.axes.resize(2);

  pub_msg.axes[1] = normalized_speed;
  pub_msg.axes[0] = normalized_angle;

  joy_pub_->publish(pub_msg);
}

AutowareへデータをPub

WHILLから取得できるデータは、ModelCr2State.msg形式で提供されます。
WHILLは差動駆動型車両であるため、車両の速度とステアリング角度は以下のサイトを参照して算出しています。

https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-vehicle-interface-package/customizing-for-differential-drive-model/

https://memo.soarcloud.com/2輪差動駆動型ロボット2wdの運動学モデル/

https://taketake2.com/S19.html?utm_source=chatgpt.com

6. Build Autoware

Autowareの公式インストールガイドに従ってビルドを進めてください。
ただし、Autowareをクローンする際には、以下のコマンドを使用してcustom_autowareリポジトリのwhillブランチをクローンしてください。

git clone https://github.com/peregrine-884/custom_autoware.git -b whill

なお、フォークしていない部分に変更が加わるとエラーが発生する可能性があります。そのため、ビルドがうまくいかない場合は、自分の環境で正常に動作しているコードを以下のリンクから提供していますので、こちらを利用してください。
https://drive.google.com/file/d/1xF0ZD8gu3KDKUk-TShiNX6rgv_lf9ULM/view?usp=sharing

7. Run Autoware

以下のコマンドでAutowareを起動します。

ros2 launch autoware_launch autoware.launch.xml vehicle_model:=whill_vehicle sensor_model:=whill_sensor_kit map_path:=<absolute map path>

正しく動作しないとき

ターミナルでログを確認したり、以下のコマンドを使用してエラー箇所を特定できます。

ros2 run rqt_runtime_monitor rqt_runtime_monitor

さらに、Node diagramを参照し、各ノードが正常に動作しているか、トピックが正しく出力されているかを確認することも問題解決に役立ちます。

Autoボタンが押せない

autoware.universe/system/system_diagnostic_monitor/config/autoware-main.yaml に記載されている条件を確認することで、問題の原因を特定できます。設定ファイル内には、Autoボタンが有効になるための条件が詳細に記載されているので、ここで確認して解決策を見つけましょう。

autoware-main.yaml
files:
  - { path: $(find-pkg-share system_diagnostic_monitor)/config/map.yaml }
  - { path: $(find-pkg-share system_diagnostic_monitor)/config/localization.yaml }
  - { path: $(find-pkg-share system_diagnostic_monitor)/config/planning.yaml }
  - { path: $(find-pkg-share system_diagnostic_monitor)/config/perception.yaml }
  - { path: $(find-pkg-share system_diagnostic_monitor)/config/control.yaml }
  - { path: $(find-pkg-share system_diagnostic_monitor)/config/vehicle.yaml }
  - { path: $(find-pkg-share system_diagnostic_monitor)/config/system.yaml }
units:
  - path: /autoware/modes/stop
    type: ok
  - path: /autoware/modes/autonomous
    type: and
    list:
      - { type: link, link: /autoware/map }
      - { type: link, link: /autoware/localization }
      - { type: link, link: /autoware/planning }
      - { type: link, link: /autoware/perception }
      - { type: link, link: /autoware/control }
      - { type: link, link: /autoware/vehicle }
      - { type: link, link: /autoware/system }
  - path: /autoware/modes/local
    type: and
    list:
      - { type: link, link: /autoware/vehicle }
      - { type: link, link: /autoware/system }
      - { type: link, link: /autoware/control/local }
  - path: /autoware/modes/remote
    type: and
    list:
      - { type: link, link: /autoware/vehicle }
      - { type: link, link: /autoware/system }
      - { type: link, link: /autoware/control/remote }
  - path: /autoware/modes/emergency_stop
    type: and
    list:
      - { type: link, link: /autoware/vehicle }
      - { type: link, link: /autoware/system }
  - path: /autoware/modes/comfortable_stop
    type: and
    list:
      - { type: link, link: /autoware/map }
      - { type: link, link: /autoware/localization }
      - { type: link, link: /autoware/planning }
      - { type: link, link: /autoware/perception }
      - { type: link, link: /autoware/control }
      - { type: link, link: /autoware/vehicle }
      - { type: link, link: /autoware/system }
  - path: /autoware/modes/pull_over
    type: and
    list:
      - { type: link, link: /autoware/map }
      - { type: link, link: /autoware/localization }
      - { type: link, link: /autoware/planning }
      - { type: link, link: /autoware/perception }
      - { type: link, link: /autoware/control }
      - { type: link, link: /autoware/vehicle }
      - { type: link, link: /autoware/system }
  - path: /autoware/debug/tools
    type: and
    list:
      - { type: link, link: /autoware/system/service_log_checker }

さいごに

Autowareをゼロから起動してみましたが、予想以上に大変でした。
エラーの原因を特定するのに時間がかかりましたが、1つ1つ確認しながら進めることが近道だと感じました。
作成したカスタムAutowareのGitHubはこちらからアクセスできます。

岐阜大学アレックス研究室
岐阜大学アレックス研究室Publication

岐阜大学工学部電気電子・情報工学科(情報コース)　 自動運転の研究しています。

Discussion