自動運転AIチャレンジ ピットストップエリアの停止にチャレンジ
背景
自動運転AIチャレンジ2024にチームで参加しており、大会への貢献のために開発した内容をまとめております。今回はピットストップエリアでの停止機能を開発したのでその内容をまとめます。
ルール
今大会は前回大会から以下のルールが追加されております。
- コンディション：車両の仮想的な値。一定以上になると速度制限がかかる。コースを一定間隔走行すると増加する。1周目では240まで上昇する。おそらく8つの障害物ポイントを通過するだけで 30 は増加する仕様。
- 仮想障害物:コース上の8つのポイントに配置される。周ごとにランダムで上下左右に座標が変化する。衝突するとコンディションが大きく増加する。
- ピットストップ：スタート地点に配置されるエリア。停止することでコンディションを0にし、速度制限をリセットできる。
そのため、速度制限がかかったタイミングでピットストップエリアに停止して、速度制限をなくすことが重要になります。
goal_pose_setterの変更
デフォルトの自動運転AIチャレンジ2024のリポジトリではgoal_pose_setter
という目的地を設定するパッケージが存在します。今回はこちらのパッケージを変更します。
設計
デフォルトのgoal_pose_setterでは以下のように、ゴールの座標、中間ゴールの座標を交互に目的地として送信しています。
今回は以下のようにピットストップエリアの座標を目的地として送信する状態を追加します。
実装
コンディションの監視
まず、コンディションの値を監視するノードを作成します。以下のようにコンディションを受信するsubscriberを定義します。
以下ではコールバック関数を onVehicleCondition という名前にしております。
vehicle_condition_subscriber_ = this->create_subscription<std_msgs::msg::Int32>(
"/aichallenge/pitstop/condition", 1,
std::bind(&GoalPosePublisher::onVehicleCondition, this, std::placeholders::_1));
次に以下のようにコールバック関数を定義します。以下は受信したコンディションが一定の値(今回は1000）以上になると、pit_stop_flagをTrueにします。
void GoalPosePublisher::onVehicleCondition(const std_msgs::msg::Int32::SharedPtr msg){
vehicle_condition_ = msg->data;
if (vehicle_condition_ >= 1000){
pit_stop_flag = true;
}
}
ピットストップエリアの設定
デフォルトではゴール座標、中間ゴール座標がdefault_goal_pose.param.yamlに設定されています。今回はピットストップエリアの座標をyamlに追加し、ソースコード内でロードします。
まず、以下のようにピットストップエリアの座標をyamlに追記します。
pit_stop_area.position.x: 89626.3671875
pit_stop_area.position.y: 43134.921875
pit_stop_area.position.z: 42.10000228881836
pit_stop_area .orientation.x: 0.0
pit_stop_area.orientation.y: -0.0
pit_stop_area.orientation.z: -0.878817200660705
pit_stop_area.orientation.w: -0.47715866565704346
pit_stop_area.size.x: 4.0
pit_stop_area.size.y: 2.0
次に以下をソースコードに追記し、ノードでパラメータを定義します。注意点として、パラメータをas_double()のフォーマットにしているため、yamlファイルの値を変更することがあれば小数点以下まで記載してください。
this->declare_parameter("pit_stop_area.position.x", 89626.367);
this->declare_parameter("pit_stop_area.position.y", 43134.921);
this->declare_parameter("pit_stop_area.position.z", 42.100);
this->declare_parameter("pit_stop_area.orientation.x", 0.0);
this->declare_parameter("pit_stop_area.orientation.y", -0.0);
this->declare_parameter("pit_stop_area.orientation.z", -0.878);
this->declare_parameter("pit_stop_area.orientation.w", -0.477);
pit_stop_goal_position_.position.x = this->get_parameter("pit_stop_area.position.x").as_double();
pit_stop_goal_position_.position.y = this->get_parameter("pit_stop_area.position.y").as_double();
pit_stop_goal_position_.position.z = this->get_parameter("pit_stop_area.position.z").as_double();
pit_stop_goal_position_.orientation.x = this->get_parameter("pit_stop_area.orientation.x").as_double();
pit_stop_goal_position_.orientation.y = this->get_parameter("pit_stop_area.orientation.y").as_double();
pit_stop_goal_position_.orientation.z = this->get_parameter("pit_stop_area.orientation.z").as_double();
pit_stop_goal_position_.orientation.w = this->get_parameter("pit_stop_area.orientation.w").as_double();
目的地の設定
各状態ごとで目的を変更するように odometry_callback 関数を以下のように変更します。
まず、"中間ゴールへの走行"状態に遷移する条件に"pit_stop_flag == false" を追加します
// Publish half goal pose for loop
if( pit_stop_flag == false &&
half_goal_pose_published_ == false &&
tier4_autoware_utils::calcDistance2d(msg->pose.pose, goal_position_) < goal_range_)
次に"ピットストップエリアへ走行"状態に遷移する部分を記載します。
// Publish pit_sotp_pose
if (pit_stop_flag == true &&
pit_stop_published == false &&
half_goal_pose_published_ == false &&
tier4_autoware_utils::calcDistance2d(msg->pose.pose, goal_position_) < goal_range_)
{
auto goal_pose = std::make_shared<geometry_msgs::msg::PoseStamped>();
goal_pose->header.stamp = this->get_clock()->now();
goal_pose->header.frame_id = "map";
goal_pose->pose = pit_stop_goal_position_;
goal_publisher_->publish(*goal_pose);
RCLCPP_INFO(this->get_logger(), "Publishing goal pose for loop");
pit_st
最後に"ピットストップエリアへ走行"状態から"中間ゴールへの走行"状態に遷移する部分を記載します。今回はコンディションの値が0になれば、遷移することにしました。
// Check vehicle condition and publish half goal pose
if (vehicle_condition_ == 0 && pit_stop_published == true)
{
auto goal_pose = std::make_shared<geometry_msgs::msg::PoseStamped>();
goal_pose->header.stamp = this->get_clock()->now();
goal_pose->header.frame_id = "map";
goal_pose->pose = half_goal_position_;
goal_publisher_->publish(*goal_pose);
RCLCPP_INFO(this->get_logger(), "Publishing half goal pose for loop");
half_goal_pose_published_ = true;
pit_stop_published = false;
pit_stop_flag = false;
}
ソースコード
変更したgoal_pose_setterのファイルはまとめると以下になります。
simple_pure_pursuiteの変更
デフォルトのsimple_pure_pursuiteでは目的値にピッタリ停止することができず、通り過ぎてしまうため、微調整を行いました。目的地に停止する方法はいろいろあると思いますが、できるだけデフォルトのソースコードから変更しない方法を紹介します。
設計
まず、デフォルトのsimple_pure_pursuiteのソースコードはこちらになります。simple_pure_pursuiteについてはこちらの記事で分かりやすく解説いただいております。
simple_pure_pursuiteの縦方向の制御では、trajectory_point ごとに設定される速度をそのまま目標の速度として使用します。また、残りのtrajectory_points数がある一定個数（デフォルトでは５個）以下になると停止のために目標速度を0にし、アクセルを負の値にします。今回は残りのtrajectory_points数の閾値を調整し、ピットストップエリアを通りすぎないようにします。
実装
パラメータの設定
残りのtrajectory_points数の閾値を minimum_trj_point_size_ とし、パラメータとして設定できるよういします。まず、以下のようなyamlファイルをパッケージ内に作成します。
/**:
ros__parameters:
minimum_trj_point_size: 16
次にソースコード内でパラメータをロードするように以下を追記します。
this->declare_parameter("minimum_trj_point_size", 10);
minimum_trj_point_size_ = this->get_parameter("minimum_trj_point_size").as_int();
最後に作成したdefault_simple_pure_pursuit.yamlを読み込むようにlaunch ファイルのsimple_pure_pursuitを起動する部分を変更します。
<node pkg="simple_pure_pursuit" exec="simple_pure_pursuit" name="simple_pure_pursuit_node" output="screen">
<param from="$(find-pkg-share simple_pure_pursuit)/config/default_simple_pure_pursuit.yaml"/>
<param name="use_external_target_vel" value="true"/>
<param name="external_target_vel" value="8.0"/>
<param name="lookahead_gain" value="0.3"/>
<param name="lookahead_min_distance" value="4.0"/>
<param name="speed_proportional_gain" value="1.0"/>
<remap from="input/kinematics" to="/localization/kinematic_state"/>
<remap from="input/trajectory" to="/planning/scenario_planning/trajectory"/>
<remap from="output/control_cmd" to="/control/command/control_cmd"/>
</node>
アルゴリズムの変更
ピットストップエリアに停止するためにsimple_pure_pursuit.cppのonTimer関数を変更していきます。
まず、以下で残りのtrajectory_points数を判定します。ここでパラメータ化したminimum_trj_point_size_ を使用します。さらに座標点の残りがminimum_trj_point_size_以下であれば停止のために速度を 0.0 、アクセルを -30.0 にします。
if (
(closet_traj_point_idx == trajectory_->points.size() - 1) ||
(trajectory_->points.size() <= minimum_trj_point_size_)) {
cmd.longitudinal.speed = 0.0;
cmd.longitudinal.acceleration = -30.0;
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "reached to the goal");
}
座標点の残りが少ない場合でも横方向の制御は計算するように、steering_tire_angleの計算部分をelse の外に出します。
else {
// get closest trajectory point from current position
TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx);
// calc longitudinal speed and acceleration
target_longitudinal_vel =
use_external_target_vel_ ? external_target_vel_ : closet_traj_point.longitudinal_velocity_mps;
double current_longitudinal_vel = odometry_->twist.twist.linear.x;
cmd.longitudinal.speed = target_longitudinal_vel;
cmd.longitudinal.acceleration =
speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel);
}
// calc lateral control
//// calc lookahead distance
double lookahead_distance = lookahead_gain_ * target_longitudinal_vel + lookahead_min_distance_;
//// calc center coordinate of rear wheel
double rear_x = odometry_->pose.pose.position.x -
wheel_base_ / 2.0 * std::cos(odometry_->pose.pose.orientation.z);
double rear_y = odometry_->pose.pose.position.y -
wheel_base_ / 2.0 * std::sin(odometry_->pose.pose.orientation.z);
//// search lookahead point
auto lookahead_point_itr = std::find_if(
trajectory_->points.begin() + closet_traj_point_idx, trajectory_->points.end(),
[&](const TrajectoryPoint & point) {
return std::hypot(point.pose.position.x - rear_x, point.pose.position.y - rear_y) >=
lookahead_distance;
});
if (lookahead_point_itr == trajectory_->points.end()) {
lookahead_point_itr = trajectory_->points.end() - 1;
}
double lookahead_point_x = lookahead_point_itr->pose.position.x;
double lookahead_point_y = lookahead_point_itr->pose.position.y;
// calc steering angle for lateral control
double alpha = std::atan2(lookahead_point_y - rear_y, lookahead_point_x - rear_x) -
tf2::getYaw(odometry_->pose.pose.orientation);
cmd.lateral.steering_tire_angle =
std::atan2(2.0 * wheel_base_ * std::sin(alpha), lookahead_distance);
pub_cmd_->publish(cmd);
ソースコード
作成・変更したファイルはまとめると以下になります。
- simple_pure_pursuit.cpp
- simple_pure_pursuit.hpp
- default_simple_pure_pursuit.yaml
- reference.launch.xml
結果
simple_pure_pursuite(デフォルト)
以下はsimple_pure_pursuiteを変更せずに、目的地のみを変更した結果になります。動画のようにピットストップエリアを通り過ぎてしまいます。
simple_pure_pursuite (変更版)
以下が今回作成した内容で走行した結果になります。
リポジトリ
今回作成したソースコード全体はこちらに公開しております。
まとめ
今回はピットストップエリアに停止するための変更方法を紹介しました。今回はsimple_pure_pursuiteを変更することで停止するように車両制御しましたが、実車の環境では停止位置がズレる可能性が高いため、柔軟な対応が必要になります。そのため、今後は車両制御方法の変更にも挑戦していきたいです。
Discussion