岐阜大学アレックス研究室です。
前回は電動車椅子の自動運転に挑戦しましたが、今回はシミュレータ上の車両をAutowareで動かすことに挑戦しました!
環境
BeamNG実行PC
- CPU: Intel Core i5-13400F
- GPU: NVIDIA GeForce RTX 3060 Ti
- メモリ: 16GB
- OS: Windows 11
Autoware実行PC
- CPU: Intel Core i7-13700HX
- GPU: NVIDIA GeForce RTX 4060 Laptop GPU
- メモリ: 16GB
- OS: Ubuntu 22.04
- ROS2: Humble
BeamNGとは？
BeamNGは、リアルな車両シミュレーションを可能にする物理エンジンを搭載したソフトウェアです。特に、リアルな衝突や車両の挙動をシミュレートできる点が特徴で、ゲーム用途から研究開発まで幅広く活用されています。
左：車両損傷の様子 右：筑波山の雪道を走行する車両
BeamNGには、大きく分けてBeamNG.driveとBeamNG.techの2つのバージョンがあります。
- BeamNG.drive：ゲーム向けのシミュレーター。リアルな車両挙動や衝突を再現し、自由な運転が楽しめる。
- BeamNG.tech：研究開発向けのバージョン。自動運転やADASのテスト環境を提供し、カメラ・LiDARなどのセンサーやPython API（BeamNGpy）を活用できる。
今回は、このBeamNG.techを使用してAutowareとの連携に挑戦しました。
BeamNG.techの申請
BeamNG.techの学術ライセンスは、研究機関向けに最大1年間無料で提供されています。申請はこちらから行えます。
BeamNGのデータ取得
BeamNG.techのversion 0.32では、Linuxでの動作がサポートされていませんでした。そのため、BeamNG.techをWindowsで動作させ、AutowareをLinuxで実行するという構成を採用しました。
公式のbeamng-ros-integrationを使用することで、Windowsで動作するBeamNGのデータをLinux側へROS2トピックとして配信できます。これにより、Autowareとの連携が可能になります。
しかし、公式のROS2ブリッジではセンサーデータの取得頻度が低いという問題がありました。例えば、LiDARの取得頻度は2〜5Hzと低く、リアルタイムな自動運転シミュレーションには不十分でした。
公式ROS2ブリッジを使用したデータ取得
Zenohを用いたデータ転送の最適化
Zenohという通信プロトコルを導入することで、この問題を解決しました。
Zenohは、データの移動、保存、計算を効率的に統一するプロトコルで、リアルタイムでのデータ転送に優れた性能を発揮します。この特性を活かし、zenoh_beamng_bridgeを開発しました。
その結果、LiDARの取得頻度を10Hz以上に向上させることができました。
Zenohを活用したデータ取得
zenoh_beamng_bridge
Zenohを活用したデータ転送の最適化において、BeamNGからのデータ取得はPythonで実装されていました。そのため、当初はZenohの処理もPythonで記述しましたが、データのシリアライズに時間がかかり、転送効率が向上しないという課題が発生しました。
そこで、Zenohの処理をRustで実装し、PyO3を用いてPythonから利用できるようにすることで、シリアライズの高速化とデータ転送のボトルネック解消を実現しました。
Zenohを活用したデータ取得の構成
この最適化により、データ処理速度が大幅に改善されました。以下の図は、PythonとRustでのデータ処理時間を比較したものです。
データの処理時間(左：Python 右：Rust)
Autoware
Autowareのセットアップ方法については、前回の記事で詳しく解説していますので、そちらをご参照ください。
BeamNG + Autoware
BeamNG上でAutowareを動作させるために、いくつかの工夫を行いました。
LiDARデータの処理
LiDARデータは、CropBoxフィルタや歪み補正、ノイズ除去などの前処理を施した後、最終的に/sensing/lidar/concatenated/pointcloudとして生成されます。しかし、この前処理にはIntensity(測定した点の反射率を示す値)などの情報が必要ですが、BeamNGから取得できるのはXYZ座標のみで、必要なデータを得ることができませんでした。
しかし、BeamNGのLiDARデータはノイズが少なく精度が高いため、取得データのフレームをbase_linkフレームに変換し、前処理を省略することで対応しました。
base_linkフレームへの変換コード
import numpy as np
from scipy.spatial.transform import Rotation as R
import time
from shared import *
intensity = 128
downsample_rate = 9
offset = np.array([0.3302292, 1.653519373, 0.12556159596657])
def send_lidar_data(lidar):
data_publisher_instance = DataPublisherSingleton()
vehicle_instance = VehicleSingleton()
stop_event_instance = StopEventSingleton()
lidar_hz = 10
lidar_interval = 1.0 / lidar_hz
base_time = time.time()
while True:
if stop_event_instance.get_value():
break
data = lidar.poll()
pointcloud = data['pointCloud'][::downsample_rate]
num_points = pointcloud.shape[0]
if pointcloud is not None and len(pointcloud) > 0:
state = vehicle_instance.get_state()
if state is None:
continue
position = state["pos"]
relative_pointcloud = np.array(pointcloud - position + offset, dtype=np.float32)
rotation = state["rotation"]
r = R.from_quat(rotation)
yaw = r.as_euler('xyz', degrees=True)[2] + 90
pitch = r.as_euler('xyz', degrees=True)[1]
roll = r.as_euler('xyz', degrees=True)[0]
new_rotation = R.from_euler('xyz', [roll, pitch, yaw], degrees=True)
rotation_matrix = new_rotation.as_matrix()
rotated_pointcloud = np.dot(relative_pointcloud, rotation_matrix.T)
new_column_intensity = np.full((num_points, 1), intensity)
pointcloud_4d = np.concatenate([rotated_pointcloud, new_column_intensity], axis=1).astype(np.float32)
data_publisher_instance.lidar(pointcloud_4d, "base_link")
next_time = max(0, lidar_interval - (time.time() - base_time))
time.sleep(next_time)
base_time = time.time()
走行経路の生成
Laneletを用いて車両の走行経路を定義していますが、LiDARデータにIntensity情報が含まれていないため、白線の位置を特定できず、正確なLaneletを作成するのが困難でした。
そこで、車両の走行位置を記録し、Trajectoryを生成するノードを開発。このTrajectoryを基に走行経路を定義することで、車線に沿った適切なLaneletを作成できるようになりました。
Trajectoryを基に作成したLanelet
Trajectoryを生成するノード
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <fstream>
class CreateTrajectory : public rclcpp::Node
{
public:
CreateTrajectory() : Node("create_trajectory_node")
{
declare_parameter("csv_pth", "default.csv");
csv_pth_ = this->get_parameter("csv_pth").as_string();
RCLCPP_INFO(this->get_logger(), "CSV file path: %s", csv_pth_.c_str());
std::ofstream csv_file(csv_pth_, std::ios_base::trunc);
csv_file << "x,y,z,yaw,velocity,change_flag\n";
csv_file.close();
localization_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
"/localization/kinematic_state", 10,
std::bind(&CreateTrajectory::localization_callback, this, std::placeholders::_1)
);
counter_ = 0;
}
private:
void localization_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
{
// CSV形式の出力ファイル名
std::ofstream csv_file;
csv_file.open(csv_pth_, std::ios_base::app); // 追記モード
if (counter_ % 10 == 0) {
if (csv_file.is_open())
{
// オドメトリデータからx, y, zを取得
double x = msg->pose.pose.position.x;
double y = msg->pose.pose.position.y;
double z = msg->pose.pose.position.z;
// yaw, velocity, change_flagを0に設定
double yaw = 0.0;
double velocity = 0.0;
int change_flag = 0;
// CSV形式でデータを追加
csv_file << x << "," << y << "," << z << "," << yaw << "," << velocity << "," << change_flag << "\n";
csv_file.close();
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to open CSV file for writing.");
}
}
counter_++;
}
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr localization_sub_;
std::string csv_pth_;
bool header_written_;
int counter_;
};
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<CreateTrajectory>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
制御コマンドの変換
Autowareの出力であるAckermannControlCommandを、BeamNGの車両制御に適したActuationCommandへ変換するために、autoware_accel_brake_map_calibratorとautoware_raw_vehicle_cmd_converterを活用しました。
autoware_accel_brake_map_calibratorは、アクセルやブレーキ入力と車両の加減速特性を記録し、accel_map.csvおよびbrake_map.csvを生成するノードです。
autoware_raw_vehicle_cmd_converterは、生成されたマップを参照し、AckermannControlCommandの加速度情報を適切なアクセルおよびブレーキ制御コマンドへ変換します。
accel_mapとbrake_mapを生成する様子
Run Autoware
BeamNG上に首都高速道路MODを導入し、Autowareによる走行を実施しました。
MODの導入方法については、こちらを参考にしてください。
最後に
今回、BeamNG上でAutowareの動作に挑戦し、多くの知見を得ることができました。シミュレーターならではの課題もありましたが、実車を使わずに試行錯誤できる点は大きなメリットだと実感しました。
Discussion