AIチャレンジ2025活動記 第6弾です！
予選も残りおよそ1週間に迫ってきましたね。
ところで、8月に入り2度に渡ってアップデートされたAWSIMはもう確認しましたか？？とくに2度目の更新ではピットスタート方式になったため、1周目と2周目以降での振る舞いが大幅に変わりましたね。
そこで今回は1周目と2周目以降でTrajectoryを変えてみる！をテーマにお届けします！
情報の信頼性
GNSS情報にはノイズが含まれていて不安定だったり、同じTrajectoryやParameterで走行していても時々壁に衝突するパターンがあったり、不確定要素に悩まされている方も多いのではないでしょうか？
パラメータを動的に変更するとしてもそのタイミングをどうするか？は1つの重要ポイントです。ノイズや誤差が含まれるような情報をトリガーとしてしまうと、変更時の挙動がおかしくなりかねません。
参照できる情報は走行中にros2 topic listを叩けば一覧で確認できます。
(AIC_DEV) user@PC:/aichallenge$ ros2 topic list
/aichallenge/awsim/change_time_scale
/aichallenge/awsim/reset
/aichallenge/awsim/status
/autoware_orientation
/awsim/control_cmd
/awsim/control_mode_request_topic
/clock
/control/command/actuation_cmd
/control/command/control_cmd
/control/command/emergency_cmd
/control/command/gear_cmd
/control/command/hazard_lights_cmd
/control/command/turn_indicators_cmd
/control/debug/lookahead_point
/diagnostics
/initialpose
/joint_states
/localization/acceleration
/localization/biased_pose
/localization/biased_pose_with_covariance
/localization/debug
・
・
・
（つづく）
沢山ありすぎて目が回ってしまうかもしれませんが、実はこの一覧をTopicがどこから出ているか？で分類することができるのです！
たしかな情報を使用する
マニュアル通りの実行方法run_evaluation.bashの中身を覗いてみるとrun_simulator.bashとrun_autoware.bashの起動コマンドが実行されていることがわかります。つまり、ターミナルでrun_simulator.bashでAWSIMのみを、run_autoware.bashでAutowareのみを起動することができるということです。
どちらか片方のみを起動した状態でtopic listを見ると、各Topicがどちらから出ているかを知ることができます。AutowareからのTopic情報はセンサーデータや推測値等の”疑いのある”データです。一方、AWSIMから直接得られるTopic情報はラップタイムや周回数のようなコース/レースに関する情報の真値です。
例えば、記録となるタイムはAWSIM側で計測されていたり、35km/hのリミッターもAWSIM側での速度情報を基準に判定されていたりします。
つまり、AWSIM側からの情報をもとに制御を操作するのが安心です！！
周回数の情報はAWSIMからのTopic/aichallenge/awsim/statusにて取得できます。これを使用すると、周回ごとに参照するcsvファイルを変更させる処理を実装できるのです。
複数のcsvファイルを入力する
切り替えタイミングを知る方法がわかったら、次はいよいよ複数のcsvファイルを入力して途中で変更させましょう。
Trajectoryに関する処理はsimple_trajectory_generatorパッケージで行われます。csvファイルから経由地を読み込み、Trajectory形式に変換してメッセージとして配信します。
まずsimple_trajectory_generator.cppをTopicのSubsclibeと複数のcsvファイルパス入力に対応させる修正をします。デフォルトでは1つのcsvファイルをcsv_pathとして扱いますが、これを配列csv_pathsに複数のファイルを格納して使用する方式に変更します。
また、Trajectoryの切り替わりによる挙動の影響を受けないよう、ピットアウトゾーンやスタートラインから離れた第7セクションに入ったタイミングで切り替わるようにしましょう。
+#include <std_msgs/msg/string.hpp>
+#include "std_msgs/msg/float32_multi_array.hpp"
class CSVToTrajectory : public rclcpp::Node
{
public:
CSVToTrajectory() : Node("csv_to_trajectory_node"), switched_to_second_path_(false)
{
+ declare_parameter<std::vector<std::string>>("csv_paths", std::vector<std::string>{});
- if (csv_path.empty()) {
- RCLCPP_ERROR(get_logger(), "CSV path is not specified");
- return;
- }
- if (!loadCSVTrajectory(csv_path)) {
- RCLCPP_ERROR(get_logger(), "Failed to load CSV file: %s", csv_path.c_str());
- return;
- }
+ auto csv_paths = get_parameter("csv_paths").as_string_array();
+ if (csv_paths.size() != 2) {
+ RCLCPP_ERROR(get_logger(), "csv_paths must contain exactly 2 paths.");
+ return;
+ }
+ csv_paths_ = csv_paths;
+ current_csv_index_ = 0;
+ if (!loadCSVTrajectory(csv_paths_[current_csv_index_])) {
+ RCLCPP_ERROR(get_logger(), "Failed to load initial CSV file: %s", csv_paths_[current_csv_index_].c_str());
+ return;
+ }
- RCLCPP_INFO(get_logger(), "Loaded trajectory from CSV with %zu points", csv_trajectory_.points.size());
+ RCLCPP_INFO(get_logger(), "Loaded initial trajectory from: %s", csv_paths_[current_csv_index_].c_str());
+ sub_status_ = create_subscription<std_msgs::msg::Float32MultiArray>(
+ "/aichallenge/awsim/status", rclcpp::QoS{1}.best_effort(),
+ std::bind(&CSVToTrajectory::statusCallback, this, std::placeholders::_1));
}
private:
rcl_interfaces::msg::SetParametersResult on_parameter_event(
const std::vector<rclcpp::Parameter> & parameters)
{
for (const auto & param : parameters) {
if (param.get_name() == "csv_path") {
if (param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) {
- RCLCPP_INFO(get_logger(), "csv_path parameter changed from '%s' to '%s'",
- current_csv_path_.c_str(), new_csv_path.c_str());
if (loadCSVTrajectory(new_csv_path)) {
- RCLCPP_INFO(get_logger(), "Successfully loaded new trajectory from CSV: %s with %zu points",
- current_csv_path_.c_str(), csv_trajectory_.points.size());
}
}
}
}
+ void statusCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
+ {
+ if (switched_to_second_path_) return;
+
+ if (msg->data.size() < 4) return;
+ const int lap = static_cast<int>(msg->data[1]);
+ const int section = static_cast<int>(msg->data[3]);
+
+ switch (lap) {
+ case 1:
+ switch (section) {
+ case 7:
+ current_csv_index_ = 1;
+ if (loadCSVTrajectory(csv_paths_[current_csv_index_])) {
+ switched_to_second_path_ = true;
+ }
+ break;
+ default:
+ break;
+ }
+ break;
+ default:
+ break;
+ }
+ }
+ rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr sub_status_;
+ std::vector<std::string> csv_paths_;
+ int current_csv_index_;
+ bool switched_to_second_path_;
参照ファイルパスはreference.launch.xmlのplanningセクションにて指定します。デフォルトでは1つのcsvファイルのみ指定されていますが、ここに2つのファイルのパスを記述しておきましょう。
<!-- Planning -->
<group>
<push-ros-namespace namespace="planning"/>
<!-- scenario_planning -->
<group>
<push-ros-namespace namespace="scenario_planning"/>
<!-- Customizable -->
<node pkg="simple_trajectory_generator" exec="simple_trajectory_generator_node" name="simple_trajectory_generator" output="screen">
- <param name="csv_path" value="$(find-pkg-share simple_trajectory_generator)/data/file.csv"/>
+ <param name="csv_path" value="$(find-pkg-share simple_trajectory_generator)/data/file1.csv"/>
+ <param name="csv_paths" value="[$(find-pkg-share simple_trajectory_generator)/data/file1.csv, $(find-pkg-share simple_trajectory_generator)/data/file2.csv]"/>
<param name="z" value="6.5"/>
</node>
</group>
</group>
実行
前項の修正が済んだらTrajectory Editorで1周目と2周目以降のそれぞれのcsvファイルを作成し、reference.launch.xmlでパスを指定してから実行してみましょう。
第7セクションに入るタイミングで緑色のTrajectoryが切り替わるようになりました！
これで2周目以降にピットアウトゾーンの影響を受けずに走行させることもできそうですね。
Trajectory Editorの使い方はこちらの記事で詳しくご紹介しております。
おわりに
今回はTrajectoryの途中変更の方法についてご紹介しましたが、同じ方法で他の各種パラメータも任意のタイミングで動的に変更させることができます。もうこれ以上は改良のしようがない...と諦めていた方にもまだまだタイム更新のチャンスがあるかもしれません！
過去の記事も合わせてご覧ください。
Discussion