Baidu Apollo代码解析之predicition-container

Apollo中预测模块的任务是预测n秒内障碍物可能的运动轨迹，是以评估每条Lane Sequence的概率然后筛选进行的。预测模块由3个部分组成：Container，Evaluator，Predictor。而Container又分为3种：PoseContainer，ADCTrajectoryContainer，ObstaclesContainer。

Container主要是用来存储数据的，不同Container的输入都是ProtoBuf格式的Message，接收后格式转换（dynamic_cast）为特定Container所对应的数据类型。那么在存储之前，数据经过了什么处理，又是以什么格式存储的呢？我们从代码中一探究竟。

• PoseContainer

public:
  static const int ID = -1;
  static const perception::PerceptionObstacle::Type type_ =
      perception::PerceptionObstacle::VEHICLE;

 private:
  std::unique_ptr<perception::PerceptionObstacle> obstacle_ptr_;

主要就2个函数：Insert()和Update()。主要的数据处理都在Update()中。从上述的PoseContainer包含的数据成员来看，其只存储了1条PerceptionObstacle类型的信息。当新的定位信息到来后，Update()会从其中提取出有关数据构造PerceptionObstacle类型的obstacle_ptr_，并把以前的数据覆盖。

void PoseContainer::Insert(const ::google::protobuf::Message& message) {
  localization::LocalizationEstimate localization;
  localization.CopyFrom(dynamic_cast<const LocalizationEstimate&>(message));
  Update(localization);
}

void PoseContainer::Update(
    const localization::LocalizationEstimate& localization) {
  if (!localization.has_header() ||
      !localization.header().has_timestamp_sec()) {
    AERROR << "Localization message has no timestamp ["
           << localization.ShortDebugString() << "].";
    return;
  } else if (!localization.has_pose()) {
    AERROR << "Localization message has no pose ["
           << localization.ShortDebugString() << "].";
    return;
  } else if (!localization.pose().has_position() ||
             !localization.pose().has_linear_velocity()) {
    AERROR << "Localization message has no position or linear velocity ["
           << localization.ShortDebugString() << "].";
    return;
  }

  if (obstacle_ptr_.get() == nullptr) {
    obstacle_ptr_.reset(new PerceptionObstacle());
  }
  obstacle_ptr_->Clear();

  obstacle_ptr_->set_id(ID);
  Point position;
  position.set_x(localization.pose().position().x());
  position.set_y(localization.pose().position().y());
  position.set_z(localization.pose().position().z());
  obstacle_ptr_->mutable_position()->CopyFrom(position);

  double theta = 0.0;
  if (localization.pose().has_orientation() &&
      localization.pose().orientation().has_qx() &&
      localization.pose().orientation().has_qy() &&
      localization.pose().orientation().has_qz() &&
      localization.pose().orientation().has_qw()) {
    double qw = localization.pose().orientation().qw();
    double qx = localization.pose().orientation().qx();
    double qy = localization.pose().orientation().qy();
    double qz = localization.pose().orientation().qz();
    theta = common::math::QuaternionToHeading(qw, qx, qy, qz);
  }
  obstacle_ptr_->set_theta(theta);

  Point velocity;
  velocity.set_x(localization.pose().linear_velocity().x());
  velocity.set_y(localization.pose().linear_velocity().y());
  velocity.set_z(localization.pose().linear_velocity().z());
  obstacle_ptr_->mutable_velocity()->CopyFrom(velocity);

  obstacle_ptr_->set_type(type_);
  obstacle_ptr_->set_timestamp(localization.header().timestamp_sec());

  ADEBUG << "ADC obstacle [" << obstacle_ptr_->ShortDebugString() << "].";
}

• ADCTrajectoryContainer

private:
  planning::ADCTrajectory adc_trajectory_;
  common::math::Polygon2d adc_junction_polygon_;
  common::math::Polygon2d adc_pnc_junction_polygon_;
  std::shared_ptr<const hdmap::JunctionInfo> adc_junction_info_ptr_;
  std::shared_ptr<const hdmap::PNCJunctionInfo> adc_pnc_junction_info_ptr_;
  double s_dist_to_junction_;
  std::unordered_set<std::string> adc_lane_ids_;
  std::vector<std::string> adc_lane_seq_;
  std::mutex adc_trajectory_mutex_;

重点看该类的Insert()。从上述的ADCTrajectoryContainer包含的数据成员来看，其存储了1条ADCTrajectory类型的轨迹信息、路口的多边形轮廓信息、lane sequence信息。Insert()中，SetJunctionPolygon()和SetPNCJunctionPolygon()都是去寻找距adc_trajectory_最近的junction，超出搜索范围FLAGS_junction_search_radius的忽略，区别仅在于2种Junction的不同。目前我还没有看到二者的不同在哪？以后来填这个坑。然后沿着adc_trajectory_的lane_id，依次存入adc_lane_seq_。

void ADCTrajectoryContainer::Insert(
    const ::google::protobuf::Message& message) {
  adc_lane_ids_.clear();
  adc_lane_seq_.clear();
  adc_junction_polygon_ = std::move(Polygon2d());

  std::lock_guard<std::mutex> lock(adc_trajectory_mutex_);
  adc_trajectory_.CopyFrom(dynamic_cast<const ADCTrajectory&>(message));
  ADEBUG << "Received a planning message ["
         << adc_trajectory_.ShortDebugString() << "].";

  // Find junction
  SetJunctionPolygon();
  ADEBUG << "Generate a polygon [" << adc_junction_polygon_.DebugString()
         << "].";
  SetPNCJunctionPolygon();
  ADEBUG << "Generate a polygon [" << adc_pnc_junction_polygon_.DebugString()
         << "].";

  // Find ADC lane sequence
  SetLaneSequence();
  ADEBUG << "Generate an ADC lane id sequence [" << ToString(adc_lane_seq_)
         << "].";
}

SetPosition()的目的应该是调整position变动对lane sequence的影响。当车到达了某个position，他就不会沿着原来的lane sequence继续行进了，而是从当前位置继续沿lane sequence行进。因此，这里寻找距position最近的lane_id，重新插入到adc_lane_ids_中。

void ADCTrajectoryContainer::SetPosition(const Vec2d& position) {
  for (auto it = adc_lane_seq_.begin(); it != adc_lane_seq_.end(); ++it) {
    auto lane_info = PredictionMap::LaneById(*it);
    if (lane_info != nullptr && lane_info->IsOnLane(position)) {
      adc_lane_ids_.clear();
      adc_lane_ids_.insert(it, adc_lane_seq_.end());
      break;
    }
  }
  ADEBUG << "Generate an ADC lane ids [" << ToString(adc_lane_ids_) << "].";
}

• ObstaclesContainer

未完待续。