apollo::prediction::RightOfWay类 参考

#include <right_of_way.h>

apollo::prediction::RightOfWay 的协作图:

Public 成员函数
	RightOfWay ()=delete
	Constructor

静态 Public 成员函数
static void	Analyze (ContainerManager *container_manager)
	Set right_of_way for all lane_sequence

详细描述

在文件 right_of_way.h 第 24 行定义.

构造及析构函数说明

RightOfWay()

apollo::prediction::RightOfWay::RightOfWay ( )

delete

Constructor

成员函数说明

Analyze()

void apollo::prediction::RightOfWay::Analyze ( ContainerManager *  container_manager )

static

Set right_of_way for all lane_sequence

返回

Scenario features

在文件 right_of_way.cc 第 38 行定义.

                                                            {
  ObstaclesContainer* obstacles_container =
      container_manager->GetContainer<ObstaclesContainer>(
          AdapterConfig::PERCEPTION_OBSTACLES);
  if (obstacles_container == nullptr) {
    AERROR << "Null obstacles container found";
    return;
  }
 
  ADCTrajectoryContainer* adc_trajectory_container =
      container_manager->GetContainer<ADCTrajectoryContainer>(
          AdapterConfig::PLANNING_TRAJECTORY);
  if (adc_trajectory_container == nullptr) {
    AERROR << "adc_trajectory_container is nullptr";
    return;
  }
  const std::vector<std::string>& lane_ids =
      adc_trajectory_container->GetADCLaneIDSequence();
  if (lane_ids.empty()) {
    return;
  }
 
  auto pose_container = container_manager->GetContainer<PoseContainer>(
      AdapterConfig::LOCALIZATION);
  if (pose_container == nullptr) {
    AERROR << "Pose container pointer is a null pointer.";
    return;
  }
  const PerceptionObstacle* pose_obstacle_ptr =
      pose_container->ToPerceptionObstacle();
  if (pose_obstacle_ptr == nullptr) {
    AERROR << "Pose obstacle pointer is a null pointer.";
    return;
  }
  double pose_x = pose_obstacle_ptr->position().x();
  double pose_y = pose_obstacle_ptr->position().y();
  double ego_vehicle_s = std::numeric_limits<double>::max();
  double ego_vehicle_l = std::numeric_limits<double>::max();
  double accumulated_s = 0.0;
  std::string ego_lane_id = "";
  // loop for lane_ids to findout ego_vehicle_s and lane_id
  for (const std::string& lane_id : lane_ids) {
    std::shared_ptr<const LaneInfo> lane_info_ptr =
        PredictionMap::LaneById(lane_id);
    if (lane_info_ptr == nullptr) {
      AERROR << "Null lane info pointer found.";
      continue;
    }
    double s = 0.0;
    double l = 0.0;
    if (PredictionMap::GetProjection({pose_x, pose_y}, lane_info_ptr, &s, &l)) {
      if (std::fabs(l) < std::fabs(ego_vehicle_l)) {
        ego_vehicle_s = accumulated_s + s;
        ego_vehicle_l = l;
        ego_lane_id = lane_id;
      }
    }
    accumulated_s += lane_info_ptr->total_length();
  }
 
  // loop again to assign ego_back_lane_id_set_ & ego_front_lane_id_set_
  accumulated_s = 0.0;
  std::unordered_set<std::string> ego_back_lane_id_set_;
  std::unordered_set<std::string> ego_front_lane_id_set_;
  for (const std::string& lane_id : lane_ids) {
    std::shared_ptr<const LaneInfo> lane_info_ptr =
        PredictionMap::LaneById(lane_id);
    if (lane_info_ptr == nullptr) {
      AERROR << "Null lane info pointer found.";
      continue;
    }
    accumulated_s += lane_info_ptr->total_length();
    if (accumulated_s < ego_vehicle_s) {
      ego_back_lane_id_set_.insert(lane_id);
    } else if (accumulated_s > ego_vehicle_s) {
      ego_front_lane_id_set_.insert(lane_id);
    }
  }
 
  // then loop through all obstacle vehicles
  for (const int id :
       obstacles_container->curr_frame_considered_obstacle_ids()) {
    Obstacle* obstacle_ptr = obstacles_container->GetObstacle(id);
    if (obstacle_ptr == nullptr) {
      continue;
    }
    Feature* latest_feature_ptr = obstacle_ptr->mutable_latest_feature();
    if (latest_feature_ptr->type() != PerceptionObstacle::VEHICLE) {
      continue;
    }
    ADEBUG << "RightOfWay for obstacle [" << latest_feature_ptr->id() << "], "
           << "with lane_sequence_size: "
           << latest_feature_ptr->lane().lane_graph().lane_sequence_size();
    for (auto& lane_sequence : *latest_feature_ptr->mutable_lane()
                                    ->mutable_lane_graph()
                                    ->mutable_lane_sequence()) {
      accumulated_s = 0.0;
      for (auto lane_segment : lane_sequence.lane_segment()) {
        accumulated_s += lane_segment.end_s() - lane_segment.start_s();
        // set lower righ_of_way for turn lanes
        if (lane_segment.lane_turn_type() == 2) {  // left_turn
          lane_sequence.set_right_of_way(-20);
        } else if (lane_segment.lane_turn_type() == 3) {  // right_turn
          lane_sequence.set_right_of_way(-10);
        }
        if (accumulated_s > 50.0) {
          break;
        }
      }
    }
  }
}

---

该类的文档由以下文件生成:

• modules/prediction/scenario/right_of_way/right_of_way.h
• modules/prediction/scenario/right_of_way/right_of_way.cc