diff --git a/modules/planning/common/planning_gflags.cc b/modules/planning/common/planning_gflags.cc index afd2ba0beca66c66188dfc6afa5c4e654f3cb0f2..1604ac0364b941fb355caadf3972482203867229 100644 --- a/modules/planning/common/planning_gflags.cc +++ b/modules/planning/common/planning_gflags.cc @@ -311,3 +311,5 @@ DEFINE_double(backward_routing_distance, 100.0, "The backward routing distance."); DEFINE_double(decision_valid_stop_range, 0.5, "The valid stop range in decision."); +DEFINE_bool(enable_record_debug, true, + "True to enable record debug into debug protobuf."); diff --git a/modules/planning/common/planning_gflags.h b/modules/planning/common/planning_gflags.h index 4adea59f78fcd4edb2fc8cab9a6c01a0d23e74d8..3bde74e9ca122682dd89b21aa09a8651ce561989 100644 --- a/modules/planning/common/planning_gflags.h +++ b/modules/planning/common/planning_gflags.h @@ -214,4 +214,6 @@ DECLARE_string(offline_routing_file); DECLARE_double(backward_routing_distance); DECLARE_double(decision_valid_stop_range); +DECLARE_bool(enable_record_debug); + #endif // MODULES_PLANNING_COMMON_PLANNING_GFLAGS_H_ diff --git a/modules/planning/planning.cc b/modules/planning/planning.cc index 64926df052e2fc3aa340bc22fc5576c4e213f758..c8be5bf9e4434fa7fc08da8059852182db1c5f08 100644 --- a/modules/planning/planning.cc +++ b/modules/planning/planning.cc @@ -105,6 +105,28 @@ Status Planning::Start() { return Status::OK(); } +void Planning:: RecordInput(ADCTrajectory* trajectory_pb) { + if (!FLAGS_enable_record_debug) { + ADEBUG << "Skip record input into debug"; + return; + } + auto planning_data = trajectory_pb->mutable_debug()->mutable_planning_data(); + auto adc_position = planning_data->mutable_adc_position(); + const auto& localization = + AdapterManager::GetLocalization()->GetLatestObserved(); + adc_position->CopyFrom(localization); + + const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved(); + auto debug_chassis= planning_data->mutable_chassis(); + debug_chassis->CopyFrom(chassis); + + const auto &routing_result = + AdapterManager::GetRoutingResult()->GetLatestObserved(); + + auto debug_routing = planning_data->mutable_routing(); + debug_routing->CopyFrom(routing_result); +} + void Planning::RunOnce() { AdapterManager::Observe(); if (AdapterManager::GetLocalization()->Empty()) { @@ -139,13 +161,15 @@ void Planning::RunOnce() { apollo::common::time::ToSecond(apollo::common::time::Clock::Now()) + planning_cycle_time; + ADCTrajectory trajectory_pb; + RecordInput(&trajectory_pb); + if (!DataCenter::instance()->init_current_frame( AdapterManager::GetPlanning()->GetSeqNum() + 1)) { AERROR << "DataCenter init frame failed"; return; } - ADCTrajectory trajectory_pb; bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE; bool res_planning = Plan(is_auto_mode, execution_start_time, &trajectory_pb); if (res_planning) { diff --git a/modules/planning/planning.h b/modules/planning/planning.h index 32c5b74b3c0dee99994a01e6200e1f640817c0f1..5c0b7e4c2c5c5aa5455ead594ed2de8bb53aaf23 100644 --- a/modules/planning/planning.h +++ b/modules/planning/planning.h @@ -86,6 +86,7 @@ class Planning : public apollo::common::ApolloApp { private: void RegisterPlanners(); void RunOnce(); + void RecordInput(ADCTrajectory* trajectory_pb); std::pair ComputeStartingPointFromLastTrajectory(const double curr_time) const; diff --git a/modules/planning/proto/BUILD b/modules/planning/proto/BUILD index 26da0b18392e4ae7197f2b1e007f91931e522e3e..20df34f777b23729d5acb846bad9991d5b829539 100644 --- a/modules/planning/proto/BUILD +++ b/modules/planning/proto/BUILD @@ -21,6 +21,7 @@ cc_proto_library( "//modules/common/proto:path_point_proto", "//modules/decision/proto:decision_proto", "//modules/localization/proto:localization_proto", + "//modules/map/proto:map_proto", "//modules/perception/proto:perception_proto", "//modules/prediction/proto:prediction_proto", ], diff --git a/modules/planning/proto/planning_internal.proto b/modules/planning/proto/planning_internal.proto index 368dc39b46996e9a14b9dc44d9f72ce6e35a5485..d59a9d4031f5deab1cd436fed195c6442ff023a3 100644 --- a/modules/planning/proto/planning_internal.proto +++ b/modules/planning/proto/planning_internal.proto @@ -2,11 +2,13 @@ syntax = "proto2"; package apollo.planning_internal; +import "modules/canbus/proto/chassis.proto"; import "modules/common/proto/header.proto"; import "modules/common/proto/error_code.proto"; import "modules/common/proto/path_point.proto"; import "modules/localization/proto/localization.proto"; import "modules/localization/proto/pose.proto"; +import "modules/map/proto/routing.proto"; import "modules/perception/proto/perception_obstacle.proto"; import "modules/planning/proto/decision.proto"; import "modules/prediction/proto/prediction_obstacle.proto"; @@ -85,9 +87,15 @@ message PathDebug { optional apollo.common.Path path = 2; } -// next id: 7 +// next id: 10 message PlanningData { optional apollo.common.Header header = 1; + + // input + optional apollo.localization.LocalizationEstimate adc_position = 7; + optional apollo.canbus.Chassis chassis = 8; + optional apollo.hdmap.RoutingResult routing = 9; + optional apollo.localization.Pose init_status = 2; // initial status of adc // processed planning_obstacles repeated PlanningObstacle planning_obstacle = 4;