apollo自动驾驶进阶学习之：Planning如何发送调试信息到dreamview

1、 ADCTrajectory消息定义

message PlanningData {
  // input
  optional apollo.localization.LocalizationEstimate adc_position = 7;
  optional apollo.canbus.Chassis chassis = 8;
  optional apollo.routing.RoutingResponse routing = 9;
  optional apollo.common.TrajectoryPoint init_point = 10;

  repeated apollo.common.Path path = 6;

  repeated SpeedPlan speed_plan = 13;
  repeated STGraphDebug st_graph = 14;
  repeated SLFrameDebug sl_frame = 15;

  optional apollo.common.Header prediction_header = 16;
  optional SignalLightDebug signal_light = 17;

  repeated ObstacleDebug obstacle = 18;
  repeated ReferenceLineDebug reference_line = 19;
  optional DpPolyGraphDebug dp_poly_graph = 20;
  optional LatticeStTraining lattice_st_image = 21;
  optional apollo.relative_map.MapMsg relative_map = 22;
  optional AutoTuningTrainingData auto_tuning_training_data = 23;
  optional double front_clear_distance = 24;
  repeated apollo.dreamview.Chart chart = 25;
  optional ScenarioDebug scenario = 26;
  optional OpenSpaceDebug open_space = 27;
  optional SmootherDebug smoother = 28;
  optional PullOverDebug pull_over = 29;
  optional HybridModelDebug hybrid_model = 30;
}

2、 最终消息发送：

on_lane_planning.cc中：

ptr_debug->MergeFrom(best_ref_info->debug());

  auto start_time = adc_trajectory_pb.header().timestamp_sec();
  const double dt = start_time - adc_trajectory_pb.header().timestamp_sec();
  for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) {
    p.set_relative_time(p.relative_time() + dt);
  }
  planning_writer_->Write(adc_trajectory_pb);

3、内容更新

3.1 st_bounds_decider.cc

  STGraphDebug* st_graph_debug = reference_line_info->mutable_debug()
                                     ->mutable_planning_data()
                                     ->add_st_graph();
  RecordSTGraphDebug(st_boundaries, regular_st_bound, st_guide_line,
                     st_graph_debug);

3.2 lane_follow_stage.cc

 const auto path_decision = reference_line_info->path_decision();
  for (const auto obstacle : path_decision->obstacles().Items()) {
    auto obstacle_debug = ptr_debug->mutable_planning_data()->add_obstacle();
    obstacle_debug->set_id(obstacle->Id());
    obstacle_debug->mutable_sl_boundary()->CopyFrom(
        obstacle->PerceptionSLBoundary());
    const auto& decider_tags = obstacle->decider_tags();
    const auto& decisions = obstacle->decisions();
    if (decider_tags.size() != decisions.size()) {
      AERROR << "decider_tags size: " << decider_tags.size()
             << " different from decisions size:" << decisions.size();
    }
    for (size_t i = 0; i < decider_tags.size(); ++i) {
      auto decision_tag = obstacle_debug->add_decision_tag();
      decision_tag->set_decider_tag(decider_tags[i]);
      decision_tag->mutable_decision()->CopyFrom(decisions[i]);
    }
  }

3.3 path_bounds_decider.cc

PathData left_path_data;
  left_path_data.SetReferenceLine(&(reference_line_info->reference_line()));
  left_path_data.SetFrenetPath(std::move(frenet_frame_left_path));
  PathData right_path_data;
  right_path_data.SetReferenceLine(&(reference_line_info->reference_line()));
  right_path_data.SetFrenetPath(std::move(frenet_frame_right_path));

  // Insert the transformed PathData into the simulator display.
  auto* ptr_display_path_1 =
      reference_line_info->mutable_debug()->mutable_planning_data()->add_path();
  ptr_display_path_1->set_name("planning_path_boundary_1");
  ptr_display_path_1->mutable_path_point()->CopyFrom(
      {left_path_data.discretized_path().begin(),
       left_path_data.discretized_path().end()});
  auto* ptr_display_path_2 =
      reference_line_info->mutable_debug()->mutable_planning_data()->add_path();
  ptr_display_path_2->set_name("planning_path_boundary_2");
  ptr_display_path_2->mutable_path_point()->CopyFrom(
      {right_path_data.discretized_path().begin(),
       right_path_data.discretized_path().end()});

4、参考线中位置和速度赋值：

bool ReferenceLineInfo::CombinePathAndSpeedProfile(
    const double relative_time, const double start_s,
    DiscretizedTrajectory* ptr_discretized_trajectory) {
  ACHECK(ptr_discretized_trajectory != nullptr);
  // use varied resolution to reduce data load but also provide enough data
  // point for control module
  const double kDenseTimeResoltuion = FLAGS_trajectory_time_min_interval;
  const double kSparseTimeResolution = FLAGS_trajectory_time_max_interval;
  const double kDenseTimeSec = FLAGS_trajectory_time_high_density_period;

  if (path_data_.discretized_path().empty()) {
    AERROR << "path data is empty";
    return false;
  }

  if (speed_data_.empty()) {
    AERROR << "speed profile is empty";
    return false;
  }

  for (double cur_rel_time = 0.0; cur_rel_time < speed_data_.TotalTime();
       cur_rel_time += (cur_rel_time < kDenseTimeSec ? kDenseTimeResoltuion
                                                     : kSparseTimeResolution)) {
    common::SpeedPoint speed_point;
    if (!speed_data_.EvaluateByTime(cur_rel_time, &speed_point)) {
      AERROR << "Fail to get speed point with relative time " << cur_rel_time;
      return false;
    }

    if (speed_point.s() > path_data_.discretized_path().Length()) {
      break;
    }
    common::PathPoint path_point =
        path_data_.GetPathPointWithPathS(speed_point.s());
    path_point.set_s(path_point.s() + start_s);

    common::TrajectoryPoint trajectory_point;
    trajectory_point.mutable_path_point()->CopyFrom(path_point);
    trajectory_point.set_v(speed_point.v());
    trajectory_point.set_a(speed_point.a());
    trajectory_point.set_relative_time(speed_point.t() + relative_time);
    ptr_discretized_trajectory->AppendTrajectoryPoint(trajectory_point);
  }
  return true;
}

5、最终发送信息中轨迹信息赋值

void PublishableTrajectory::PopulateTrajectoryProtobuf(
    ADCTrajectory* trajectory_pb) const {
  CHECK_NOTNULL(trajectory_pb);
  trajectory_pb->mutable_header()->set_timestamp_sec(header_time_);
  trajectory_pb->mutable_trajectory_point()->CopyFrom({begin(), end()});
  if (!empty()) {
    const auto& last_tp = back();
    trajectory_pb->set_total_path_length(last_tp.path_point().s());
    trajectory_pb->set_total_path_time(last_tp.relative_time());
  }
}

6、BUILD文件定义

cc_test(
    name = "reference_line_provider_test",
    size = "small",
    srcs = ["reference_line_provider_test.cc"],
    data = [
         "//modules/map/data:map_sunnyvale_loop",
         ":localization_testdata",
    ],
    deps = [
        "//cyber/common:log",
        ":reference_line_provider",
        "//modules/common/util",
        "//modules/planning/common:planning_common",
        "@com_google_googletest//:gtest_main",
        "//modules/common/vehicle_state:vehicle_state_provider",
        #"@local_config_python//:python_headers",
        #"@local_config_python//:python_lib",
        #"//modules/planning/proto:planning_cc_proto",
        #":matplotlibcpp"
    ],
)