diff --git a/.gitignore b/.gitignore index d919135a5f4..f4ca679b881 100644 --- a/.gitignore +++ b/.gitignore @@ -85,3 +85,4 @@ tools/FlameGraph modules/studio_connector #WORKSPACE +.VSCodeCounter/ diff --git a/modules/planning/planners/public_road/public_road_planner.cc b/modules/planning/planners/public_road/public_road_planner.cc index f8dfeef3555..a570c2168ef 100644 --- a/modules/planning/planners/public_road/public_road_planner.cc +++ b/modules/planning/planners/public_road/public_road_planner.cc @@ -28,23 +28,33 @@ using apollo::common::TrajectoryPoint; Status PublicRoadPlanner::Init( const std::shared_ptr& injector, const std::string& config_path) { + // 根据配置注册不同的场景 + // 支持的场景 modules/planning/planners/public_road/conf/planner_config.pb.txt Planner::Init(injector, config_path); LoadConfig(config_path, &config_); scenario_manager_.Init(injector, config_); return Status::OK(); } +// 公路场景规划器的Plan函数，用于生成规划轨迹 Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point, Frame* frame, ADCTrajectory* ptr_computed_trajectory) { + // 更新场景管理器 frame包含场景指令信息 scenario_manager_.Update(planning_start_point, frame); + + // 获取场景 scenario_ = scenario_manager_.mutable_scenario(); if (!scenario_) { + // 如果场景为空，返回错误 return Status(apollo::common::ErrorCode::PLANNING_ERROR, "Unknown Scenario"); } + + // 处理场景并获取结果 auto result = scenario_->Process(planning_start_point, frame); + // 如果启用了调试记录，则记录场景调试信息 if (FLAGS_enable_record_debug) { auto scenario_debug = ptr_computed_trajectory->mutable_debug() ->mutable_planning_data() @@ -54,14 +64,18 @@ Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point, scenario_debug->set_msg(scenario_->GetMsg()); } + // 仅在前一个场景的状态为STATUS_DONE时更新场景管理器 if (result.GetScenarioStatus() == ScenarioStatusType::STATUS_DONE) { // only updates scenario manager when previous scenario's status is // STATUS_DONE scenario_manager_.Update(planning_start_point, frame); } else if (result.GetScenarioStatus() == ScenarioStatusType::STATUS_UNKNOWN) { + // 如果场景的状态为STATUS_UNKNOWN，返回错误 return Status(common::PLANNING_ERROR, result.GetTaskStatus().error_message()); } + + // 返回处理结果的状态 return Status(common::OK, result.GetTaskStatus().error_message()); } diff --git a/modules/planning/planning_base/common/frame.h b/modules/planning/planning_base/common/frame.h index a91a7e15204..85d4220fad6 100644 --- a/modules/planning/planning_base/common/frame.h +++ b/modules/planning/planning_base/common/frame.h @@ -227,6 +227,7 @@ class Frame { ADCTrajectory current_frame_planned_trajectory_; // current frame path for future possible speed fallback + // 当前帧中规划并发布的轨迹 DiscretizedPath current_frame_planned_path_; const ReferenceLineProvider *reference_line_provider_ = nullptr; diff --git a/modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_problem.cc b/modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_problem.cc index 6e81682a760..ddfabbf9b1f 100644 --- a/modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_problem.cc +++ b/modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_problem.cc @@ -47,7 +47,7 @@ PiecewiseJerkProblem::PiecewiseJerkProblem( weight_x_ref_vec_ = std::vector(num_of_knots_, 0.0); } - +// 构建优化问题 bool PiecewiseJerkProblem::FormulateProblem(OSQPData* data) { // calculate kernel std::vector P_data; @@ -55,7 +55,7 @@ bool PiecewiseJerkProblem::FormulateProblem(OSQPData* data) { std::vector P_indptr; CalculateKernel(&P_data, &P_indices, &P_indptr); - // calculate affine constraints + // calculate affine constraints 线性约束 std::vector A_data; std::vector A_indices; std::vector A_indptr; @@ -64,6 +64,7 @@ bool PiecewiseJerkProblem::FormulateProblem(OSQPData* data) { CalculateAffineConstraint(&A_data, &A_indices, &A_indptr, &lower_bounds, &upper_bounds); + // offset补偿，用于尽量拟合参考线 // calculate offset std::vector q; CalculateOffset(&q); @@ -93,6 +94,7 @@ bool PiecewiseJerkProblem::Optimize(const int max_iter) { FreeData(data); return false; } + // OSQP设置 OSQPSettings* settings = SolverDefaultSettings(); settings->max_iter = max_iter; OSQPWorkspace* osqp_work = nullptr; @@ -150,7 +152,8 @@ void PiecewiseJerkProblem::CalculateAffineConstraint( num_of_variables); int constraint_index = 0; - // set x, x', x'' bounds + // 变量约束，设置上下边界 + // set x, x', x'' bounds for (int i = 0; i < num_of_variables; ++i) { if (i < n) { variables[i].emplace_back(constraint_index, 1.0); @@ -176,6 +179,8 @@ void PiecewiseJerkProblem::CalculateAffineConstraint( } CHECK_EQ(constraint_index, num_of_variables); + // 三个运动学约束 + // x(i->i+1)''' = (x(i+1)'' - x(i)'') / delta_s for (int i = 0; i + 1 < n; ++i) { variables[2 * n + i].emplace_back(constraint_index, -1.0); @@ -221,7 +226,7 @@ void PiecewiseJerkProblem::CalculateAffineConstraint( upper_bounds->at(constraint_index) = 0.0; ++constraint_index; } - + // 初始状态约束 最终轨迹的初始状态要与当前车辆状态保持一致。 // constrain on x_init variables[0].emplace_back(constraint_index, 1.0); lower_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0]; diff --git a/modules/planning/planning_component/on_lane_planning.cc b/modules/planning/planning_component/on_lane_planning.cc index 25d3929775b..0d6d1e0047e 100644 --- a/modules/planning/planning_component/on_lane_planning.cc +++ b/modules/planning/planning_component/on_lane_planning.cc @@ -129,12 +129,13 @@ Status OnLanePlanning::Init(const PlanningConfig& config) { if (config_.has_reference_line_config()) { reference_line_config = &config_.reference_line_config(); } + // 启动参考线提供器，会另启动一个线程，执行一个定时任务，每隔50ms提供一次参考线。 reference_line_provider_ = std::make_unique( injector_->vehicle_state(), reference_line_config); reference_line_provider_->Start(); // dispatch planner - LoadPlanner(); + LoadPlanner(); // 设置规划器 if (!planner_) { return Status( ErrorCode::PLANNING_ERROR, @@ -239,17 +240,17 @@ void OnLanePlanning::GenerateStopTrajectory(ADCTrajectory* ptr_trajectory_pb) { next_point->CopyFrom(tp); } } - +// onlaneplanner主体逻辑，由事件触发执行 void OnLanePlanning::RunOnce(const LocalView& local_view, ADCTrajectory* const ptr_trajectory_pb) { // when rerouting, reference line might not be updated. In this case, planning // module maintains not-ready until be restarted. - local_view_ = local_view; + local_view_ = local_view; // 包含规划所需的信息 const double start_timestamp = Clock::NowInSeconds(); const double start_system_timestamp = std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()) - .count(); + .count(); // 记录时间戳 // localization ADEBUG << "Get localization:" @@ -267,7 +268,10 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, << "start_timestamp is behind vehicle_state_timestamp by " << start_timestamp - vehicle_state_timestamp << " secs"; - if (!status.ok() || !util::IsVehicleStateValid(vehicle_state)) { + if (!status.ok() || + !util::IsVehicleStateValid( + vehicle_state)) { // 车辆状态信息获取失败 -> + // 记录错误信息，设置轨迹未就绪，并且返回 const std::string msg = "Update VehicleStateProvider failed " "or the vehicle state is out dated."; @@ -288,12 +292,12 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, common::monitor::MonitorLogBuffer monitor_logger_buffer( common::monitor::MonitorMessageItem::PLANNING); monitor_logger_buffer.ERROR("ego system time is behind GPS time"); - } + } // 检查系统时间是否与GPS时间接近 if (start_timestamp - vehicle_state_timestamp < FLAGS_message_latency_threshold) { vehicle_state = AlignTimeStamp(vehicle_state, start_timestamp); - } + } // 检查车辆状态时间戳是否与开始时间对齐 // Update reference line provider and reset scenario if new routing reference_line_provider_->UpdateVehicleState(vehicle_state); @@ -306,32 +310,37 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, injector_->planning_context()->mutable_planning_status()->Clear(); reference_line_provider_->UpdatePlanningCommand( *(local_view_.planning_command)); - planner_->Reset(frame_.get()); + planner_->Reset(frame_.get()); // 更新参考线提供者，有新的路线时重置场景 } // Get end lane way point. - reference_line_provider_->GetEndLaneWayPoint(local_view_.end_lane_way_point); + reference_line_provider_->GetEndLaneWayPoint( + local_view_.end_lane_way_point); // 获取终止车道点 // planning is triggered by prediction data, but we can still use an estimated // cycle time for stitching const double planning_cycle_time = - 1.0 / static_cast(FLAGS_planning_loop_rate); + 1.0 / static_cast(FLAGS_planning_loop_rate); // 计算规划周期时间 std::string replan_reason; + // 轨迹拼接 std::vector stitching_trajectory = TrajectoryStitcher::ComputeStitchingTrajectory( *(local_view_.chassis), vehicle_state, start_timestamp, planning_cycle_time, FLAGS_trajectory_stitching_preserved_length, true, last_publishable_trajectory_.get(), &replan_reason); + // 使用最终轨迹更新ego injector_->ego_info()->Update(stitching_trajectory.back(), vehicle_state); const uint32_t frame_num = static_cast(seq_num_++); AINFO << "Planning start frame sequence id = [" << frame_num << "]"; + // 使用给定的序列号和轨迹点初始化frame status = InitFrame(frame_num, stitching_trajectory.back(), vehicle_state); + // 检查初始化是否成功 if (status.ok()) { injector_->ego_info()->CalculateFrontObstacleClearDistance( frame_->obstacles()); } - + // 记录debug信息 if (FLAGS_enable_record_debug) { frame_->RecordInputDebug(ptr_trajectory_pb->mutable_debug()); } @@ -367,6 +376,7 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, return; } + // 判断是否符合交通规则 for (auto& ref_line_info : *frame_->mutable_reference_line_info()) { auto traffic_status = traffic_decider_.Execute(frame_.get(), &ref_line_info); @@ -376,10 +386,11 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, << " traffic decider failed"; } } - + // 规划轨迹 status = Plan(start_timestamp, stitching_trajectory, ptr_trajectory_pb); - // print trajxy + // 记录轨迹信息到LOG + // print trajxy PrintCurves trajectory_print_curve; for (const auto& p : ptr_trajectory_pb->trajectory_point()) { trajectory_print_curve.AddPoint("trajxy", p.path_point().x(), @@ -387,7 +398,8 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, } trajectory_print_curve.PrintToLog(); - // print obstacle polygon + // 记录轨迹和障碍物多边形 + // print obstacle polygon for (const auto& obstacle : frame_->obstacles()) { obstacle->PrintPolygonCurve(); } @@ -401,6 +413,7 @@ void OnLanePlanning::RunOnce(const LocalView& local_view, std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()) .count(); + // 记录规划时间 const auto time_diff_ms = (end_system_timestamp - start_system_timestamp) * 1000; ADEBUG << "total planning time spend: " << time_diff_ms << " ms."; @@ -544,7 +557,7 @@ Status OnLanePlanning::Plan( frame_->mutable_open_space_info()->set_debug(ptr_debug); frame_->mutable_open_space_info()->sync_debug_instance(); } - + // 调用onlaneplanner_planner auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(), ptr_trajectory_pb); @@ -703,580 +716,583 @@ Status OnLanePlanning::Plan( } bool OnLanePlanning::CheckPlanningConfig(const PlanningConfig& config) { - // TODO(All): check other config params - return true; + // TODO(All): check other config params + return true; } void PopulateChartOptions(double x_min, double x_max, std::string x_label, double y_min, double y_max, std::string y_label, bool display, Chart* chart) { - auto* options = chart->mutable_options(); - options->mutable_x()->set_min(x_min); - options->mutable_x()->set_max(x_max); - options->mutable_y()->set_min(y_min); - options->mutable_y()->set_max(y_max); - options->mutable_x()->set_label_string(x_label); - options->mutable_y()->set_label_string(y_label); - options->set_legend_display(display); + auto* options = chart->mutable_options(); + options->mutable_x()->set_min(x_min); + options->mutable_x()->set_max(x_max); + options->mutable_y()->set_min(y_min); + options->mutable_y()->set_max(y_max); + options->mutable_x()->set_label_string(x_label); + options->mutable_y()->set_label_string(y_label); + options->set_legend_display(display); } void AddSTGraph(const STGraphDebug& st_graph, Chart* chart) { - if (st_graph.name() == "DP_ST_SPEED_OPTIMIZER") { - chart->set_title("Speed Heuristic"); - } else { - chart->set_title("Planning S-T Graph"); - } - PopulateChartOptions(-2.0, 10.0, "t (second)", -10.0, 220.0, "s (meter)", - false, chart); - - for (const auto& boundary : st_graph.boundary()) { - // from 'ST_BOUNDARY_TYPE_' to the end - std::string type = - StGraphBoundaryDebug_StBoundaryType_Name(boundary.type()).substr(17); - - auto* boundary_chart = chart->add_polygon(); - auto* properties = boundary_chart->mutable_properties(); - (*properties)["borderWidth"] = "2"; - (*properties)["pointRadius"] = "0"; - (*properties)["lineTension"] = "0"; - (*properties)["cubicInterpolationMode"] = "monotone"; - (*properties)["showLine"] = "true"; - (*properties)["showText"] = "true"; - (*properties)["fill"] = "false"; - - if (type == "DRIVABLE_REGION") { - (*properties)["color"] = "\"rgba(0, 255, 0, 0.5)\""; - } else { - (*properties)["color"] = "\"rgba(255, 0, 0, 0.8)\""; - } + if (st_graph.name() == "DP_ST_SPEED_OPTIMIZER") { + chart->set_title("Speed Heuristic"); + } else { + chart->set_title("Planning S-T Graph"); + } + PopulateChartOptions(-2.0, 10.0, "t (second)", -10.0, 220.0, "s (meter)", + false, chart); + + for (const auto& boundary : st_graph.boundary()) { + // from 'ST_BOUNDARY_TYPE_' to the end + std::string type = + StGraphBoundaryDebug_StBoundaryType_Name(boundary.type()) + .substr(17); + + auto* boundary_chart = chart->add_polygon(); + auto* properties = boundary_chart->mutable_properties(); + (*properties)["borderWidth"] = "2"; + (*properties)["pointRadius"] = "0"; + (*properties)["lineTension"] = "0"; + (*properties)["cubicInterpolationMode"] = "monotone"; + (*properties)["showLine"] = "true"; + (*properties)["showText"] = "true"; + (*properties)["fill"] = "false"; + + if (type == "DRIVABLE_REGION") { + (*properties)["color"] = "\"rgba(0, 255, 0, 0.5)\""; + } else { + (*properties)["color"] = "\"rgba(255, 0, 0, 0.8)\""; + } - boundary_chart->set_label(boundary.name() + "_" + type); - for (const auto& point : boundary.point()) { - auto* point_debug = boundary_chart->add_point(); - point_debug->set_x(point.t()); - point_debug->set_y(point.s()); - } - } + boundary_chart->set_label(boundary.name() + "_" + type); + for (const auto& point : boundary.point()) { + auto* point_debug = boundary_chart->add_point(); + point_debug->set_x(point.t()); + point_debug->set_y(point.s()); + } + } - auto* speed_profile = chart->add_line(); - auto* properties = speed_profile->mutable_properties(); - (*properties)["color"] = "\"rgba(255, 255, 255, 0.5)\""; - for (const auto& point : st_graph.speed_profile()) { - auto* point_debug = speed_profile->add_point(); - point_debug->set_x(point.t()); - point_debug->set_y(point.s()); - } + auto* speed_profile = chart->add_line(); + auto* properties = speed_profile->mutable_properties(); + (*properties)["color"] = "\"rgba(255, 255, 255, 0.5)\""; + for (const auto& point : st_graph.speed_profile()) { + auto* point_debug = speed_profile->add_point(); + point_debug->set_x(point.t()); + point_debug->set_y(point.s()); + } } void AddSLFrame(const SLFrameDebug& sl_frame, Chart* chart) { - chart->set_title(sl_frame.name()); - PopulateChartOptions(0.0, 80.0, "s (meter)", -8.0, 8.0, "l (meter)", false, - chart); - auto* sl_line = chart->add_line(); - sl_line->set_label("SL Path"); - for (const auto& sl_point : sl_frame.sl_path()) { - auto* point_debug = sl_line->add_point(); - point_debug->set_x(sl_point.s()); - point_debug->set_x(sl_point.l()); - } + chart->set_title(sl_frame.name()); + PopulateChartOptions(0.0, 80.0, "s (meter)", -8.0, 8.0, "l (meter)", + false, chart); + auto* sl_line = chart->add_line(); + sl_line->set_label("SL Path"); + for (const auto& sl_point : sl_frame.sl_path()) { + auto* point_debug = sl_line->add_point(); + point_debug->set_x(sl_point.s()); + point_debug->set_x(sl_point.l()); + } } void AddSpeedPlan( const ::google::protobuf::RepeatedPtrField& speed_plans, Chart* chart) { - chart->set_title("Speed Plan"); - PopulateChartOptions(0.0, 80.0, "s (meter)", 0.0, 50.0, "v (m/s)", false, - chart); - - for (const auto& speed_plan : speed_plans) { - auto* line = chart->add_line(); - line->set_label(speed_plan.name()); - for (const auto& point : speed_plan.speed_point()) { - auto* point_debug = line->add_point(); - point_debug->set_x(point.s()); - point_debug->set_y(point.v()); - } + chart->set_title("Speed Plan"); + PopulateChartOptions(0.0, 80.0, "s (meter)", 0.0, 50.0, "v (m/s)", false, + chart); + + for (const auto& speed_plan : speed_plans) { + auto* line = chart->add_line(); + line->set_label(speed_plan.name()); + for (const auto& point : speed_plan.speed_point()) { + auto* point_debug = line->add_point(); + point_debug->set_x(point.s()); + point_debug->set_y(point.v()); + } - // Set chartJS's dataset properties - auto* properties = line->mutable_properties(); - (*properties)["borderWidth"] = "2"; - (*properties)["pointRadius"] = "0"; - (*properties)["fill"] = "false"; - (*properties)["showLine"] = "true"; - if (speed_plan.name() == "DpStSpeedOptimizer") { - (*properties)["color"] = "\"rgba(27, 249, 105, 0.5)\""; - } else if (speed_plan.name() == "QpSplineStSpeedOptimizer") { - (*properties)["color"] = "\"rgba(54, 162, 235, 1)\""; - } - } + // Set chartJS's dataset properties + auto* properties = line->mutable_properties(); + (*properties)["borderWidth"] = "2"; + (*properties)["pointRadius"] = "0"; + (*properties)["fill"] = "false"; + (*properties)["showLine"] = "true"; + if (speed_plan.name() == "DpStSpeedOptimizer") { + (*properties)["color"] = "\"rgba(27, 249, 105, 0.5)\""; + } else if (speed_plan.name() == "QpSplineStSpeedOptimizer") { + (*properties)["color"] = "\"rgba(54, 162, 235, 1)\""; + } + } } void OnLanePlanning::ExportFailedLaneChangeSTChart( const planning_internal::Debug& debug_info, planning_internal::Debug* debug_chart) { - const auto& src_data = debug_info.planning_data(); - auto* dst_data = debug_chart->mutable_planning_data(); - for (const auto& st_graph : src_data.st_graph()) { - AddSTGraph(st_graph, dst_data->add_chart()); - } + const auto& src_data = debug_info.planning_data(); + auto* dst_data = debug_chart->mutable_planning_data(); + for (const auto& st_graph : src_data.st_graph()) { + AddSTGraph(st_graph, dst_data->add_chart()); + } } void OnLanePlanning::ExportOnLaneChart( const planning_internal::Debug& debug_info, planning_internal::Debug* debug_chart) { - const auto& src_data = debug_info.planning_data(); - auto* dst_data = debug_chart->mutable_planning_data(); - for (const auto& st_graph : src_data.st_graph()) { - AddSTGraph(st_graph, dst_data->add_chart()); - } - for (const auto& sl_frame : src_data.sl_frame()) { - AddSLFrame(sl_frame, dst_data->add_chart()); - } - AddSpeedPlan(src_data.speed_plan(), dst_data->add_chart()); + const auto& src_data = debug_info.planning_data(); + auto* dst_data = debug_chart->mutable_planning_data(); + for (const auto& st_graph : src_data.st_graph()) { + AddSTGraph(st_graph, dst_data->add_chart()); + } + for (const auto& sl_frame : src_data.sl_frame()) { + AddSLFrame(sl_frame, dst_data->add_chart()); + } + AddSpeedPlan(src_data.speed_plan(), dst_data->add_chart()); } void OnLanePlanning::ExportOpenSpaceChart( const planning_internal::Debug& debug_info, const ADCTrajectory& trajectory_pb, planning_internal::Debug* debug_chart) { - // Export Trajectory Visualization Chart. - if (FLAGS_enable_record_debug) { - AddOpenSpaceOptimizerResult(debug_info, debug_chart); - AddPartitionedTrajectory(debug_info, debug_chart); - AddStitchSpeedProfile(debug_chart); - AddPublishedSpeed(trajectory_pb, debug_chart); - AddPublishedAcceleration(trajectory_pb, debug_chart); - // AddFallbackTrajectory(debug_info, debug_chart); - } + // Export Trajectory Visualization Chart. + if (FLAGS_enable_record_debug) { + AddOpenSpaceOptimizerResult(debug_info, debug_chart); + AddPartitionedTrajectory(debug_info, debug_chart); + AddStitchSpeedProfile(debug_chart); + AddPublishedSpeed(trajectory_pb, debug_chart); + AddPublishedAcceleration(trajectory_pb, debug_chart); + // AddFallbackTrajectory(debug_info, debug_chart); + } } void OnLanePlanning::AddOpenSpaceOptimizerResult( const planning_internal::Debug& debug_info, planning_internal::Debug* debug_chart) { - // if open space info provider success run - if (!frame_->open_space_info().open_space_provider_success()) { - return; - } + // if open space info provider success run + if (!frame_->open_space_info().open_space_provider_success()) { + return; + } - auto chart = debug_chart->mutable_planning_data()->add_chart(); - auto open_space_debug = debug_info.planning_data().open_space(); - - chart->set_title("Open Space Trajectory Optimizer Visualization"); - PopulateChartOptions(open_space_debug.xy_boundary(0) - 1.0, - open_space_debug.xy_boundary(1) + 1.0, "x (meter)", - open_space_debug.xy_boundary(2) - 1.0, - open_space_debug.xy_boundary(3) + 1.0, "y (meter)", true, - chart); - - chart->mutable_options()->set_sync_xy_window_size(true); - chart->mutable_options()->set_aspect_ratio(0.9); - int obstacle_index = 1; - for (const auto& obstacle : open_space_debug.obstacles()) { - auto* obstacle_outline = chart->add_line(); - obstacle_outline->set_label(absl::StrCat("Bdr", obstacle_index)); - obstacle_index += 1; - for (int vertice_index = 0; - vertice_index < obstacle.vertices_x_coords_size(); vertice_index++) { - auto* point_debug = obstacle_outline->add_point(); - point_debug->set_x(obstacle.vertices_x_coords(vertice_index)); - point_debug->set_y(obstacle.vertices_y_coords(vertice_index)); - } - // Set chartJS's dataset properties - auto* obstacle_properties = obstacle_outline->mutable_properties(); - (*obstacle_properties)["borderWidth"] = "2"; - (*obstacle_properties)["pointRadius"] = "0"; - (*obstacle_properties)["lineTension"] = "0"; - (*obstacle_properties)["fill"] = "false"; - (*obstacle_properties)["showLine"] = "true"; - } + auto chart = debug_chart->mutable_planning_data()->add_chart(); + auto open_space_debug = debug_info.planning_data().open_space(); + + chart->set_title("Open Space Trajectory Optimizer Visualization"); + PopulateChartOptions(open_space_debug.xy_boundary(0) - 1.0, + open_space_debug.xy_boundary(1) + 1.0, "x (meter)", + open_space_debug.xy_boundary(2) - 1.0, + open_space_debug.xy_boundary(3) + 1.0, "y (meter)", + true, chart); + + chart->mutable_options()->set_sync_xy_window_size(true); + chart->mutable_options()->set_aspect_ratio(0.9); + int obstacle_index = 1; + for (const auto& obstacle : open_space_debug.obstacles()) { + auto* obstacle_outline = chart->add_line(); + obstacle_outline->set_label(absl::StrCat("Bdr", obstacle_index)); + obstacle_index += 1; + for (int vertice_index = 0; + vertice_index < obstacle.vertices_x_coords_size(); + vertice_index++) { + auto* point_debug = obstacle_outline->add_point(); + point_debug->set_x(obstacle.vertices_x_coords(vertice_index)); + point_debug->set_y(obstacle.vertices_y_coords(vertice_index)); + } + // Set chartJS's dataset properties + auto* obstacle_properties = obstacle_outline->mutable_properties(); + (*obstacle_properties)["borderWidth"] = "2"; + (*obstacle_properties)["pointRadius"] = "0"; + (*obstacle_properties)["lineTension"] = "0"; + (*obstacle_properties)["fill"] = "false"; + (*obstacle_properties)["showLine"] = "true"; + } - auto smoothed_trajectory = open_space_debug.smoothed_trajectory(); - auto* smoothed_line = chart->add_line(); - smoothed_line->set_label("Smooth"); - // size_t adc_label = 0; - for (int i = 0; i < smoothed_trajectory.vehicle_motion_point_size() / 2; - i++) { - auto& point = smoothed_trajectory.vehicle_motion_point(i); - const auto x = point.trajectory_point().path_point().x(); - const auto y = point.trajectory_point().path_point().y(); - // const auto heading = point.trajectory_point().path_point().theta(); - /* - // Draw vehicle shape along the trajectory - auto* adc_shape = chart->add_car(); - adc_shape->set_x(x); - adc_shape->set_y(y); - adc_shape->set_heading(heading); - adc_shape->set_color("rgba(54, 162, 235, 1)"); - adc_shape->set_label(std::to_string(adc_label)); - adc_shape->set_hide_label_in_legend(true); - ++adc_label; - */ - // Draw vehicle trajectory points - auto* point_debug = smoothed_line->add_point(); - point_debug->set_x(x); - point_debug->set_y(y); - } + auto smoothed_trajectory = open_space_debug.smoothed_trajectory(); + auto* smoothed_line = chart->add_line(); + smoothed_line->set_label("Smooth"); + // size_t adc_label = 0; + for (int i = 0; i < smoothed_trajectory.vehicle_motion_point_size() / 2; + i++) { + auto& point = smoothed_trajectory.vehicle_motion_point(i); + const auto x = point.trajectory_point().path_point().x(); + const auto y = point.trajectory_point().path_point().y(); + // const auto heading = point.trajectory_point().path_point().theta(); + /* + // Draw vehicle shape along the trajectory + auto* adc_shape = chart->add_car(); + adc_shape->set_x(x); + adc_shape->set_y(y); + adc_shape->set_heading(heading); + adc_shape->set_color("rgba(54, 162, 235, 1)"); + adc_shape->set_label(std::to_string(adc_label)); + adc_shape->set_hide_label_in_legend(true); + ++adc_label; + */ + // Draw vehicle trajectory points + auto* point_debug = smoothed_line->add_point(); + point_debug->set_x(x); + point_debug->set_y(y); + } - // Set chartJS's dataset properties - auto* smoothed_properties = smoothed_line->mutable_properties(); - (*smoothed_properties)["borderWidth"] = "2"; - (*smoothed_properties)["pointRadius"] = "0"; - (*smoothed_properties)["lineTension"] = "0"; - (*smoothed_properties)["fill"] = "false"; - (*smoothed_properties)["showLine"] = "true"; - - auto warm_start_trajectory = open_space_debug.warm_start_trajectory(); - auto* warm_start_line = chart->add_line(); - warm_start_line->set_label("WarmStart"); - for (int i = 0; i < warm_start_trajectory.vehicle_motion_point_size() / 2; - i++) { - auto* point_debug = warm_start_line->add_point(); - auto& point = warm_start_trajectory.vehicle_motion_point(i); - point_debug->set_x(point.trajectory_point().path_point().x()); - point_debug->set_y(point.trajectory_point().path_point().y()); - } - // Set chartJS's dataset properties - auto* warm_start_properties = warm_start_line->mutable_properties(); - (*warm_start_properties)["borderWidth"] = "2"; - (*warm_start_properties)["pointRadius"] = "0"; - (*warm_start_properties)["lineTension"] = "0"; - (*warm_start_properties)["fill"] = "false"; - (*warm_start_properties)["showLine"] = "true"; + // Set chartJS's dataset properties + auto* smoothed_properties = smoothed_line->mutable_properties(); + (*smoothed_properties)["borderWidth"] = "2"; + (*smoothed_properties)["pointRadius"] = "0"; + (*smoothed_properties)["lineTension"] = "0"; + (*smoothed_properties)["fill"] = "false"; + (*smoothed_properties)["showLine"] = "true"; + + auto warm_start_trajectory = open_space_debug.warm_start_trajectory(); + auto* warm_start_line = chart->add_line(); + warm_start_line->set_label("WarmStart"); + for (int i = 0; i < warm_start_trajectory.vehicle_motion_point_size() / 2; + i++) { + auto* point_debug = warm_start_line->add_point(); + auto& point = warm_start_trajectory.vehicle_motion_point(i); + point_debug->set_x(point.trajectory_point().path_point().x()); + point_debug->set_y(point.trajectory_point().path_point().y()); + } + // Set chartJS's dataset properties + auto* warm_start_properties = warm_start_line->mutable_properties(); + (*warm_start_properties)["borderWidth"] = "2"; + (*warm_start_properties)["pointRadius"] = "0"; + (*warm_start_properties)["lineTension"] = "0"; + (*warm_start_properties)["fill"] = "false"; + (*warm_start_properties)["showLine"] = "true"; } void OnLanePlanning::AddPartitionedTrajectory( const planning_internal::Debug& debug_info, planning_internal::Debug* debug_chart) { - // if open space info provider success run - if (!frame_->open_space_info().open_space_provider_success()) { - return; - } + // if open space info provider success run + if (!frame_->open_space_info().open_space_provider_success()) { + return; + } - const auto& open_space_debug = debug_info.planning_data().open_space(); - const auto& chosen_trajectories = - open_space_debug.chosen_trajectory().trajectory(); - if (chosen_trajectories.empty() || - chosen_trajectories[0].trajectory_point().empty()) { - return; - } + const auto& open_space_debug = debug_info.planning_data().open_space(); + const auto& chosen_trajectories = + open_space_debug.chosen_trajectory().trajectory(); + if (chosen_trajectories.empty() || + chosen_trajectories[0].trajectory_point().empty()) { + return; + } - const auto& vehicle_state = frame_->vehicle_state(); - auto chart = debug_chart->mutable_planning_data()->add_chart(); - auto chart_kappa = debug_chart->mutable_planning_data()->add_chart(); - auto chart_theta = debug_chart->mutable_planning_data()->add_chart(); - chart->set_title("Open Space Partitioned Trajectory"); - chart_kappa->set_title("total kappa"); - chart_theta->set_title("total theta"); - auto* options = chart->mutable_options(); - options->mutable_x()->set_label_string("x (meter)"); - options->mutable_y()->set_label_string("y (meter)"); - options->set_sync_xy_window_size(true); - options->set_aspect_ratio(0.9); - - // Draw vehicle state - auto* adc_shape = chart->add_car(); - adc_shape->set_x(vehicle_state.x()); - adc_shape->set_y(vehicle_state.y()); - adc_shape->set_heading(vehicle_state.heading()); - adc_shape->set_label("ADV"); - adc_shape->set_color("rgba(54, 162, 235, 1)"); - - // Draw the chosen trajectories - const auto& chosen_trajectory = chosen_trajectories[0]; - auto* chosen_line = chart->add_line(); - chosen_line->set_label("Chosen"); - for (const auto& point : chosen_trajectory.trajectory_point()) { - auto* point_debug = chosen_line->add_point(); - point_debug->set_x(point.path_point().x()); - point_debug->set_y(point.path_point().y()); - } - auto* chosen_properties = chosen_line->mutable_properties(); - (*chosen_properties)["borderWidth"] = "2"; - (*chosen_properties)["pointRadius"] = "0"; - (*chosen_properties)["lineTension"] = "0"; - (*chosen_properties)["fill"] = "false"; - (*chosen_properties)["showLine"] = "true"; - auto* theta_line = chart_theta->add_line(); - auto* kappa_line = chart_kappa->add_line(); - // Draw partitioned trajectories - size_t partitioned_trajectory_label = 0; - for (const auto& partitioned_trajectory : - open_space_debug.partitioned_trajectories().trajectory()) { - auto* partition_line = chart->add_line(); - partition_line->set_label( - absl::StrCat("Partitioned ", partitioned_trajectory_label)); - ++partitioned_trajectory_label; - for (const auto& point : partitioned_trajectory.trajectory_point()) { - auto* point_debug = partition_line->add_point(); - auto* point_theta = theta_line->add_point(); - auto* point_kappa = kappa_line->add_point(); - point_debug->set_x(point.path_point().x()); - point_debug->set_y(point.path_point().y()); - point_theta->set_x(point.relative_time()); - point_kappa->set_x(point.relative_time()); - point_theta->set_y(point.path_point().theta()); - point_kappa->set_y(point.path_point().kappa()); - } + const auto& vehicle_state = frame_->vehicle_state(); + auto chart = debug_chart->mutable_planning_data()->add_chart(); + auto chart_kappa = debug_chart->mutable_planning_data()->add_chart(); + auto chart_theta = debug_chart->mutable_planning_data()->add_chart(); + chart->set_title("Open Space Partitioned Trajectory"); + chart_kappa->set_title("total kappa"); + chart_theta->set_title("total theta"); + auto* options = chart->mutable_options(); + options->mutable_x()->set_label_string("x (meter)"); + options->mutable_y()->set_label_string("y (meter)"); + options->set_sync_xy_window_size(true); + options->set_aspect_ratio(0.9); + + // Draw vehicle state + auto* adc_shape = chart->add_car(); + adc_shape->set_x(vehicle_state.x()); + adc_shape->set_y(vehicle_state.y()); + adc_shape->set_heading(vehicle_state.heading()); + adc_shape->set_label("ADV"); + adc_shape->set_color("rgba(54, 162, 235, 1)"); + + // Draw the chosen trajectories + const auto& chosen_trajectory = chosen_trajectories[0]; + auto* chosen_line = chart->add_line(); + chosen_line->set_label("Chosen"); + for (const auto& point : chosen_trajectory.trajectory_point()) { + auto* point_debug = chosen_line->add_point(); + point_debug->set_x(point.path_point().x()); + point_debug->set_y(point.path_point().y()); + } + auto* chosen_properties = chosen_line->mutable_properties(); + (*chosen_properties)["borderWidth"] = "2"; + (*chosen_properties)["pointRadius"] = "0"; + (*chosen_properties)["lineTension"] = "0"; + (*chosen_properties)["fill"] = "false"; + (*chosen_properties)["showLine"] = "true"; + auto* theta_line = chart_theta->add_line(); + auto* kappa_line = chart_kappa->add_line(); + // Draw partitioned trajectories + size_t partitioned_trajectory_label = 0; + for (const auto& partitioned_trajectory : + open_space_debug.partitioned_trajectories().trajectory()) { + auto* partition_line = chart->add_line(); + partition_line->set_label( + absl::StrCat("Partitioned ", partitioned_trajectory_label)); + ++partitioned_trajectory_label; + for (const auto& point : partitioned_trajectory.trajectory_point()) { + auto* point_debug = partition_line->add_point(); + auto* point_theta = theta_line->add_point(); + auto* point_kappa = kappa_line->add_point(); + point_debug->set_x(point.path_point().x()); + point_debug->set_y(point.path_point().y()); + point_theta->set_x(point.relative_time()); + point_kappa->set_x(point.relative_time()); + point_theta->set_y(point.path_point().theta()); + point_kappa->set_y(point.path_point().kappa()); + } - auto* partition_properties = partition_line->mutable_properties(); - (*partition_properties)["borderWidth"] = "2"; - (*partition_properties)["pointRadius"] = "0"; - (*partition_properties)["lineTension"] = "0"; - (*partition_properties)["fill"] = "false"; - (*partition_properties)["showLine"] = "true"; - SetChartminmax(chart_kappa, "time", "total kappa"); - SetChartminmax(chart_theta, "time", "total theta"); - } + auto* partition_properties = partition_line->mutable_properties(); + (*partition_properties)["borderWidth"] = "2"; + (*partition_properties)["pointRadius"] = "0"; + (*partition_properties)["lineTension"] = "0"; + (*partition_properties)["fill"] = "false"; + (*partition_properties)["showLine"] = "true"; + SetChartminmax(chart_kappa, "time", "total kappa"); + SetChartminmax(chart_theta, "time", "total theta"); + } - // Draw trajectory stitching point (line with only one point) - // auto* stitching_line = chart->add_line(); - // stitching_line->set_label("TrajectoryStitchingPoint"); - // auto* trajectory_stitching_point = stitching_line->add_point(); - // trajectory_stitching_point->set_x( - // open_space_debug.trajectory_stitching_point().path_point().x()); - // trajectory_stitching_point->set_y( - // open_space_debug.trajectory_stitching_point().path_point().y()); - // // Set chartJS's dataset properties - // auto* stitching_properties = stitching_line->mutable_properties(); - // (*stitching_properties)["borderWidth"] = "3"; - // (*stitching_properties)["pointRadius"] = "5"; - // (*stitching_properties)["lineTension"] = "0"; - // (*stitching_properties)["fill"] = "true"; - // (*stitching_properties)["showLine"] = "true"; - - // Draw fallback trajectory compared with the partitioned and potential - // collision_point (line with only one point) - // if (open_space_debug.is_fallback_trajectory()) { - // auto* collision_line = chart->add_line(); - // collision_line->set_label("FutureCollisionPoint"); - // auto* future_collision_point = collision_line->add_point(); - // future_collision_point->set_x( - // open_space_debug.future_collision_point().path_point().x()); - // future_collision_point->set_y( - // open_space_debug.future_collision_point().path_point().y()); - // // Set chartJS's dataset properties - // auto* collision_properties = collision_line->mutable_properties(); - // (*collision_properties)["borderWidth"] = "3"; - // (*collision_properties)["pointRadius"] = "8"; - // (*collision_properties)["lineTension"] = "0"; - // (*collision_properties)["fill"] = "true"; - // (*stitching_properties)["showLine"] = "true"; - // (*stitching_properties)["pointStyle"] = "cross"; - - // const auto& fallback_trajectories = - // open_space_debug.fallback_trajectory().trajectory(); - // if (fallback_trajectories.empty() || - // fallback_trajectories[0].trajectory_point().empty()) { - // return; - // } - // const auto& fallback_trajectory = fallback_trajectories[0]; - // // has to define chart boundary first - // auto* fallback_line = chart->add_line(); - // fallback_line->set_label("Fallback"); - // for (const auto& point : fallback_trajectory.trajectory_point()) { - // auto* point_debug = fallback_line->add_point(); - // point_debug->set_x(point.path_point().x()); - // point_debug->set_y(point.path_point().y()); - // } - // // Set chartJS's dataset properties - // auto* fallback_properties = fallback_line->mutable_properties(); - // (*fallback_properties)["borderWidth"] = "3"; - // (*fallback_properties)["pointRadius"] = "2"; - // (*fallback_properties)["lineTension"] = "0"; - // (*fallback_properties)["fill"] = "false"; - // (*fallback_properties)["showLine"] = "true"; - // } + // Draw trajectory stitching point (line with only one point) + // auto* stitching_line = chart->add_line(); + // stitching_line->set_label("TrajectoryStitchingPoint"); + // auto* trajectory_stitching_point = stitching_line->add_point(); + // trajectory_stitching_point->set_x( + // open_space_debug.trajectory_stitching_point().path_point().x()); + // trajectory_stitching_point->set_y( + // open_space_debug.trajectory_stitching_point().path_point().y()); + // // Set chartJS's dataset properties + // auto* stitching_properties = stitching_line->mutable_properties(); + // (*stitching_properties)["borderWidth"] = "3"; + // (*stitching_properties)["pointRadius"] = "5"; + // (*stitching_properties)["lineTension"] = "0"; + // (*stitching_properties)["fill"] = "true"; + // (*stitching_properties)["showLine"] = "true"; + + // Draw fallback trajectory compared with the partitioned and potential + // collision_point (line with only one point) + // if (open_space_debug.is_fallback_trajectory()) { + // auto* collision_line = chart->add_line(); + // collision_line->set_label("FutureCollisionPoint"); + // auto* future_collision_point = collision_line->add_point(); + // future_collision_point->set_x( + // open_space_debug.future_collision_point().path_point().x()); + // future_collision_point->set_y( + // open_space_debug.future_collision_point().path_point().y()); + // // Set chartJS's dataset properties + // auto* collision_properties = collision_line->mutable_properties(); + // (*collision_properties)["borderWidth"] = "3"; + // (*collision_properties)["pointRadius"] = "8"; + // (*collision_properties)["lineTension"] = "0"; + // (*collision_properties)["fill"] = "true"; + // (*stitching_properties)["showLine"] = "true"; + // (*stitching_properties)["pointStyle"] = "cross"; + + // const auto& fallback_trajectories = + // open_space_debug.fallback_trajectory().trajectory(); + // if (fallback_trajectories.empty() || + // fallback_trajectories[0].trajectory_point().empty()) { + // return; + // } + // const auto& fallback_trajectory = fallback_trajectories[0]; + // // has to define chart boundary first + // auto* fallback_line = chart->add_line(); + // fallback_line->set_label("Fallback"); + // for (const auto& point : fallback_trajectory.trajectory_point()) { + // auto* point_debug = fallback_line->add_point(); + // point_debug->set_x(point.path_point().x()); + // point_debug->set_y(point.path_point().y()); + // } + // // Set chartJS's dataset properties + // auto* fallback_properties = fallback_line->mutable_properties(); + // (*fallback_properties)["borderWidth"] = "3"; + // (*fallback_properties)["pointRadius"] = "2"; + // (*fallback_properties)["lineTension"] = "0"; + // (*fallback_properties)["fill"] = "false"; + // (*fallback_properties)["showLine"] = "true"; + // } } void OnLanePlanning::AddStitchSpeedProfile( planning_internal::Debug* debug_chart) { - if (!injector_->frame_history()->Latest()) { - AINFO << "Planning frame is empty!"; - return; - } + if (!injector_->frame_history()->Latest()) { + AINFO << "Planning frame is empty!"; + return; + } - // if open space info provider success run - if (!frame_->open_space_info().open_space_provider_success()) { - return; - } + // if open space info provider success run + if (!frame_->open_space_info().open_space_provider_success()) { + return; + } - auto chart = debug_chart->mutable_planning_data()->add_chart(); - chart->set_title("Open Space Speed Plan Visualization"); - auto* options = chart->mutable_options(); - // options->mutable_x()->set_mid_value(Clock::NowInSeconds()); - double xmin(std::numeric_limits::max()), - xmax(std::numeric_limits::lowest()), - ymin(std::numeric_limits::max()), - ymax(std::numeric_limits::lowest()); - // auto smoothed_trajectory = open_space_debug.smoothed_trajectory(); - auto* speed_profile = chart->add_line(); - speed_profile->set_label("Speed Profile"); - const auto& last_trajectory = - injector_->frame_history()->Latest()->current_frame_planned_trajectory(); - for (const auto& point : last_trajectory.trajectory_point()) { - auto* point_debug = speed_profile->add_point(); - point_debug->set_x(point.relative_time() + - last_trajectory.header().timestamp_sec()); - point_debug->set_y(point.v()); - if (point_debug->x() > xmax) xmax = point_debug->x(); - if (point_debug->x() < xmin) xmin = point_debug->x(); - if (point_debug->y() > ymax) ymax = point_debug->y(); - if (point_debug->y() < ymin) ymin = point_debug->y(); - } - options->mutable_x()->set_window_size(xmax - xmin); - options->mutable_x()->set_label_string("time (s)"); - options->mutable_y()->set_min(ymin); - options->mutable_y()->set_max(ymax); - options->mutable_y()->set_label_string("speed (m/s)"); - // Set chartJS's dataset properties - auto* speed_profile_properties = speed_profile->mutable_properties(); - (*speed_profile_properties)["borderWidth"] = "2"; - (*speed_profile_properties)["pointRadius"] = "0"; - (*speed_profile_properties)["lineTension"] = "0"; - (*speed_profile_properties)["fill"] = "false"; - (*speed_profile_properties)["showLine"] = "true"; + auto chart = debug_chart->mutable_planning_data()->add_chart(); + chart->set_title("Open Space Speed Plan Visualization"); + auto* options = chart->mutable_options(); + // options->mutable_x()->set_mid_value(Clock::NowInSeconds()); + double xmin(std::numeric_limits::max()), + xmax(std::numeric_limits::lowest()), + ymin(std::numeric_limits::max()), + ymax(std::numeric_limits::lowest()); + // auto smoothed_trajectory = open_space_debug.smoothed_trajectory(); + auto* speed_profile = chart->add_line(); + speed_profile->set_label("Speed Profile"); + const auto& last_trajectory = injector_->frame_history() + ->Latest() + ->current_frame_planned_trajectory(); + for (const auto& point : last_trajectory.trajectory_point()) { + auto* point_debug = speed_profile->add_point(); + point_debug->set_x(point.relative_time() + + last_trajectory.header().timestamp_sec()); + point_debug->set_y(point.v()); + if (point_debug->x() > xmax) xmax = point_debug->x(); + if (point_debug->x() < xmin) xmin = point_debug->x(); + if (point_debug->y() > ymax) ymax = point_debug->y(); + if (point_debug->y() < ymin) ymin = point_debug->y(); + } + options->mutable_x()->set_window_size(xmax - xmin); + options->mutable_x()->set_label_string("time (s)"); + options->mutable_y()->set_min(ymin); + options->mutable_y()->set_max(ymax); + options->mutable_y()->set_label_string("speed (m/s)"); + // Set chartJS's dataset properties + auto* speed_profile_properties = speed_profile->mutable_properties(); + (*speed_profile_properties)["borderWidth"] = "2"; + (*speed_profile_properties)["pointRadius"] = "0"; + (*speed_profile_properties)["lineTension"] = "0"; + (*speed_profile_properties)["fill"] = "false"; + (*speed_profile_properties)["showLine"] = "true"; } void OnLanePlanning::AddPublishedSpeed(const ADCTrajectory& trajectory_pb, planning_internal::Debug* debug_chart) { - // if open space info provider success run - if (!frame_->open_space_info().open_space_provider_success()) { - return; - } - - auto chart = debug_chart->mutable_planning_data()->add_chart(); - chart->set_title("Speed Partition Visualization"); - auto* options = chart->mutable_options(); - // options->mutable_x()->set_mid_value(Clock::NowInSeconds()); - // auto smoothed_trajectory = open_space_debug.smoothed_trajectory(); - auto* speed_profile = chart->add_line(); - speed_profile->set_label("Speed Profile"); - double xmin(std::numeric_limits::max()), - xmax(std::numeric_limits::lowest()), - ymin(std::numeric_limits::max()), - ymax(std::numeric_limits::lowest()); - for (const auto& point : trajectory_pb.trajectory_point()) { - auto* point_debug = speed_profile->add_point(); - point_debug->set_x(point.relative_time() + - trajectory_pb.header().timestamp_sec()); - if (trajectory_pb.gear() == canbus::Chassis::GEAR_DRIVE) { - point_debug->set_y(point.v()); - } - if (trajectory_pb.gear() == canbus::Chassis::GEAR_REVERSE) { - point_debug->set_y(-point.v()); - } - if (point_debug->x() > xmax) xmax = point_debug->x(); - if (point_debug->x() < xmin) xmin = point_debug->x(); - if (point_debug->y() > ymax) ymax = point_debug->y(); - if (point_debug->y() < ymin) ymin = point_debug->y(); - } - options->mutable_x()->set_window_size(xmax - xmin); - options->mutable_x()->set_label_string("time (s)"); - options->mutable_y()->set_min(ymin); - options->mutable_y()->set_max(ymax); - options->mutable_y()->set_label_string("speed (m/s)"); - // Set chartJS's dataset properties - auto* speed_profile_properties = speed_profile->mutable_properties(); - (*speed_profile_properties)["borderWidth"] = "2"; - (*speed_profile_properties)["pointRadius"] = "0"; - (*speed_profile_properties)["lineTension"] = "0"; - (*speed_profile_properties)["fill"] = "false"; - (*speed_profile_properties)["showLine"] = "true"; - - auto* sliding_line = chart->add_line(); - sliding_line->set_label("Time"); - - auto* point_debug_up = sliding_line->add_point(); - point_debug_up->set_x(Clock::NowInSeconds()); - point_debug_up->set_y(2.1); - auto* point_debug_down = sliding_line->add_point(); - point_debug_down->set_x(Clock::NowInSeconds()); - point_debug_down->set_y(-1.1); + // if open space info provider success run + if (!frame_->open_space_info().open_space_provider_success()) { + return; + } - // Set chartJS's dataset properties - auto* sliding_line_properties = sliding_line->mutable_properties(); - (*sliding_line_properties)["borderWidth"] = "2"; - (*sliding_line_properties)["pointRadius"] = "0"; - (*sliding_line_properties)["lineTension"] = "0"; - (*sliding_line_properties)["fill"] = "false"; - (*sliding_line_properties)["showLine"] = "true"; + auto chart = debug_chart->mutable_planning_data()->add_chart(); + chart->set_title("Speed Partition Visualization"); + auto* options = chart->mutable_options(); + // options->mutable_x()->set_mid_value(Clock::NowInSeconds()); + // auto smoothed_trajectory = open_space_debug.smoothed_trajectory(); + auto* speed_profile = chart->add_line(); + speed_profile->set_label("Speed Profile"); + double xmin(std::numeric_limits::max()), + xmax(std::numeric_limits::lowest()), + ymin(std::numeric_limits::max()), + ymax(std::numeric_limits::lowest()); + for (const auto& point : trajectory_pb.trajectory_point()) { + auto* point_debug = speed_profile->add_point(); + point_debug->set_x(point.relative_time() + + trajectory_pb.header().timestamp_sec()); + if (trajectory_pb.gear() == canbus::Chassis::GEAR_DRIVE) { + point_debug->set_y(point.v()); + } + if (trajectory_pb.gear() == canbus::Chassis::GEAR_REVERSE) { + point_debug->set_y(-point.v()); + } + if (point_debug->x() > xmax) xmax = point_debug->x(); + if (point_debug->x() < xmin) xmin = point_debug->x(); + if (point_debug->y() > ymax) ymax = point_debug->y(); + if (point_debug->y() < ymin) ymin = point_debug->y(); + } + options->mutable_x()->set_window_size(xmax - xmin); + options->mutable_x()->set_label_string("time (s)"); + options->mutable_y()->set_min(ymin); + options->mutable_y()->set_max(ymax); + options->mutable_y()->set_label_string("speed (m/s)"); + // Set chartJS's dataset properties + auto* speed_profile_properties = speed_profile->mutable_properties(); + (*speed_profile_properties)["borderWidth"] = "2"; + (*speed_profile_properties)["pointRadius"] = "0"; + (*speed_profile_properties)["lineTension"] = "0"; + (*speed_profile_properties)["fill"] = "false"; + (*speed_profile_properties)["showLine"] = "true"; + + auto* sliding_line = chart->add_line(); + sliding_line->set_label("Time"); + + auto* point_debug_up = sliding_line->add_point(); + point_debug_up->set_x(Clock::NowInSeconds()); + point_debug_up->set_y(2.1); + auto* point_debug_down = sliding_line->add_point(); + point_debug_down->set_x(Clock::NowInSeconds()); + point_debug_down->set_y(-1.1); + + // Set chartJS's dataset properties + auto* sliding_line_properties = sliding_line->mutable_properties(); + (*sliding_line_properties)["borderWidth"] = "2"; + (*sliding_line_properties)["pointRadius"] = "0"; + (*sliding_line_properties)["lineTension"] = "0"; + (*sliding_line_properties)["fill"] = "false"; + (*sliding_line_properties)["showLine"] = "true"; } VehicleState OnLanePlanning::AlignTimeStamp(const VehicleState& vehicle_state, const double curr_timestamp) const { - // TODO(Jinyun): use the same method in trajectory stitching - // for forward prediction - auto future_xy = injector_->vehicle_state()->EstimateFuturePosition( - curr_timestamp - vehicle_state.timestamp()); - - VehicleState aligned_vehicle_state = vehicle_state; - aligned_vehicle_state.set_x(future_xy.x()); - aligned_vehicle_state.set_y(future_xy.y()); - aligned_vehicle_state.set_timestamp(curr_timestamp); - return aligned_vehicle_state; + // TODO(Jinyun): use the same method in trajectory stitching + // for forward prediction + auto future_xy = injector_->vehicle_state()->EstimateFuturePosition( + curr_timestamp - vehicle_state.timestamp()); + + VehicleState aligned_vehicle_state = vehicle_state; + aligned_vehicle_state.set_x(future_xy.x()); + aligned_vehicle_state.set_y(future_xy.y()); + aligned_vehicle_state.set_timestamp(curr_timestamp); + return aligned_vehicle_state; } void OnLanePlanning::AddPublishedAcceleration( const ADCTrajectory& trajectory_pb, planning_internal::Debug* debug) { - // if open space info provider success run - if (!frame_->open_space_info().open_space_provider_success()) { - return; - } - double xmin(std::numeric_limits::max()), - xmax(std::numeric_limits::lowest()), - ymin(std::numeric_limits::max()), - ymax(std::numeric_limits::lowest()); - auto chart = debug->mutable_planning_data()->add_chart(); - chart->set_title("Acceleration Partition Visualization"); - auto* options = chart->mutable_options(); - // options->mutable_x()->set_mid_value(Clock::NowInSeconds()); - - auto* acceleration_profile = chart->add_line(); - acceleration_profile->set_label("Acceleration Profile"); - for (const auto& point : trajectory_pb.trajectory_point()) { - auto* point_debug = acceleration_profile->add_point(); - point_debug->set_x(point.relative_time() + - trajectory_pb.header().timestamp_sec()); - if (trajectory_pb.gear() == canbus::Chassis::GEAR_DRIVE) - point_debug->set_y(point.a()); - if (trajectory_pb.gear() == canbus::Chassis::GEAR_REVERSE) - point_debug->set_y(-point.a()); - if (point_debug->x() > xmax) xmax = point_debug->x(); - if (point_debug->x() < xmin) xmin = point_debug->x(); - if (point_debug->y() > ymax) ymax = point_debug->y(); - if (point_debug->y() < ymin) ymin = point_debug->y(); - } - options->mutable_x()->set_window_size(xmax - xmin); - options->mutable_x()->set_label_string("time (s)"); - options->mutable_y()->set_min(ymin); - options->mutable_y()->set_max(ymax); - options->mutable_y()->set_label_string("acceleration (m/s)"); - // Set chartJS's dataset properties - auto* acceleration_profile_properties = - acceleration_profile->mutable_properties(); - (*acceleration_profile_properties)["borderWidth"] = "2"; - (*acceleration_profile_properties)["pointRadius"] = "0"; - (*acceleration_profile_properties)["lineTension"] = "0"; - (*acceleration_profile_properties)["fill"] = "false"; - (*acceleration_profile_properties)["showLine"] = "true"; - - auto* sliding_line = chart->add_line(); - sliding_line->set_label("Time"); - - auto* point_debug_up = sliding_line->add_point(); - point_debug_up->set_x(Clock::NowInSeconds()); - point_debug_up->set_y(2.1); - auto* point_debug_down = sliding_line->add_point(); - point_debug_down->set_x(Clock::NowInSeconds()); - point_debug_down->set_y(-1.1); - - // Set chartJS's dataset properties - auto* sliding_line_properties = sliding_line->mutable_properties(); - (*sliding_line_properties)["borderWidth"] = "2"; - (*sliding_line_properties)["pointRadius"] = "0"; - (*sliding_line_properties)["lineTension"] = "0"; - (*sliding_line_properties)["fill"] = "false"; - (*sliding_line_properties)["showLine"] = "true"; + // if open space info provider success run + if (!frame_->open_space_info().open_space_provider_success()) { + return; + } + double xmin(std::numeric_limits::max()), + xmax(std::numeric_limits::lowest()), + ymin(std::numeric_limits::max()), + ymax(std::numeric_limits::lowest()); + auto chart = debug->mutable_planning_data()->add_chart(); + chart->set_title("Acceleration Partition Visualization"); + auto* options = chart->mutable_options(); + // options->mutable_x()->set_mid_value(Clock::NowInSeconds()); + + auto* acceleration_profile = chart->add_line(); + acceleration_profile->set_label("Acceleration Profile"); + for (const auto& point : trajectory_pb.trajectory_point()) { + auto* point_debug = acceleration_profile->add_point(); + point_debug->set_x(point.relative_time() + + trajectory_pb.header().timestamp_sec()); + if (trajectory_pb.gear() == canbus::Chassis::GEAR_DRIVE) + point_debug->set_y(point.a()); + if (trajectory_pb.gear() == canbus::Chassis::GEAR_REVERSE) + point_debug->set_y(-point.a()); + if (point_debug->x() > xmax) xmax = point_debug->x(); + if (point_debug->x() < xmin) xmin = point_debug->x(); + if (point_debug->y() > ymax) ymax = point_debug->y(); + if (point_debug->y() < ymin) ymin = point_debug->y(); + } + options->mutable_x()->set_window_size(xmax - xmin); + options->mutable_x()->set_label_string("time (s)"); + options->mutable_y()->set_min(ymin); + options->mutable_y()->set_max(ymax); + options->mutable_y()->set_label_string("acceleration (m/s)"); + // Set chartJS's dataset properties + auto* acceleration_profile_properties = + acceleration_profile->mutable_properties(); + (*acceleration_profile_properties)["borderWidth"] = "2"; + (*acceleration_profile_properties)["pointRadius"] = "0"; + (*acceleration_profile_properties)["lineTension"] = "0"; + (*acceleration_profile_properties)["fill"] = "false"; + (*acceleration_profile_properties)["showLine"] = "true"; + + auto* sliding_line = chart->add_line(); + sliding_line->set_label("Time"); + + auto* point_debug_up = sliding_line->add_point(); + point_debug_up->set_x(Clock::NowInSeconds()); + point_debug_up->set_y(2.1); + auto* point_debug_down = sliding_line->add_point(); + point_debug_down->set_x(Clock::NowInSeconds()); + point_debug_down->set_y(-1.1); + + // Set chartJS's dataset properties + auto* sliding_line_properties = sliding_line->mutable_properties(); + (*sliding_line_properties)["borderWidth"] = "2"; + (*sliding_line_properties)["pointRadius"] = "0"; + (*sliding_line_properties)["lineTension"] = "0"; + (*sliding_line_properties)["fill"] = "false"; + (*sliding_line_properties)["showLine"] = "true"; } -} // namespace planning + } // namespace planning } // namespace apollo diff --git a/modules/planning/planning_component/planning_base.cc b/modules/planning/planning_component/planning_base.cc index 51436669f94..6a2fae65287 100644 --- a/modules/planning/planning_component/planning_base.cc +++ b/modules/planning/planning_component/planning_base.cc @@ -113,6 +113,7 @@ void PlanningBase::FillPlanningPb(const double timestamp, void PlanningBase::LoadPlanner() { // Use PublicRoadPlanner as default Planner std::string planner_name = "apollo::planning::PublicRoadPlanner"; + // 读取配置文件的设置 if ("" != config_.planner()) { planner_name = config_.planner(); planner_name = ConfigUtil::GetFullPlanningClassName(planner_name); diff --git a/modules/planning/planning_component/planning_base.h b/modules/planning/planning_component/planning_base.h index 31139b39172..ee890880fe3 100644 --- a/modules/planning/planning_component/planning_base.h +++ b/modules/planning/planning_component/planning_base.h @@ -98,7 +98,7 @@ class PlanningBase { PlanningConfig config_; TrafficDecider traffic_decider_; std::unique_ptr frame_; - std::shared_ptr planner_; + std::shared_ptr planner_; // 实际调用的规划器，会被继承使用 std::unique_ptr last_publishable_trajectory_; std::shared_ptr injector_; }; diff --git a/modules/planning/planning_component/planning_component.cc b/modules/planning/planning_component/planning_component.cc index ad7850c3d16..bf64925e958 100644 --- a/modules/planning/planning_component/planning_component.cc +++ b/modules/planning/planning_component/planning_component.cc @@ -40,7 +40,7 @@ using apollo::storytelling::Stories; bool PlanningComponent::Init() { injector_ = std::make_shared(); - if (FLAGS_use_navigation_mode) { + if (FLAGS_use_navigation_mode) { // OnLanePlanning是默认规划器 planning_base_ = std::make_unique(injector_); } else { planning_base_ = std::make_unique(injector_); @@ -60,6 +60,7 @@ bool PlanningComponent::Init() { planning_base_->Init(config_); + // 订阅外部指令 planning_command_reader_ = node_->CreateReader( config_.topic_config().planning_command_topic(), [this](const std::shared_ptr& planning_command) { @@ -69,6 +70,7 @@ bool PlanningComponent::Init() { planning_command_.CopyFrom(*planning_command); }); + // 订阅红绿灯消息 traffic_light_reader_ = node_->CreateReader( config_.topic_config().traffic_light_detection_topic(), [this](const std::shared_ptr& traffic_light) { @@ -77,6 +79,7 @@ bool PlanningComponent::Init() { traffic_light_.CopyFrom(*traffic_light); }); + // 订阅planning流程干预消息 pad_msg_reader_ = node_->CreateReader( config_.topic_config().planning_pad_topic(), [this](const std::shared_ptr& pad_msg) { @@ -102,15 +105,19 @@ bool PlanningComponent::Init() { relative_map_.CopyFrom(*map_message); }); } + // 创建planning输出轨迹发布者 planning_writer_ = node_->CreateWriter( config_.topic_config().planning_trajectory_topic()); + // 道路被阻塞时重规划的client rerouting_client_ = node_->CreateClient( config_.topic_config().routing_request_topic()); planning_learning_data_writer_ = node_->CreateWriter( config_.topic_config().planning_learning_data_topic()); + + // 创建planning导航命令实时执行状态发布者 command_status_writer_ = node_->CreateWriter( FLAGS_planning_command_status); return true; @@ -125,9 +132,11 @@ bool PlanningComponent::Proc( ACHECK(prediction_obstacles != nullptr); // check and process possible rerouting request + // 检查是否需要重新规划线路 CheckRerouting(); // process fused input data + // 数据存入local_view_并且检查 local_view_.prediction_obstacles = prediction_obstacles; local_view_.chassis = chassis; local_view_.localization_estimate = localization_estimate; @@ -200,7 +209,8 @@ bool PlanningComponent::Proc( } ADCTrajectory adc_trajectory_pb; - planning_base_->RunOnce(local_view_, &adc_trajectory_pb); + // 执行注册好的planner + planning_base_->RunOnce(local_view_, &adc_trajectory_pb); // 开始规划 auto start_time = adc_trajectory_pb.header().timestamp_sec(); common::util::FillHeader(node_->Name(), &adc_trajectory_pb); @@ -209,8 +219,10 @@ bool PlanningComponent::Proc( for (auto& p : *adc_trajectory_pb.mutable_trajectory_point()) { p.set_relative_time(p.relative_time() + dt); } + // 发布路线消息 planning_writer_->Write(adc_trajectory_pb); + //执行状态反馈 // Send command execution feedback. // Error occured while executing the command. external_command::CommandStatus command_status; diff --git a/modules/planning/planning_interface_base/scenario_base/scenario.cc b/modules/planning/planning_interface_base/scenario_base/scenario.cc index 29ee79fd215..429d6e1ab65 100644 --- a/modules/planning/planning_interface_base/scenario_base/scenario.cc +++ b/modules/planning/planning_interface_base/scenario_base/scenario.cc @@ -83,11 +83,14 @@ bool Scenario::Init(std::shared_ptr injector, return true; } +// 处理场景的主函数，根据当前的stage和轨迹点来执行任务 ScenarioResult Scenario::Process( const common::TrajectoryPoint& planning_init_point, Frame* frame) { + //若当前stage为空，创建初始stage if (current_stage_ == nullptr) { current_stage_ = CreateStage( *stage_pipeline_map_[scenario_pipeline_config_.stage(0).name()]); + //创建失败 if (nullptr == current_stage_) { AERROR << "Create stage " << scenario_pipeline_config_.stage(0).name() << " failed!"; @@ -96,22 +99,27 @@ ScenarioResult Scenario::Process( } AINFO << "Create stage " << current_stage_->Name(); } + // 如果当前阶段为空名，则表示场景已完成 if (current_stage_->Name().empty()) { scenario_result_.SetScenarioStatus(ScenarioStatusType::STATUS_DONE); return scenario_result_; } + // 处理当前阶段并获取返回结果 auto ret = current_stage_->Process(planning_init_point, frame); scenario_result_.SetStageResult(ret); + // 根据返回结果来更新场景和阶段的状态 switch (ret.GetStageStatus()) { case StageStatusType::ERROR: { AERROR << "Stage '" << current_stage_->Name() << "' returns error"; scenario_result_.SetScenarioStatus(ScenarioStatusType::STATUS_UNKNOWN); break; } + //下个周期继续执行当前stage case StageStatusType::RUNNING: { scenario_result_.SetScenarioStatus(ScenarioStatusType::STATUS_PROCESSING); break; } + //切换下一个stage case StageStatusType::FINISHED: { auto next_stage = current_stage_->NextStage(); if (next_stage != current_stage_->Name()) { diff --git a/modules/planning/planning_interface_base/task_base/common/path_util/path_optimizer_util.cc b/modules/planning/planning_interface_base/task_base/common/path_util/path_optimizer_util.cc index e69cfe88dde..d393becf588 100644 --- a/modules/planning/planning_interface_base/task_base/common/path_util/path_optimizer_util.cc +++ b/modules/planning/planning_interface_base/task_base/common/path_util/path_optimizer_util.cc @@ -93,21 +93,45 @@ PathOptimizerUtil::ConvertPathPointRefFromFrontAxeToRearAxe( return ret; } +// 该函数在 PathOptimizerUtil 命名空间中，用于对路径进行优化 bool PathOptimizerUtil::OptimizePath( - const SLState& init_state, const std::array& end_state, - std::vector l_ref, std::vector l_ref_weight, + // 初始状态：横向位置、横向速度、横向加速度 + const SLState& init_state, + // 期望的终点状态：横向位置、横向速度、横向加速度 + const std::array& end_state, + // 参考横向位置 + std::vector l_ref, + // 参考横向位置的权重 + std::vector l_ref_weight, + // 路径边界 const PathBoundary& path_boundary, - const std::vector>& ddl_bounds, double dddl_bound, - const PiecewiseJerkPathConfig& config, std::vector* x, - std::vector* dx, std::vector* ddx) { - // num of knots + // 横向加速度的边界 + const std::vector>& ddl_bounds, + // 横向加加速度的最大值 + double dddl_bound, + // 路径优化的配置 + const PiecewiseJerkPathConfig& config, + // 优化后的横向位置 + std::vector* x, + // 优化后的横向速度 + std::vector* dx, + // 优化后的横向加速度 + std::vector* ddx) { + // 路径点的数量 const auto& lat_boundaries = path_boundary.boundary(); const size_t kNumKnots = lat_boundaries.size(); + // 路径点之间的间隔 double delta_s = path_boundary.delta_s(); + + // 路径优化 PiecewiseJerkPathProblem piecewise_jerk_problem(kNumKnots, delta_s, init_state.second); + + // 用于调试的类，用于记录路径点 PrintCurves print_curve; + + // 记录路径点 for (size_t i = 0; i < kNumKnots; i++) { print_curve.AddPoint(path_boundary.label() + "_ref_l", i * path_boundary.delta_s(), l_ref[i]); @@ -122,56 +146,80 @@ bool PathOptimizerUtil::OptimizePath( print_curve.AddPoint(path_boundary.label() + "_ddl_upper", i * path_boundary.delta_s(), ddl_bounds[i].second); } + + // 记录初始状态 print_curve.AddPoint(path_boundary.label() + "_opt_l", 0, init_state.second[0]); print_curve.AddPoint(path_boundary.label() + "_opt_dl", 0, init_state.second[1]); print_curve.AddPoint(path_boundary.label() + "_opt_ddl", 0, init_state.second[2]); - // TODO(Hongyi): update end_state settings + + // TODO(Hongyi): 更新 end_state 设置 std::array end_state_weight = {config.weight_end_state_l(), config.weight_end_state_dl(), config.weight_end_state_ddl()}; + + // 设置期望的终点状态 piecewise_jerk_problem.set_end_state_ref(end_state_weight, end_state); + + // 设置参考横向位置 piecewise_jerk_problem.set_x_ref(std::move(l_ref_weight), l_ref); - // for debug:here should use std::move + + // 用于调试：在此处使用 std::move piecewise_jerk_problem.set_weight_x(config.l_weight()); piecewise_jerk_problem.set_weight_dx(config.dl_weight()); piecewise_jerk_problem.set_weight_ddx(config.ddl_weight()); piecewise_jerk_problem.set_weight_dddx(config.dddl_weight()); + // 设置缩放因子 状态的量级 piecewise_jerk_problem.set_scale_factor({1.0, 10.0, 100.0}); + // 记录开始时间 auto start_time = std::chrono::system_clock::now(); + // 设置横向位置的边界 piecewise_jerk_problem.set_x_bounds(lat_boundaries); + + // 设置横向速度的边界 piecewise_jerk_problem.set_dx_bounds( -config.lateral_derivative_bound_default(), config.lateral_derivative_bound_default()); + + // 设置横向加速度的边界 piecewise_jerk_problem.set_ddx_bounds(ddl_bounds); + // 设置横向加加速度的最大值 piecewise_jerk_problem.set_dddx_bound(dddl_bound); + // OSQP进行路径优化 bool success = piecewise_jerk_problem.Optimize(config.max_iteration()); + // 记录结束时间 auto end_time = std::chrono::system_clock::now(); + + // 计算路径优化所用的时间 std::chrono::duration diff = end_time - start_time; - ADEBUG << "Path Optimizer used time: " << diff.count() * 1000 << " ms."; + ADEBUG << "路径优化器所用时间: " << diff.count() * 1000 << " ms."; + // 如果路径优化失败，返回 false if (!success) { - AERROR << path_boundary.label() << "piecewise jerk path optimizer failed"; - AINFO << "init s(" << init_state.first[0] << "," << init_state.first[1] + AERROR << path_boundary.label() << "分段三次曲线路径优化器失败"; + AINFO << "初始状态：s(" << init_state.first[0] << "," << init_state.first[1] << "," << init_state.first[2] << ") l (" << init_state.second[0] << "," << init_state.second[1] << "," << init_state.second[2]; - AINFO << "dx bound" << config.lateral_derivative_bound_default(); - AINFO << "jerk bound" << dddl_bound; + AINFO << "dx 边界: " << config.lateral_derivative_bound_default(); + AINFO << "jerk 边界: " << dddl_bound; print_curve.PrintToLog(); return false; } + // 记录优化后的路径点 *x = piecewise_jerk_problem.opt_x(); *dx = piecewise_jerk_problem.opt_dx(); *ddx = piecewise_jerk_problem.opt_ddx(); + + // 记录优化后的路径点 for (size_t i = 0; i < kNumKnots; i++) { print_curve.AddPoint(path_boundary.label() + "_opt_l", i * path_boundary.delta_s(), (*x)[i]); @@ -180,7 +228,11 @@ bool PathOptimizerUtil::OptimizePath( print_curve.AddPoint(path_boundary.label() + "_opt_ddl", i * path_boundary.delta_s(), (*ddx)[i]); } + + // 打印并记录路径点 print_curve.PrintToLog(); + + // 如果路径优化成功，返回 true return true; } diff --git a/modules/planning/scenarios/lane_follow/lane_follow_scenario.cc b/modules/planning/scenarios/lane_follow/lane_follow_scenario.cc index 59bab1cf954..56dc8629d5b 100644 --- a/modules/planning/scenarios/lane_follow/lane_follow_scenario.cc +++ b/modules/planning/scenarios/lane_follow/lane_follow_scenario.cc @@ -26,11 +26,14 @@ namespace apollo { namespace planning { +// 场景切换器中调用，判断是否可以切换到此场景，返回true则切换 bool LaneFollowScenario::IsTransferable(const Scenario* other_scenario, const Frame& frame) { + //frame是否包含lane_follow if (!frame.local_view().planning_command->has_lane_follow_command()) { return false; } + //是否有参考线 if (frame.reference_line_info().empty()) { return false; } diff --git a/modules/planning/scenarios/lane_follow/lane_follow_stage.cc b/modules/planning/scenarios/lane_follow/lane_follow_stage.cc index 6d7223154d0..6e3e5c4f63c 100644 --- a/modules/planning/scenarios/lane_follow/lane_follow_stage.cc +++ b/modules/planning/scenarios/lane_follow/lane_follow_stage.cc @@ -78,56 +78,73 @@ void LaneFollowStage::RecordObstacleDebugInfo( } } } - +// lane_follow场景的执行函数 +// 处理车辆在规划路径上的行为，包括跟随车道线、变道等 StageResult LaneFollowStage::Process( const TrajectoryPoint& planning_start_point, Frame* frame) { + // 如果没有可用的参考线信息，返回任务完成 if (frame->reference_line_info().empty()) { return StageResult(StageStatusType::FINISHED); } + // 标记是否存在可行驶的参考线 bool has_drivable_reference_line = false; + // 调试信息：输出参考线数量 ADEBUG << "Number of reference lines:\t" << frame->mutable_reference_line_info()->size(); unsigned int count = 0; StageResult result; + // 依次处理每条参考线 for (auto& reference_line_info : *frame->mutable_reference_line_info()) { // TODO(SHU): need refactor + // 若已处理到最后一条参考线，跳出循环 if (count++ == frame->mutable_reference_line_info()->size()) { break; } ADEBUG << "No: [" << count << "] Reference Line."; ADEBUG << "IsChangeLanePath: " << reference_line_info.IsChangeLanePath(); + // 若已存在可行驶的参考线，将后续参考线标记为不可行驶并跳出循环 if (has_drivable_reference_line) { reference_line_info.SetDrivable(false); break; } + // 基于当前参考线进行路径和速度规划 result = PlanOnReferenceLine(planning_start_point, frame, &reference_line_info); + // 若路径和速度规划没有出错 if (!result.HasError()) { + // 若当前参考线不是变道路径 if (!reference_line_info.IsChangeLanePath()) { + // 调试信息：输出当前参考线不是变道路径 ADEBUG << "reference line is NOT lane change ref."; + // 标记当前参考线为可行驶 has_drivable_reference_line = true; continue; } + // 若当前参考线的代价小于指定阈值 if (reference_line_info.Cost() < kStraightForwardLineCost) { // If the path and speed optimization succeed on target lane while // under smart lane-change or IsClearToChangeLane under older version + // 若路径和速度优化在目标车道上成功，标记当前参考线为可行驶 has_drivable_reference_line = true; reference_line_info.SetDrivable(true); } else { + // 若路径和速度优化在目标车道上失败，标记当前参考线为不可行驶 reference_line_info.SetDrivable(false); ADEBUG << "\tlane change failed"; } } else { + // 若路径和速度规划出错，标记当前参考线为不可行驶 reference_line_info.SetDrivable(false); } } + // 返回任务状态：若存在可行驶的参考线，返回任务运行中状态；否则返回任务出错状态 return has_drivable_reference_line ? result.SetStageStatus(StageStatusType::RUNNING) : result.SetStageStatus(StageStatusType::ERROR); @@ -136,6 +153,8 @@ StageResult LaneFollowStage::Process( StageResult LaneFollowStage::PlanOnReferenceLine( const TrajectoryPoint& planning_start_point, Frame* frame, ReferenceLineInfo* reference_line_info) { + // 判断是否当前参考线是可换道的车道，如果不是那么增加cost + // RULE_BASED_STOP_DECIDER之后是速度相关 if (!reference_line_info->IsChangeLanePath()) { reference_line_info->AddCost(kStraightForwardLineCost); } @@ -150,9 +169,9 @@ StageResult LaneFollowStage::PlanOnReferenceLine( std::chrono::duration( std::chrono::system_clock::now().time_since_epoch()) .count(); - + // 设置任务状态 ret.SetTaskStatus(task->Execute(frame, reference_line_info)); - + // 记录调试信息 const double end_timestamp = Clock::NowInSeconds(); const double time_diff_ms = (end_timestamp - start_timestamp) * 1000; ADEBUG << "after task[" << task->Name() @@ -240,7 +259,7 @@ StageResult LaneFollowStage::PlanOnReferenceLine( } } } - + // 检查轨迹有效性 if (FLAGS_enable_trajectory_check) { if (ConstraintChecker::ValidTrajectory(trajectory) != ConstraintChecker::Result::VALID) { @@ -249,7 +268,7 @@ StageResult LaneFollowStage::PlanOnReferenceLine( return ret.SetStageStatus(StageStatusType::ERROR, msg); } } - + // 设置轨迹和可行使性 reference_line_info->SetTrajectory(trajectory); reference_line_info->SetDrivable(true); ret.SetStageStatus(StageStatusType::RUNNING); diff --git a/modules/planning/tasks/lane_change_path/lane_change_path.cc b/modules/planning/tasks/lane_change_path/lane_change_path.cc index 8249d924ab8..c7d6fc89c06 100644 --- a/modules/planning/tasks/lane_change_path/lane_change_path.cc +++ b/modules/planning/tasks/lane_change_path/lane_change_path.cc @@ -62,6 +62,7 @@ apollo::common::Status LaneChangePath::Process( ->mutable_planning_status() ->mutable_change_lane() ->status(); + // 检查是否变道 if (!reference_line_info->IsChangeLanePath() || reference_line_info->path_reusable()) { ADEBUG << "Skip this time" << reference_line_info->IsChangeLanePath() @@ -78,15 +79,17 @@ apollo::common::Status LaneChangePath::Process( } std::vector candidate_path_boundaries; std::vector candidate_path_data; - + // 尝试获取变道路径的边界和数据 GetStartPointSLState(); if (!DecidePathBounds(&candidate_path_boundaries)) { return Status(ErrorCode::PLANNING_ERROR, "lane change path bounds failed"); } + // 尝试优化变道路径 if (!OptimizePath(candidate_path_boundaries, &candidate_path_data)) { return Status(ErrorCode::PLANNING_ERROR, "lane change path optimize failed"); } + // 评估变道路径 if (!AssessPath(&candidate_path_data, reference_line_info->mutable_path_data())) { return Status(ErrorCode::PLANNING_ERROR, "No valid lane change path"); diff --git a/modules/planning/tasks/lane_follow_path/lane_follow_path.cc b/modules/planning/tasks/lane_follow_path/lane_follow_path.cc index 5ad0d7cbc16..97a9885edf8 100644 --- a/modules/planning/tasks/lane_follow_path/lane_follow_path.cc +++ b/modules/planning/tasks/lane_follow_path/lane_follow_path.cc @@ -55,15 +55,21 @@ apollo::common::Status LaneFollowPath::Process( std::vector candidate_path_boundaries; std::vector candidate_path_data; + // 基于参考线信息和初始SL状态来决定路径边界 GetStartPointSLState(); + + // 基于参考线信息和初始SL状态来决定路径边界。 if (!DecidePathBounds(&candidate_path_boundaries)) { AERROR << "Decide path bound failed"; return Status::OK(); } + // 基于候选路径边界来优化路径。 if (!OptimizePath(candidate_path_boundaries, &candidate_path_data)) { AERROR << "Optmize path failed"; return Status::OK(); } + + // 对候选路径进行评估并返回最优路径 if (!AssessPath(&candidate_path_data, reference_line_info->mutable_path_data())) { AERROR << "Path assessment failed"; @@ -102,15 +108,14 @@ bool LaneFollowPath::DecidePathBounds(std::vector* boundary) { *reference_line_info_, init_sl_state_, config_.extend_buffer(), &path_bound); } - PrintCurves print_curve; + PrintCurves print_curve; // 用于LOG auto indexed_obstacles = reference_line_info_->path_decision()->obstacles(); for (const auto* obs : indexed_obstacles.Items()) { const auto& sl_bound = obs->PerceptionSLBoundary(); for (int i = 0; i < sl_bound.boundary_point_size(); i++) { std::string name = obs->Id() + "_obs_sl_boundary"; - print_curve.AddPoint( - name, sl_bound.boundary_point(i).s(), - sl_bound.boundary_point(i).l()); + print_curve.AddPoint(name, sl_bound.boundary_point(i).s(), + sl_bound.boundary_point(i).l()); } } print_curve.PrintToLog(); @@ -136,8 +141,8 @@ bool LaneFollowPath::DecidePathBounds(std::vector* boundary) { // lane_follow_status update auto* lane_follow_status = injector_->planning_context() - ->mutable_planning_status() - ->mutable_lane_follow(); + ->mutable_planning_status() + ->mutable_lane_follow(); if (!blocking_obstacle_id.empty()) { double current_time = ::apollo::cyber::Clock::NowInSeconds(); lane_follow_status->set_block_obstacle_id(blocking_obstacle_id); @@ -167,8 +172,7 @@ bool LaneFollowPath::DecidePathBounds(std::vector* boundary) { init_sl_state_.second[0] < path_bound[0].l_lower.l) { AINFO << "not in self lane maybe lane borrow , init l : " << init_sl_state_.second[0] << ", path_bound l: [ " - << path_bound[0].l_lower.l << "," - << path_bound[0].l_upper.l << " ]"; + << path_bound[0].l_lower.l << "," << path_bound[0].l_upper.l << " ]"; return false; } // std::vector> regular_path_bound_pair; @@ -182,22 +186,43 @@ bool LaneFollowPath::DecidePathBounds(std::vector* boundary) { return true; } +// 该函数基于提供的路径边界来优化路径。 +// 它迭代遍历每个路径边界，使用 PathOptimizerUtil 计算最佳路径， +// 并将优化后的路径添加到 candidate_path_data 中。 +// 如果找不到有效的路径，则返回 false。 bool LaneFollowPath::OptimizePath( const std::vector& path_boundaries, std::vector* candidate_path_data) { + // 加载路径优化器的配置。 const auto& config = config_.path_optimizer_config(); + + // 获取参考线信息中的参考线。 const ReferenceLine& reference_line = reference_line_info_->reference_line(); + + // 初始化优化的结束状态。 std::array end_state = {0.0, 0.0, 0.0}; + + // 迭代计算每个路径边界。 for (const auto& path_boundary : path_boundaries) { + // 获取路径边界的大小。 size_t path_boundary_size = path_boundary.boundary().size(); + + // 检查路径边界是否有效。 if (path_boundary_size <= 1U) { - AERROR << "Get invalid path boundary with size: " << path_boundary_size; + AERROR << "Gets an invalid path boundary of magnitude: " + << path_boundary_size; return false; } + + // 初始化向量来存储优化后的路径。 std::vector opt_l, opt_dl, opt_ddl; + + // 计算加速度界限以进行优化。 std::vector> ddl_bounds; PathOptimizerUtil::CalculateAccBound(path_boundary, reference_line, &ddl_bounds); + + // 打印调试信息以获取参考线的 kappa。 PrintCurves print_debug; for (size_t i = 0; i < path_boundary_size; ++i) { double s = static_cast(i) * path_boundary.delta_s() + @@ -207,64 +232,101 @@ bool LaneFollowPath::OptimizePath( "ref_kappa", static_cast(i) * path_boundary.delta_s(), kappa); } print_debug.PrintToLog(); + + // 计算用于优化的加速度界限。 const double jerk_bound = PathOptimizerUtil::EstimateJerkBoundary( std::fmax(init_sl_state_.first[1], 1e-12)); + + // 初始化用于优化的参考路径。 std::vector ref_l(path_boundary_size, 0); std::vector weight_ref_l(path_boundary_size, 0); PathOptimizerUtil::UpdatePathRefWithBound( path_boundary, config.path_reference_l_weight(), &ref_l, &weight_ref_l); + + // 使用计算出的参数来优化路径。 bool res_opt = PathOptimizerUtil::OptimizePath( init_sl_state_, end_state, ref_l, weight_ref_l, path_boundary, ddl_bounds, jerk_bound, config, &opt_l, &opt_dl, &opt_ddl); + + // 如果优化成功，将优化后的路径添加到 candidate_path_data 中。 if (res_opt) { auto frenet_frame_path = PathOptimizerUtil::ToPiecewiseJerkPath( opt_l, opt_dl, opt_ddl, path_boundary.delta_s(), path_boundary.start_s()); + PathData path_data; path_data.SetReferenceLine(&reference_line); path_data.SetFrenetPath(std::move(frenet_frame_path)); + + // 如果在路径规划中使用前轴中心的标志被设置， + // 则将路径点引用从前轴转换为后轴。 if (FLAGS_use_front_axe_center_in_path_planning) { auto discretized_path = DiscretizedPath( PathOptimizerUtil::ConvertPathPointRefFromFrontAxeToRearAxe( path_data)); path_data.SetDiscretizedPath(discretized_path); } + + // 设置路径的标签和阻塞物体的 ID。 path_data.set_path_label(path_boundary.label()); path_data.set_blocking_obstacle_id(path_boundary.blocking_obstacle_id()); + candidate_path_data->push_back(std::move(path_data)); } } + + // 如果 candidate_path_data 为空，返回 false。 if (candidate_path_data->empty()) { return false; } + return true; } +// 路径评估函数，对候选路径进行评估并返回最优路径。 +// 它首先检查候选路径是否有效，然后对路径点进行决策并将其添加到路径数据中。 +// 最后，它将最优路径返回到 final_path 并更新参考线信息中的相关数据。 bool LaneFollowPath::AssessPath(std::vector* candidate_path_data, PathData* final_path) { + // 获取最后一个候选路径。 PathData& curr_path_data = candidate_path_data->back(); + + // 记录调试信息。 RecordDebugInfo(curr_path_data, curr_path_data.path_label(), reference_line_info_); + + // 评估候选路径是否有效。 if (!PathAssessmentDeciderUtil::IsValidRegularPath(*reference_line_info_, curr_path_data)) { AINFO << "Lane follow path is invalid"; return false; } + // 初始化路径点决策。 std::vector path_decision; PathAssessmentDeciderUtil::InitPathPointDecision( curr_path_data, PathData::PathPointType::IN_LANE, &path_decision); curr_path_data.SetPathPointDecisionGuide(std::move(path_decision)); + // 检查路径是否为空。 if (curr_path_data.Empty()) { AINFO << "Lane follow path is empty after trimed"; return false; } + + // 将最优路径复制到 final_path。 *final_path = curr_path_data; + + // 记录最优路径的标签和阻塞物体的 ID。 AINFO << final_path->path_label() << final_path->blocking_obstacle_id(); + + // 将最优路径添加到参考线信息中的候选路径数据中。 reference_line_info_->MutableCandidatePathData()->push_back(*final_path); + + // 更新参考线信息中的阻塞物体。 reference_line_info_->SetBlockingObstacle( curr_path_data.blocking_obstacle_id()); + return true; } diff --git a/modules/planning/tasks/path_time_heuristic/path_time_heuristic_optimizer.cc b/modules/planning/tasks/path_time_heuristic/path_time_heuristic_optimizer.cc index 9b518a34ba6..f8a9c593b51 100644 --- a/modules/planning/tasks/path_time_heuristic/path_time_heuristic_optimizer.cc +++ b/modules/planning/tasks/path_time_heuristic/path_time_heuristic_optimizer.cc @@ -60,28 +60,37 @@ bool PathTimeHeuristicOptimizer::SearchPathTimeGraph( return true; } +// 处理路径数据并生成速度数据 Status PathTimeHeuristicOptimizer::Process( const PathData& path_data, const common::TrajectoryPoint& init_point, SpeedData* const speed_data) { + // 保存初始点 init_point_ = init_point; + // 若路径数据为空，返回错误状态 if (path_data.discretized_path().empty()) { const std::string msg = "Empty path data"; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } + // ST以生成速度数据 if (!SearchPathTimeGraph(speed_data)) { const std::string msg = absl::StrCat( Name(), ": Failed to search graph with dynamic programming."); AERROR << msg; + // 记录调试信息 RecordDebugInfo(*speed_data, reference_line_info_->mutable_st_graph_data() ->mutable_st_graph_debug()); return Status(ErrorCode::PLANNING_ERROR, msg); } + + // 再次记录调试信息 RecordDebugInfo( *speed_data, reference_line_info_->mutable_st_graph_data()->mutable_st_graph_debug()); + + // 若一切正常，返回成功状态 return Status::OK(); } diff --git a/modules/planning/tasks/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.cc b/modules/planning/tasks/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.cc index 0e8b6a309ef..6662563db32 100644 --- a/modules/planning/tasks/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.cc +++ b/modules/planning/tasks/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.cc @@ -18,19 +18,21 @@ * @file piecewise_jerk_fallback_speed.cc **/ -#include +#include "modules/planning/tasks/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.h" +#include #include #include #include + #include "modules/common_msgs/basic_msgs/pnc_point.pb.h" + #include "modules/common/vehicle_state/vehicle_state_provider.h" -#include "modules/planning/planning_base/gflags/planning_gflags.h" #include "modules/planning/planning_base/common/speed_profile_generator.h" #include "modules/planning/planning_base/common/st_graph_data.h" #include "modules/planning/planning_base/common/util/print_debug_info.h" +#include "modules/planning/planning_base/gflags/planning_gflags.h" #include "modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_speed_problem.h" -#include "modules/planning/tasks/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.h" namespace apollo { namespace planning { @@ -55,18 +57,20 @@ bool PiecewiseJerkSpeedOptimizer::Init( Status PiecewiseJerkSpeedOptimizer::Process(const PathData& path_data, const TrajectoryPoint& init_point, SpeedData* const speed_data) { + // 检查是否到达终点，如果到达终点不再进行速度规划 if (reference_line_info_->ReachedDestination()) { return Status::OK(); } ACHECK(speed_data != nullptr); SpeedData reference_speed_data = *speed_data; - + // 检查路径是否为空 if (path_data.discretized_path().empty()) { const std::string msg = "Empty path data"; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } + // 获取信息 StGraphData& st_graph_data = *reference_line_info_->mutable_st_graph_data(); PrintCurves print_debug; const auto& veh_param = @@ -91,6 +95,7 @@ Status PiecewiseJerkSpeedOptimizer::Process(const PathData& path_data, // Update STBoundary const double kEpsilon = 0.1; std::vector> s_bounds; + // 根据障碍物和ST graph构建s的约束 for (int i = 0; i < num_of_knots; ++i) { double curr_t = i * delta_t; double s_lower_bound = 0.0; @@ -169,6 +174,7 @@ Status PiecewiseJerkSpeedOptimizer::Process(const PathData& path_data, print_debug.AddPoint("sv_boundary_upper", path_s, v_upper_bound); } AdjustInitStatus(s_dot_bounds, delta_t, init_s); + // 速度规划任务 PiecewiseJerkSpeedProblem piecewise_jerk_problem(num_of_knots, delta_t, init_s); piecewise_jerk_problem.set_weight_ddx(config_.acc_weight()); @@ -185,7 +191,7 @@ Status PiecewiseJerkSpeedOptimizer::Process(const PathData& path_data, piecewise_jerk_problem.set_penalty_dx(penalty_dx); piecewise_jerk_problem.set_dx_bounds(std::move(s_dot_bounds)); - // Solve the problem + // Solve the problem 优化求解 if (!piecewise_jerk_problem.Optimize()) { const std::string msg = "Piecewise jerk speed optimizer failed!"; AERROR << msg << ".try to fallback."; @@ -228,6 +234,7 @@ Status PiecewiseJerkSpeedOptimizer::Process(const PathData& path_data, speed_data->AppendSpeedPoint(s[i], delta_t * i, ds[i], dds[i], (dds[i] - dds[i - 1]) / delta_t); } + // 填充速度点，满足轨迹的时间长度 SpeedProfileGenerator::FillEnoughSpeedPoints(speed_data); RecordDebugInfo(*speed_data, st_graph_data.mutable_st_graph_debug()); print_debug.PrintToLog(); diff --git a/modules/planning/tasks/piecewise_jerk_speed_nonlinear/piecewise_jerk_speed_nonlinear_optimizer.cc b/modules/planning/tasks/piecewise_jerk_speed_nonlinear/piecewise_jerk_speed_nonlinear_optimizer.cc index ff475e1c795..e71198b89b3 100644 --- a/modules/planning/tasks/piecewise_jerk_speed_nonlinear/piecewise_jerk_speed_nonlinear_optimizer.cc +++ b/modules/planning/tasks/piecewise_jerk_speed_nonlinear/piecewise_jerk_speed_nonlinear_optimizer.cc @@ -25,12 +25,13 @@ #include "modules/common_msgs/basic_msgs/pnc_point.pb.h" #include "modules/planning/planning_base/proto/ipopt_return_status.pb.h" + #include "modules/common/util/util.h" #include "modules/common/vehicle_state/vehicle_state_provider.h" -#include "modules/planning/planning_base/gflags/planning_gflags.h" #include "modules/planning/planning_base/common/speed_profile_generator.h" #include "modules/planning/planning_base/common/st_graph_data.h" #include "modules/planning/planning_base/common/util/print_debug_info.h" +#include "modules/planning/planning_base/gflags/planning_gflags.h" #include "modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_path_problem.h" #include "modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_speed_problem.h" #include "modules/planning/tasks/piecewise_jerk_speed_nonlinear/piecewise_jerk_speed_nonlinear_ipopt_interface.h" @@ -77,11 +78,11 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::Process( AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } - + // 若到达终点则不进行速度规划 if (reference_line_info_->ReachedDestination()) { return Status::OK(); } - + // 构建优化变量边界 const auto problem_setups_status = SetUpStatesAndBounds(path_data, *speed_data); if (!problem_setups_status.ok()) { @@ -94,7 +95,7 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::Process( std::vector acceleration; const auto qp_start = std::chrono::system_clock::now(); - + // 基于QP算法对动态规划的粗ST曲线进行平滑 const auto qp_smooth_status = OptimizeByQP(speed_data, &distance, &velocity, &acceleration); @@ -106,7 +107,7 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::Process( speed_data->clear(); return qp_smooth_status; } - + // 基于QP算法平滑限速曲线 const bool speed_limit_check_status = CheckSpeedLimitFeasibility(); if (speed_limit_check_status) { @@ -141,7 +142,7 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::Process( } const auto nlp_start = std::chrono::system_clock::now(); - + // 基于非线性规划优化ST曲线 const auto nlp_smooth_status = OptimizeByNLP(&distance, &velocity, &acceleration); @@ -243,18 +244,15 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::SetUpStatesAndBounds( s_soft_upper_bound = std::fmin(s_soft_upper_bound, s_upper); break; case STBoundary::BoundaryType::FOLLOW: - s_upper_bound = - std::fmin(s_upper_bound, s_upper); + s_upper_bound = std::fmin(s_upper_bound, s_upper); if (!speed_data.EvaluateByTime(curr_t, &sp)) { const std::string msg = "rough speed profile estimation for soft follow fence failed"; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } - s_soft_upper_bound = - std::fmin(s_soft_upper_bound, - s_upper - - std::min(7.0, 2.5 * sp.v())); + s_soft_upper_bound = std::fmin( + s_soft_upper_bound, s_upper - std::min(7.0, 2.5 * sp.v())); break; case STBoundary::BoundaryType::OVERTAKE: s_lower_bound = std::fmax(s_lower_bound, s_lower); @@ -454,10 +452,12 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::OptimizeByQP( std::array init_states = {s_init_, s_dot_init_, s_ddot_init_}; PiecewiseJerkSpeedProblem piecewise_jerk_problem(num_of_knots_, delta_t_, init_states); + // 设置速度、加速度和加加速度的边界 piecewise_jerk_problem.set_dx_bounds( 0.0, std::fmax(FLAGS_planning_upper_speed_limit, init_states[1])); piecewise_jerk_problem.set_ddx_bounds(s_ddot_min_, s_ddot_max_); piecewise_jerk_problem.set_dddx_bound(s_dddot_min_, s_dddot_max_); + // 设置路径 s 的边界 piecewise_jerk_problem.set_x_bounds(s_bounds_); // TODO(Jinyun): parameter tunnings @@ -465,7 +465,9 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::OptimizeByQP( piecewise_jerk_problem.set_weight_dx(0.0); piecewise_jerk_problem.set_weight_ddx(config_.acc_weight()); piecewise_jerk_problem.set_weight_dddx(config_.jerk_weight()); + // 用于调试的 PrintCurves 实例 PrintCurves debug; + // 获取路径 s 的参考值 std::vector x_ref; for (int i = 0; i < num_of_knots_; ++i) { const double curr_t = i * delta_t_; @@ -486,6 +488,7 @@ Status PiecewiseJerkSpeedNonlinearOptimizer::OptimizeByQP( return Status(ErrorCode::PLANNING_ERROR, msg); } + // 获取优化结果 *distance = piecewise_jerk_problem.opt_x(); *velocity = piecewise_jerk_problem.opt_dx(); *acceleration = piecewise_jerk_problem.opt_ddx(); diff --git a/modules/planning/tasks/speed_bounds_decider/speed_bounds_decider.cc b/modules/planning/tasks/speed_bounds_decider/speed_bounds_decider.cc index 55629ddf58a..74102572329 100644 --- a/modules/planning/tasks/speed_bounds_decider/speed_bounds_decider.cc +++ b/modules/planning/tasks/speed_bounds_decider/speed_bounds_decider.cc @@ -25,9 +25,9 @@ #include "modules/common/vehicle_state/vehicle_state_provider.h" #include "modules/planning/planning_base/common/path/path_data.h" #include "modules/planning/planning_base/common/planning_context.h" -#include "modules/planning/planning_base/gflags/planning_gflags.h" #include "modules/planning/planning_base/common/st_graph_data.h" #include "modules/planning/planning_base/common/util/common.h" +#include "modules/planning/planning_base/gflags/planning_gflags.h" #include "modules/planning/tasks/speed_bounds_decider/speed_limit_decider.h" #include "modules/planning/tasks/speed_bounds_decider/st_boundary_mapper.h" @@ -49,7 +49,7 @@ bool SpeedBoundsDecider::Init( // Load the config this task. return Decider::LoadConfig(&config_); } - +// 产生ST可行驶区间 Status SpeedBoundsDecider::Process( Frame *const frame, ReferenceLineInfo *const reference_line_info) { // retrieve data from frame and reference_line_info diff --git a/modules/planning/tasks/speed_decider/speed_decider.cc b/modules/planning/tasks/speed_decider/speed_decider.cc index 1d167955efb..a304513dadc 100644 --- a/modules/planning/tasks/speed_decider/speed_decider.cc +++ b/modules/planning/tasks/speed_decider/speed_decider.cc @@ -84,6 +84,7 @@ common::Status SpeedDecider::Execute(Frame* frame, SpeedDecider::STLocation SpeedDecider::GetSTLocation( const PathDecision* const path_decision, const SpeedData& speed_profile, const STBoundary& st_boundary) const { + // 若boundary为空，设置为BELOW；一般情况下这个障碍物直接被跳过，不考虑 if (st_boundary.IsEmpty()) { return BELOW; } @@ -92,9 +93,11 @@ SpeedDecider::STLocation SpeedDecider::GetSTLocation( bool st_position_set = false; const double start_t = st_boundary.min_t(); const double end_t = st_boundary.max_t(); + // 遍历速度曲线中的每一个点 for (size_t i = 0; i + 1 < speed_profile.size(); ++i) { const STPoint curr_st(speed_profile[i].s(), speed_profile[i].t()); const STPoint next_st(speed_profile[i + 1].s(), speed_profile[i + 1].t()); + // 跳过和障碍物不在一个ST范围内的 if (curr_st.t() < start_t && next_st.t() < start_t) { continue; } @@ -104,11 +107,13 @@ SpeedDecider::STLocation SpeedDecider::GetSTLocation( if (!FLAGS_use_st_drivable_boundary) { common::math::LineSegment2d speed_line(curr_st, next_st); + // 检查是否碰撞 if (st_boundary.HasOverlap(speed_line)) { ADEBUG << "speed profile cross st_boundaries."; st_location = CROSS; if (!FLAGS_use_st_drivable_boundary) { + // 检查类型是否是KEEP_CLEAR if (st_boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) { if (!CheckKeepClearCrossable(path_decision, speed_profile, @@ -218,22 +223,24 @@ bool SpeedDecider::IsFollowTooClose(const Obstacle& obstacle) const { Status SpeedDecider::MakeObjectDecision( const SpeedData& speed_profile, PathDecision* const path_decision) const { + // 检查DP的结果 if (speed_profile.size() < 2) { const std::string msg = "dp_st_graph failed to get speed profile."; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); } - + // 遍历障碍物 for (const auto* obstacle : path_decision->obstacles().Items()) { auto* mutable_obstacle = path_decision->Find(obstacle->Id()); const auto& boundary = mutable_obstacle->path_st_boundary(); - + // 若障碍物的STBoundary在ST_Graph内，忽略 if (boundary.IsEmpty() || boundary.max_s() < 0.0 || boundary.max_t() < 0.0 || boundary.min_t() >= speed_profile.back().t()) { AppendIgnoreDecision(mutable_obstacle); continue; } + // 若已经有了纵向决策，忽略 if (obstacle->HasLongitudinalDecision()) { AppendIgnoreDecision(mutable_obstacle); continue; @@ -258,9 +265,9 @@ Status SpeedDecider::MakeObjectDecision( } continue; } - + // 根据路径决策、DP得到的速度曲线、障碍物边界获取STlocation auto location = GetSTLocation(path_decision, speed_profile, boundary); - + // 检查KEEP_CLEAR区域是否阻塞 if (!FLAGS_use_st_drivable_boundary) { if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) { if (CheckKeepClearBlocked(path_decision, *obstacle)) { @@ -271,12 +278,14 @@ Status SpeedDecider::MakeObjectDecision( switch (location) { case BELOW: + // 若障碍物类型为KEEP_CLEAR，创建停止决策 if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) { ObjectDecisionType stop_decision; if (CreateStopDecision(*mutable_obstacle, &stop_decision, 0.0)) { mutable_obstacle->AddLongitudinalDecision("dp_st_graph/keep_clear", stop_decision); } + // 检查ADC是否处于跟车的状态 } else if (CheckIsFollow(*obstacle, boundary)) { // stop for low_speed decelerating if (IsFollowTooClose(*mutable_obstacle)) { @@ -318,6 +327,7 @@ Status SpeedDecider::MakeObjectDecision( } break; case CROSS: + // 障碍物是否阻塞 if (mutable_obstacle->IsBlockingObstacle()) { ObjectDecisionType stop_decision; if (CreateStopDecision(*mutable_obstacle, &stop_decision, diff --git a/modules/routing/core/navigator.cc b/modules/routing/core/navigator.cc index 307af8120c1..b88000028e8 100644 --- a/modules/routing/core/navigator.cc +++ b/modules/routing/core/navigator.cc @@ -55,8 +55,7 @@ bool ShowRequestInfo(const routing::RoutingRequest& request, return true; } -bool GetWayNodes(const routing::RoutingRequest& request, - const TopoGraph* graph, +bool GetWayNodes(const routing::RoutingRequest& request, const TopoGraph* graph, std::vector* const way_nodes, std::vector* const way_s) { for (const auto& point : request.waypoint()) { @@ -124,10 +123,12 @@ bool Navigator::Init(const routing::RoutingRequest& request, std::vector* const way_nodes, std::vector* const way_s) { Clear(); + // 获取routing请求，对应图中的节点 if (!GetWayNodes(request, graph_.get(), way_nodes, way_s)) { AERROR << "Failed to find search terminal point in graph!"; return false; } + // 根据请求生成对应的黑名单lane black_list_generator_->GenerateBlackMapFromRequest(request, graph_.get(), &topo_range_manager_); return true; @@ -156,10 +157,12 @@ bool Navigator::SearchRouteByStrategy( const std::vector& way_s, std::vector* const result_nodes) const { std::unique_ptr strategy_ptr; + // 通过Astar算法来查找路径 strategy_ptr.reset(new AStarStrategy(FLAGS_enable_change_lane_in_result)); result_nodes->clear(); std::vector node_vec; + // 编译routing_request节点 for (size_t i = 1; i < way_nodes.size(); ++i) { const auto* way_start = way_nodes[i - 1]; const auto* way_end = way_nodes[i]; @@ -167,23 +170,29 @@ bool Navigator::SearchRouteByStrategy( double way_end_s = way_s[i]; TopoRangeManager full_range_manager = topo_range_manager_; + // 添加黑名单，这里主要是把车道根据起点和终点做分割。 black_list_generator_->AddBlackMapFromTerminal( way_start, way_end, way_start_s, way_end_s, &full_range_manager); + // 因为对车道做了分割，这里会创建子图，比如一个车道分成2个子节点， + // 2个子节点会创建一张子图。 SubTopoGraph sub_graph(full_range_manager.RangeMap()); + + // 获取起点 const auto* start = sub_graph.GetSubNodeWithS(way_start, way_start_s); if (start == nullptr) { AERROR << "Sub graph node is nullptr, origin node id: " << way_start->LaneId() << ", s:" << way_start_s; return false; } + // 获取终点 const auto* end = sub_graph.GetSubNodeWithS(way_end, way_end_s); if (end == nullptr) { AERROR << "Sub graph node is nullptr, origin node id: " << way_end->LaneId() << ", s:" << way_end_s; return false; } - + // 通过Astar查找最优路径 std::vector cur_result_nodes; if (!strategy_ptr->Search(graph, &sub_graph, start, end, &cur_result_nodes)) { @@ -191,11 +200,11 @@ bool Navigator::SearchRouteByStrategy( << " to " << end->LaneId(); return false; } - + // 保存结果到node_vec node_vec.insert(node_vec.end(), cur_result_nodes.begin(), cur_result_nodes.end()); } - + // 合并Route if (!MergeRoute(node_vec, result_nodes)) { AERROR << "Failed to merge route."; return false; diff --git a/modules/routing/routing.cc b/modules/routing/routing.cc index 88d379d1704..70bfc7de6a9 100644 --- a/modules/routing/routing.cc +++ b/modules/routing/routing.cc @@ -34,11 +34,16 @@ std::string Routing::Name() const { return FLAGS_routing_node_name; } Routing::Routing() : monitor_logger_buffer_(common::monitor::MonitorMessageItem::ROUTING) {} +// 初始化函数 apollo::common::Status Routing::Init() { + // 读取拓扑地图routing_map的文件位置信息 const auto routing_map_file = apollo::hdmap::RoutingMapFile(); AINFO << "Use routing topology graph path: " << routing_map_file; + // 在Navigator类加载指定的graph图 navigator_ptr_.reset(new Navigator(routing_map_file)); + // 通过map模块提供的功能包,读取原始地图信息,即包括点和边的信息 + // 据此查找routing request请求的点距离最近的lane,并且返回对应的lane id. hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr(); ACHECK(hdmap_) << "Failed to load map file:" << apollo::hdmap::BaseMapFile(); @@ -124,41 +129,56 @@ std::vector Routing::FillLaneInfoIfMissing( ADEBUG << "Fixed routing request:" << fixed_request.DebugString(); } return fixed_requests; -} - -bool Routing::Process( - const std::shared_ptr& routing_request, - routing::RoutingResponse* const routing_response) { - if (nullptr == routing_request) { - AWARN << "Routing request is empty!"; - return true; - } - CHECK_NOTNULL(routing_response); - AINFO << "Get new routing request:" << routing_request->DebugString(); - const auto& fixed_requests = FillLaneInfoIfMissing(*routing_request); - double min_routing_length = std::numeric_limits::max(); - for (const auto& fixed_request : fixed_requests) { - routing::RoutingResponse routing_response_temp; - if (navigator_ptr_->SearchRoute(fixed_request, &routing_response_temp)) { - const double routing_length = - routing_response_temp.measurement().distance(); - if (routing_length < min_routing_length) { - routing_response->CopyFrom(routing_response_temp); - min_routing_length = routing_length; + // 处理路由请求并返回相应的路由响应 + bool Routing::Process( + // 该函数接受一个指向RoutingRequest对象的智能指针作为参数 + const std::shared_ptr& routing_request, + // 该函数返回一个指向RoutingResponse对象的指针 + routing::RoutingResponse* const routing_response) { + // 首先检查传入的路由请求是否为空 + if (nullptr == routing_request) { + AWARN << "Routing request is empty!"; + // 如果为空，返回true并结束函数 + return true; + } + // 接着检查传入的路由响应是否为空 + CHECK_NOTNULL(routing_response); + // 记录接收到的路由请求 + AINFO << "Get new routing request:" << routing_request->DebugString(); + + // 填充缺失的车道信息，并返回一组新的路由请求 + const auto& fixed_requests = FillLaneInfoIfMissing(*routing_request); + // 定义一个变量来存储最短的路由长度 + double min_routing_length = std::numeric_limits::max(); + // 迭代处理每一个新的路由请求 + for (const auto& fixed_request : fixed_requests) { + // 为每一个新的路由请求创建一个路由响应 + routing::RoutingResponse routing_response_temp; + // 在导航器中搜索路由并将结果存储在临时路由响应中 + if (navigator_ptr_->SearchRoute(fixed_request, &routing_response_temp)) { + // 计算临时路由响应的路由长度 + const double routing_length = + routing_response_temp.measurement().distance(); + // 如果路由长度小于目前存储的最短路由长度，则更新最短路由长度和路由响应 + if (routing_length < min_routing_length) { + routing_response->CopyFrom(routing_response_temp); + min_routing_length = routing_length; + } } } - } - if (min_routing_length < std::numeric_limits::max()) { - monitor_logger_buffer_.INFO("Routing success!"); - return true; - } + // 如果找到了最短的路由，则记录路由成功并返回true + if (min_routing_length < std::numeric_limits::max()) { + monitor_logger_buffer_.INFO("Routing success!"); + return true; + } - AERROR << "Failed to search route with navigator."; - monitor_logger_buffer_.WARN("Routing failed! " + - routing_response->status().msg()); - return false; -} + // 如果没有找到最短的路由，则记录路由失败并返回false + AERROR << "Failed to search route with navigator."; + monitor_logger_buffer_.WARN("Routing failed! " + + routing_response->status().msg()); + return false; + } } // namespace routing } // namespace apollo diff --git a/modules/routing/routing_component.cc b/modules/routing/routing_component.cc index f9ee366b895..9805ca21bed 100644 --- a/modules/routing/routing_component.cc +++ b/modules/routing/routing_component.cc @@ -27,6 +27,7 @@ namespace apollo { namespace routing { bool RoutingComponent::Init() { + // 获取配置参数 RoutingConfig routing_conf; ACHECK(cyber::ComponentBase::GetProtoConfig(&routing_conf)) << "Unable to load routing conf file: " @@ -34,7 +35,7 @@ bool RoutingComponent::Init() { AINFO << "Config file: " << cyber::ComponentBase::ConfigFilePath() << " is loaded."; - + // 设置消息qos apollo::cyber::proto::RoleAttributes attr; attr.set_channel_name(routing_conf.topic_config().routing_response_topic()); auto qos = attr.mutable_qos_profile(); @@ -79,13 +80,16 @@ bool RoutingComponent::Init() { return routing_.Init().ok() && routing_.Start().ok(); } +// 收到router请求触发 bool RoutingComponent::Proc( const std::shared_ptr& request) { auto response = std::make_shared(); + // router请求是否成功 if (!routing_.Process(request, response.get())) { return false; } common::util::FillHeader(node_->Name(), response.get()); + // 发送数据 response_writer_->Write(response); { std::lock_guard guard(mutex_); diff --git a/modules/routing/routing_component.h b/modules/routing/routing_component.h index b08bef10c5a..697e74abc7c 100644 --- a/modules/routing/routing_component.h +++ b/modules/routing/routing_component.h @@ -31,15 +31,14 @@ class RoutingComponent final public: bool Init() override; - bool Proc( - const std::shared_ptr& request) override; + bool Proc(const std::shared_ptr& request) override; private: std::shared_ptr<::apollo::cyber::Writer> response_writer_ = nullptr; std::shared_ptr<::apollo::cyber::Writer> response_history_writer_ = nullptr; - Routing routing_; + Routing routing_; // class 包含router需要的状态反馈，处理函数 std::shared_ptr response_ = nullptr; std::unique_ptr<::apollo::cyber::Timer> timer_; std::mutex mutex_; diff --git a/modules/routing/strategy/a_star_strategy.cc b/modules/routing/strategy/a_star_strategy.cc index 906c79b8d6e..e1f8407589d 100644 --- a/modules/routing/strategy/a_star_strategy.cc +++ b/modules/routing/strategy/a_star_strategy.cc @@ -179,13 +179,15 @@ bool Reconstruct( const std::unordered_map& came_from, const TopoNode* dest_node, std::vector* result_nodes) { std::vector result_node_vec; + // 终点设置为第一个搜索点 result_node_vec.push_back(dest_node); - + // 从search 获取came_from的map中搜索路径车道ID auto iter = came_from.find(dest_node); while (iter != came_from.end()) { result_node_vec.push_back(iter->second); iter = came_from.find(iter->second); } + // 路线反转，生成顺序正确的路线信息 std::reverse(result_node_vec.begin(), result_node_vec.end()); if (!AdjustLaneChange(&result_node_vec)) { AERROR << "Failed to adjust lane change"; @@ -221,43 +223,49 @@ double AStarStrategy::HeuristicCost(const TopoNode* src_node, return distance; } +// 输入:起点、终点、读取的拓扑地图以及根据起点终点生成的子拓扑图、到起点到达终点的点集： bool AStarStrategy::Search(const TopoGraph* graph, const SubTopoGraph* sub_graph, const TopoNode* src_node, const TopoNode* dest_node, std::vector* const result_nodes) { Clear(); AINFO << "Start A* search algorithm."; - + // 该优先列表将f最小的值作为最优点 std::priority_queue open_set_detail; - + // 将地图选取的起点作为搜索的第一个点,计算起点到终点的代价值(曼哈顿距离)并作为f SearchNode src_search_node(src_node); src_search_node.f = HeuristicCost(src_node, dest_node); + // 将该点加入到开放列表之中 open_set_detail.push(src_search_node); - open_set_.insert(src_node); g_score_[src_node] = 0.0; enter_s_[src_node] = src_node->StartS(); - + // 定义当前节点 SearchNode current_node; std::unordered_set next_edge_set; std::unordered_set sub_edge_set; while (!open_set_detail.empty()) { + // 若open集非空,选取最优的为当前节点,开始搜索 current_node = open_set_detail.top(); const auto* from_node = current_node.topo_node; + // 若当前点等于目标点,结束搜索 if (current_node.topo_node == dest_node) { + // ReconstructL:从终点到起点进行反向搜索，获取轨迹点 if (!Reconstruct(came_from_, from_node, result_nodes)) { AERROR << "Failed to reconstruct route."; return false; } return true; } + // 将当前点从搜索点集中清除 open_set_.erase(from_node); open_set_detail.pop(); - + // 跳过closed集中添加的点 if (closed_set_.count(from_node) != 0) { // if showed before, just skip... continue; } + // 将当前点添加到closed集中 closed_set_.emplace(from_node); // if residual_s is less than FLAGS_min_length_for_lane_change, only move @@ -277,18 +285,24 @@ bool AStarStrategy::Search(const TopoGraph* graph, for (const auto* edge : next_edge_set) { const auto* to_node = edge->ToNode(); + // 跳过closed集中的点 if (closed_set_.count(to_node) == 1) { continue; } + // 跳过不能换到到达的点 if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) { continue; } + // 将当前节点的g值和能够达到的点的cost值相加 + // GetCostToNeighbor返回相邻节点的cost和边edge的cost tentative_g_score = g_score_[current_node.topo_node] + GetCostToNeighbor(edge); + // 若下一点需要换道才能到达,cost会减少部分值 if (edge->Type() != TopoEdgeType::TET_FORWARD) { tentative_g_score -= (edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2; } + // 计算能够达到点的f值 double f = tentative_g_score + HeuristicCost(to_node, dest_node); if (open_set_.count(to_node) != 0 && f >= g_score_[to_node]) { continue; @@ -311,10 +325,12 @@ bool AStarStrategy::Search(const TopoGraph* graph, } g_score_[to_node] = f; + // 初始化下一节点 SearchNode next_node(to_node); next_node.f = f; open_set_detail.push(next_node); came_from_[to_node] = from_node; + // 将能够达到的点加入open集 if (open_set_.count(to_node) == 0) { open_set_.insert(to_node); } diff --git a/modules/routing/topo_creator/graph_creator.cc b/modules/routing/topo_creator/graph_creator.cc index f83cf01b11b..a10a3ed9e35 100644 --- a/modules/routing/topo_creator/graph_creator.cc +++ b/modules/routing/topo_creator/graph_creator.cc @@ -61,6 +61,8 @@ GraphCreator::GraphCreator(const std::string& base_map_file_path, routing_conf_(routing_conf) {} bool GraphCreator::Create() { + // 这里注意，有2种格式，一种是Opendrive格式，通过OpendriveAdapter来读取 + // 另外一种是apollo自己定义的格式。 if (absl::EndsWith(base_map_file_path_, ".xml")) { if (!hdmap::adapter::OpendriveAdapter::LoadData(base_map_file_path_, &pbmap_)) { @@ -75,14 +77,14 @@ bool GraphCreator::Create() { } AINFO << "Number of lanes: " << pbmap_.lane_size(); - + // graph_为最后保存的图，消息格式在topo_graph.proto中申明 graph_.set_hdmap_version(pbmap_.header().version()); graph_.set_hdmap_district(pbmap_.header().district()); node_index_map_.clear(); road_id_map_.clear(); showed_edge_id_set_.clear(); - + // 从base_map中读取道路和lane对应关系，base_map的消息结构在map.proto和map_road.proto中 for (const auto& road : pbmap_.road()) { for (const auto& section : road.section()) { for (const auto& lane_id : section.lane_id()) { @@ -90,11 +92,11 @@ bool GraphCreator::Create() { } } } - + // 初始化禁止的车道线，从配置文件中读取最小掉头半径 InitForbiddenLanes(); const double min_turn_radius = VehicleConfigHelper::GetConfig().vehicle_param().min_turn_radius(); - + // 遍历base_map中的lane，并且创建节点。 for (const auto& lane : pbmap_.lane()) { const auto& lane_id = lane.id().id(); if (forbidden_lane_id_set_.find(lane_id) != forbidden_lane_id_set_.end()) { @@ -108,7 +110,12 @@ bool GraphCreator::Create() { continue; } AINFO << "Current lane id: " << lane_id; + // 存储图中节点index和lane_id的关系，因为跳过node可以找到lane， + // 而通过lane_id需要遍历节点才能找到节点index。 node_index_map_[lane_id] = graph_.node_size(); + + // 如果从road_id_map_中找到lane_id，则把创建节点的时候指定道路id， + // 如果没有找到那么road_id则为空。 const auto iter = road_id_map_.find(lane_id); if (iter != road_id_map_.end()) { node_creator::GetPbNode(lane, iter->second, routing_conf_, @@ -120,18 +127,27 @@ bool GraphCreator::Create() { } for (const auto& lane : pbmap_.lane()) { + // 遍历base_map中的lane，并且创建边。 const auto& lane_id = lane.id().id(); + // 跳过不是城市道路(CITY_DRIVING)的车道 if (forbidden_lane_id_set_.find(lane_id) != forbidden_lane_id_set_.end()) { ADEBUG << "Ignored lane id: " << lane_id << " because its type is NOT CITY_DRIVING."; continue; } + + // 这里就是通过上面所说的通过lane_id找到node的index，得到节点， + // 如果不保存，则需要遍历所有节点通过lane_id来查找节点，原因为node中有lane_id， + // 而lane结构中没有node_id。 const auto& from_node = graph_.node(node_index_map_[lane_id]); + // 添加一条该节点到下一个节点的边，注意这里没有换道，所以方向为前。 AddEdge(from_node, lane.successor_id(), Edge::FORWARD); if (lane.length() < FLAGS_min_length_for_lane_change) { continue; } + // 车道有左边界，并且允许变道 + // 添加一条该节点到左边邻居的边 if (lane.has_left_boundary() && IsAllowedToCross(lane.left_boundary())) { AddEdge(from_node, lane.left_neighbor_forward_lane_id(), Edge::LEFT); } diff --git a/modules/routing/topo_creator/topo_creator.cc b/modules/routing/topo_creator/topo_creator.cc index c29fa6465ca..6f689ec84af 100644 --- a/modules/routing/topo_creator/topo_creator.cc +++ b/modules/routing/topo_creator/topo_creator.cc @@ -31,7 +31,7 @@ int main(int argc, char **argv) { AINFO << "Conf file: " << FLAGS_routing_conf_file << " is loaded."; - const auto base_map = apollo::hdmap::BaseMapFile(); + const auto base_map = apollo::hdmap::BaseMapFile(); // string, store file dir const auto routing_map = apollo::hdmap::RoutingMapFile(); apollo::routing::GraphCreator creator(base_map, routing_map, routing_conf);