この記事は以下の2つをまとめたものです．
背景・概要
この記事は，AI Challengeのシミュレーション大会に関連する記事です．この大会は，カーレース環境のシミュレーション上で順位を競うもので，基本的にAutowareを使って車両の制御プログラムを記述します．
この記事では，Autowareのbehavior_path_plannerにプログラム(module)を追加して前方車両を追い越すようなPathを作るといった内容になっています．
目的
本記事の目的は，前の車両をうまく追い越すようなPlanningをすることです．現在のAutowareではavoidance_moduleのような障害物を避けるmoduleは存在しますが，前方車両を積極的に追い越すようにPlanningするようにはなっていない(はず)です．
しかし，レースにおいては前方車両を積極的に追い越して順位を上げなくてはなりません．レースにおける追い越し(Overtake)には，いくつかのルールが存在します．
例えば，
- 繰り返し追い越しが起きないように，進路変更の回数制限
- コーナーへ進入するときには，並走する相手のために1台分以上のスペースを空ける必要がある
- 追い越し禁止区間が存在する場合がある
などです．また，レースなので，相手車両に追い越されないように出来るだけ相手車両の前に出ることも重要です．
ただ，これらすべてのコンセプトを実現するmoduleを作成することは難しいため，基本的な追い越しの機能のみを実装しようと思います．
BehaviorPathPlanner
(以下の説明には誤りがある可能性が高いので，詳しくはDocumentやプログラムを直接参照してください)
上記のような目的を達成するためには，BehaviorPathPlannerのmoduleを作る必要があります．このmoduleでやるべきことのザックリとしたイメージは"ある程度決められたPathを障害物回避や車線変更などの要求に沿うように修正する"といったものです．
続き
SceneModuleInterfaceの作り方
(以下の説明には誤りがある可能性が高いので，詳しくはDocumentやプログラムを直接参照してください)
実際にAutoware内で実装されているプログラムは，様々な状況に対応するPlanningをするための実装がなされているため，非常に複雑です．しかし，ある程度動くModuleを作成するだけなら，何点かの要点を掴むのみで大丈夫です．
まずは，Planningに用いるデータであるSceneModuleInterfaceのメンバ変数であるplanner_data_についてです．
struct PlannerData
{
Odometry::ConstSharedPtr self_odometry{};
AccelWithCovarianceStamped::ConstSharedPtr self_acceleration{};
PredictedObjects::ConstSharedPtr dynamic_object{};
OccupancyGrid::ConstSharedPtr occupancy_grid{};
OccupancyGrid::ConstSharedPtr costmap{};
LateralOffset::ConstSharedPtr lateral_offset{};
OperationModeState::ConstSharedPtr operation_mode{};
PathWithLaneId::SharedPtr reference_path{std::make_shared<PathWithLaneId>()};
PathWithLaneId::SharedPtr prev_output_path{std::make_shared<PathWithLaneId>()};
lanelet::ConstLanelets current_lanes{};
std::shared_ptr<RouteHandler> route_handler{std::make_shared<RouteHandler>()};
BehaviorPathPlannerParameters parameters{};
drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{};
template <class T>
size_t findEgoIndex(const std::vector<T> & points) const
{
return motion_utils::findFirstNearestIndexWithSoftConstraints(
points, self_odometry->pose.pose, parameters.ego_nearest_dist_threshold,
parameters.ego_nearest_yaw_threshold);
}
template <class T>
size_t findEgoSegmentIndex(const std::vector<T> & points) const
{
return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
points, self_odometry->pose.pose, parameters.ego_nearest_dist_threshold,
parameters.ego_nearest_yaw_threshold);
}
};
この中には，基本的にPlannigに必要な情報が大体はいっており，この値自体は常にどこかで更新されているっぽいです．
Overrideするべき関数のうち，抑えておくべきものは以下の2点です．
bool SceneModuleInterface::isExecutionRequestedBehaviorModuleOutput SceneModuleInterface::plan()
bool SceneModuleInterface::isExecutionRequestedは，Scene Moduleを実行するべきかどうかを判断します．例えば障害物を検知した場合にのみModuleを実行する場合は以下のようにします．
bool OverTakeModule::isExecutionRequested() const
{
return !planner_data_->dynamic_object->objects.empty();
}
BehaviorModuleOutput SceneModuleInterface::plan()は名前の通りPlanningを実行する関数です．BehaviorModuleOutputのpathという変数にPlanningで生成されたPathを，reference_pathに参照したPathを代入して返却します．この中身をどう実装するかが，メインテーマになります．
OverTakeModule::plan()の実装
実装した内容は，上の図の通りです．前方の車両に対して，その車両が右にいるか左にいるかの情報を取り出し，右にいれば左に，左にいれば右に避けるというPathを生成するものとします．
実装内容は以下のとおりです．
BehaviorModuleOutput OverTakeModule::plan()
{
PathShifter path_shifter;
auto centerline_path = util::generateCenterLinePath(planner_data_);
path_shifter.setPath(*centerline_path);
path_shifter.setLateralAccelerationLimit(10.0);
for (auto& object : planner_data_->dynamic_object->objects)
{
auto reference_path = path_shifter.getReferencePath();
auto object_current_pose = object.kinematics.initial_pose_with_covariance.pose;
auto object_frenet_coord =
util::convertToFrenetCoordinate3d(reference_path.points, object_current_pose.position, 0);
auto nearest_idx = findNearestIndex(reference_path.points, object_current_pose);
if (!nearest_idx.has_value() || (nearest_idx.value() == 0))
{
path_shifter.setBaseOffset(0.0);
}
else
{
path_shifter.setBaseOffset(object_frenet_coord.distance < 0.0 ? 3.0 : -3.0);
auto front_of_object = util::lerpPoseByLength(reference_path.points, object_frenet_coord.length + 20.0);
auto backward_of_object = util::lerpPoseByLength(reference_path.points, object_frenet_coord.length + 10.0);
ShiftLine shift_line = { backward_of_object, front_of_object, 0 };
path_shifter.addShiftLine(shift_line);
}
}
ShiftedPath shifted_path;
path_shifter.generate(&shifted_path, true);
return { std::make_shared<PathWithLaneId>(shifted_path.path),
std::make_shared<PathWithLaneId>(path_shifter.getReferencePath()) };
}
実行結果
前方を走っている車両をうまく避けるようにPlanningできていることがわかります．実装は以下に公開しています．
ぜひ見てみてください！