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

概要

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

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::isExecutionRequested
• BehaviorModuleOutput 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できていることがわかります．実装は以下に公開しています．

ぜひ見てみてください！