diff --git a/modules/planning/open_space_planning.cc b/modules/planning/open_space_planning.cc index 4cc6406595f09c1d6690a40c388228997083090a..380c03e0976a019253978721825e18b3a97ad867 100644 --- a/modules/planning/open_space_planning.cc +++ b/modules/planning/open_space_planning.cc @@ -122,6 +122,9 @@ Status OpenSpacePlanning::Init(const PlanningConfig& config) { } start_time_ = Clock::NowInSeconds(); + + AINFO << "Open Space Planner Init Done"; + return planner_->Init(config_); } @@ -145,6 +148,8 @@ Status OpenSpacePlanning::InitFrame(const uint32_t sequence_num, return status; } + AINFO << "Open Space Planner Init Frame Done"; + return Status::OK(); } @@ -206,10 +211,6 @@ void OpenSpacePlanning::RunOnce(const LocalView& local_view, status = InitFrame(frame_num, stitching_trajectory.back(), start_timestamp, vehicle_state, trajectory_pb); - if (FLAGS_enable_record_debug) { - frame_->RecordInputDebug(trajectory_pb->mutable_debug()); - } - trajectory_pb->mutable_latency_stats()->set_init_frame_time_ms( Clock::NowInSeconds() - start_timestamp); @@ -272,6 +273,7 @@ void OpenSpacePlanning::RunOnce(const LocalView& local_view, ADEBUG << "Planning pb:" << trajectory_pb->header().DebugString(); frame_->mutable_trajectory()->CopyFrom(*trajectory_pb); + const uint32_t n = frame_->SequenceNum(); FrameHistory::Instance()->Add(n, std::move(frame_)); } @@ -280,36 +282,46 @@ Status OpenSpacePlanning::Plan( const double current_time_stamp, const std::vector& stitching_trajectory, ADCTrajectory* const trajectory_pb) { + auto status = planner_->Plan(stitching_trajectory.back(), frame_.get()); + // TODO(QiL): update last_publishable_trajectory_ for stitching + trajectory_pb->CopyFrom(frame_->trajectory()); + auto* ptr_debug = trajectory_pb->mutable_debug(); if (FLAGS_enable_record_debug) { ptr_debug->mutable_planning_data()->mutable_init_point()->CopyFrom( stitching_trajectory.back()); + ADEBUG << "Open space init point added!"; } - auto status = planner_->Plan(stitching_trajectory.back(), frame_.get()); - - if (status.ok() && FLAGS_enable_record_debug && - FLAGS_open_space_planner_switchable) { - frame_.get()->RecordOpenSpacePlannerDebug(ptr_debug); + if (status.ok() && FLAGS_enable_record_debug) { + ptr_debug->mutable_planning_data()->mutable_open_space()->CopyFrom( + frame_->open_space_debug()); + ADEBUG << "Open space debug information added!"; } - // TODO(QiL): update last_publishable_trajectory_ for stitching - trajectory_pb->CopyFrom(frame_->trajectory()); + if (FLAGS_export_chart) { + ExportOpenSpaceChart(frame_->open_space_debug(), ptr_debug); + ADEBUG << "Open Space Planning debug from frame is : " + << frame_->open_space_debug().ShortDebugString(); + ADEBUG << "Open Space Planning export chart with : " + << trajectory_pb->ShortDebugString(); + } return status; } -void AddTrajectory(const OpenSpaceDebug& open_space, Chart* chart) { +void AddOpenSpaceTrajectory(const OpenSpaceDebug& open_space_debug, + Chart* chart) { chart->set_title("Open Space Trajectory Visualization"); auto* options = chart->mutable_options(); - options->mutable_x()->set_min(open_space.xy_boundary(0)); - options->mutable_x()->set_max(open_space.xy_boundary(1)); + options->mutable_x()->set_min(open_space_debug.xy_boundary(0)); + options->mutable_x()->set_max(open_space_debug.xy_boundary(1)); options->mutable_x()->set_label_string("x (meter)"); - options->mutable_y()->set_min(open_space.xy_boundary(2)); - options->mutable_y()->set_max(open_space.xy_boundary(3)); + options->mutable_y()->set_min(open_space_debug.xy_boundary(2)); + options->mutable_y()->set_max(open_space_debug.xy_boundary(3)); options->mutable_y()->set_label_string("y (meter)"); - for (const auto& obstacle : open_space.obstacles()) { + for (const auto& obstacle : open_space_debug.obstacles()) { auto* polygon = chart->add_polygon(); polygon->set_label(obstacle.id()); for (int vertice_index = 0; @@ -320,7 +332,7 @@ void AddTrajectory(const OpenSpaceDebug& open_space, Chart* chart) { } } - for (const auto& trajectory : open_space.trajectories().trajectory()) { + for (const auto& trajectory : open_space_debug.trajectories().trajectory()) { auto* line = chart->add_line(); line->set_label(trajectory.name()); for (const auto& point : trajectory.trajectory_point()) { @@ -338,12 +350,13 @@ void AddTrajectory(const OpenSpaceDebug& open_space, Chart* chart) { } } -void OpenSpacePlanning::ExportChart(const planning_internal::Debug& debug_info, - planning_internal::Debug* debug_chart) { +void OpenSpacePlanning::ExportOpenSpaceChart( + const planning_internal::OpenSpaceDebug& debug_info, + planning_internal::Debug* debug_chart) { // Export Trajectory Visualization Chart. - if (FLAGS_open_space_planner_switchable && FLAGS_enable_record_debug) { - AddTrajectory(debug_info.planning_data().open_space(), - debug_chart->mutable_planning_data()->add_chart()); + if (FLAGS_enable_record_debug) { + AddOpenSpaceTrajectory(debug_info, + debug_chart->mutable_planning_data()->add_chart()); } } diff --git a/modules/planning/open_space_planning.h b/modules/planning/open_space_planning.h index 601bb42ab7d7a02455f6125ba52a2c83a8b51884..97d59598d6dd34f2b138237f249945f89463175a 100644 --- a/modules/planning/open_space_planning.h +++ b/modules/planning/open_space_planning.h @@ -67,8 +67,8 @@ class OpenSpacePlanning : public PlanningBase { const std::vector& stitching_trajectory, ADCTrajectory* const trajectory) override; - void ExportChart(const planning_internal::Debug& debug_info, - planning_internal::Debug* debug_chart) override; + void ExportOpenSpaceChart(const planning_internal::OpenSpaceDebug& debug_info, + planning_internal::Debug* debug_chart); private: common::Status InitFrame(const uint32_t sequence_num, @@ -76,7 +76,6 @@ class OpenSpacePlanning : public PlanningBase { const double start_time, const common::VehicleState& vehicle_state, ADCTrajectory* output_trajectory); - void ExportReferenceLineDebug(planning_internal::Debug* debug); bool CheckPlanningConfig(const PlanningConfig& config); private: diff --git a/modules/planning/planning_component.cc b/modules/planning/planning_component.cc index 57e5b576c3203dc32fc798e11250657e07486bf2..23200a27256f7ba134aa64b9a793583f897cd22e 100644 --- a/modules/planning/planning_component.cc +++ b/modules/planning/planning_component.cc @@ -124,10 +124,12 @@ bool PlanningComponent::Proc( } if (!CheckInput()) { + AERROR << "Input check failed"; return false; } ADCTrajectory adc_trajectory_pb; + AINFO << "Call RunOnce in Planning"; planning_base_->RunOnce(local_view_, &adc_trajectory_pb); common::util::FillHeader(node_->Name(), &adc_trajectory_pb); planning_writer_->Write(std::make_shared(adc_trajectory_pb));