AutowareでOverTakeModule(追い越しモジュール)を作る！ Part.2

# 概要

本記事は，以下の記事の続きです．

https://qiita.com/yhisaki/items/132b1c5d690d6d9f8760

# SceneModuleInterfaceの作り方

(以下の説明には誤りがある可能性が高いので，詳しくはDocumentやプログラムを直接参照してください)

実際にAutoware内で実装されているプログラムは，様々な状況に対応するPlanningをするための実装がなされているため，非常に複雑です．しかし，ある程度動くModuleを作成するだけなら，何点かの要点を掴むのみで大丈夫です．

まずは，Planningに用いるデータである`SceneModuleInterface`のメンバ変数である`planner_data_`についてです．

```cpp
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::isExecutionRequested`
- `BehaviorModuleOutput SceneModuleInterface::plan()`

`bool SceneModuleInterface::isExecutionRequested`は，Scene Moduleを実行するべきかどうかを判断します．例えば障害物を検知した場合にのみModuleを実行する場合は以下のようにします．
```cpp
bool OverTakeModule::isExecutionRequested() const
{
  return !planner_data_->dynamic_object->objects.empty();
}
```

`BehaviorModuleOutput SceneModuleInterface::plan()`は名前の通りPlanningを実行する関数です．`BehaviorModuleOutput`の`path`という変数にPlanningで生成されたPathを，`reference_path`に参照したPathを代入して返却します．この中身をどう実装するかが，メインテーマになります．

# `OverTakeModule::plan()`の実装

![Untitled-2023-12-06-1330.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320782/906432a3-4b9e-44cd-b761-00f183b040aa.png)

実装した内容は，上の図の通りです．前方の車両に対して，その車両が右にいるか左にいるかの情報を取り出し，右にいれば左に，左にいれば右に避けるというPathを生成するものとします．
実装内容は以下のとおりです．

```cpp
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()) };
}
```

# 実行結果

<iframe width="640" height="422" src="https://www.youtube.com/embed/FcV2mQPfy2M" title="Autoware OverTake" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

前方を走っている車両をうまく避けるようにPlanningできていることがわかります．実装は以下に公開しています．

https://github.com/yhisaki/autoware.universe/tree/feat/take_over

ぜひ見てみてください！