diff --git a/modules/planning/planning.cc b/modules/planning/planning.cc index c8be5bf9e4434fa7fc08da8059852182db1c5f08..24908cbb5adb6bc186f3cd8b035ec2575f9c8012 100644 --- a/modules/planning/planning.cc +++ b/modules/planning/planning.cc @@ -200,6 +200,11 @@ bool Planning::Plan(const bool is_on_auto_mode, const double publish_time, auto matched_info = ComputeStartingPointFromLastTrajectory(execution_start_time); TrajectoryPoint matched_point = matched_info.first; + if (FLAGS_enable_record_debug) { + trajectory_pb->mutable_debug()->mutable_planning_data() + ->mutable_init_point()->CopyFrom(matched_point); + } + std::uint32_t matched_index = matched_info.second; // Compute the position deviation between current vehicle @@ -239,10 +244,15 @@ bool Planning::Plan(const bool is_on_auto_mode, const double publish_time, // 2. we don't have the trajectory from last planning cycle or // 3. the position deviation from actual and target is too high // then planning from current vehicle state. - TrajectoryPoint vehicle_state_point = + TrajectoryPoint vehicle_start_point = ComputeStartingPointFromVehicleState(planning_cycle_time); - auto status = planner_->MakePlan(vehicle_state_point, trajectory_pb); + if (FLAGS_enable_record_debug) { + trajectory_pb->mutable_debug()->mutable_planning_data() + ->mutable_init_point()->CopyFrom(vehicle_start_point); + } + + auto status = planner_->MakePlan(vehicle_start_point, trajectory_pb); if (!status.ok()) { last_trajectory_.clear(); return false; diff --git a/modules/planning/proto/planning_internal.proto b/modules/planning/proto/planning_internal.proto index d59a9d4031f5deab1cd436fed195c6442ff023a3..25f2bf05e1326e33e9a52c5f1e440163a9355ed8 100644 --- a/modules/planning/proto/planning_internal.proto +++ b/modules/planning/proto/planning_internal.proto @@ -87,7 +87,7 @@ message PathDebug { optional apollo.common.Path path = 2; } -// next id: 10 +// next id: 11 message PlanningData { optional apollo.common.Header header = 1; @@ -95,13 +95,14 @@ message PlanningData { optional apollo.localization.LocalizationEstimate adc_position = 7; optional apollo.canbus.Chassis chassis = 8; optional apollo.hdmap.RoutingResult routing = 9; + optional apollo.common.TrajectoryPoint init_point = 10; - optional apollo.localization.Pose init_status = 2; // initial status of adc // processed planning_obstacles repeated PlanningObstacle planning_obstacle = 4; repeated PathDebug path = 6; + optional apollo.localization.Pose init_status = 2 [deprecated = true]; // initial status of adc optional apollo.planning.MainDecision main_decision = 3 [deprecated = true]; optional LightSignal light_signal = 5 [deprecated = true]; } diff --git a/modules/tools/diagnostics/batch_include.py b/modules/tools/diagnostics/batch_include.py index d5fd7064b1b89c1b0341b0af2c14a424b04adf95..cf06ac3c0ac57ca524e015abb118dd636cfb8568 100644 --- a/modules/tools/diagnostics/batch_include.py +++ b/modules/tools/diagnostics/batch_include.py @@ -19,7 +19,6 @@ from modules.canbus.proto import chassis_detail_pb2 from modules.canbus.proto import chassis_pb2 from modules.common.monitor.proto import monitor_pb2 -from modules.common.configs.proto import config_extrinsics_pb2 from modules.common.configs.proto import vehicle_config_pb2 from modules.common.proto import geometry_pb2 from modules.common.proto import header_pb2