diff --git a/modules/planning/planning.cc b/modules/planning/planning.cc index 5c1117890da4fe529d4760409df5a5a5469b6341..22df0974e31bdf56c28c1f695f3516a1a4a0b6de 100644 --- a/modules/planning/planning.cc +++ b/modules/planning/planning.cc @@ -201,12 +201,15 @@ void Planning::RunOnce() { common::VehicleState::instance()->Update(localization, chassis); - double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate; + const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate; + + /** // the execution_start_time is the estimated time when the planned trajectory // will be executed by the controller. double execution_start_time = apollo::common::time::ToSecond(apollo::common::time::Clock::Now()) + planning_cycle_time; + **/ ADCTrajectory trajectory_pb; RecordInput(&trajectory_pb); @@ -218,7 +221,8 @@ void Planning::RunOnce() { } bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE; - bool res_planning = Plan(is_auto_mode, execution_start_time, &trajectory_pb); + bool res_planning = Plan(is_auto_mode, start_timestamp, planning_cycle_time, + &trajectory_pb); const double end_timestamp = apollo::common::time::ToSecond(Clock::Now()); const double time_diff_ms = (end_timestamp - start_timestamp) * 1000; @@ -227,7 +231,7 @@ void Planning::RunOnce() { if (res_planning) { AdapterManager::FillPlanningHeader("planning", &trajectory_pb); - trajectory_pb.mutable_header()->set_timestamp_sec(execution_start_time); + trajectory_pb.mutable_header()->set_timestamp_sec(start_timestamp); AdapterManager::PublishPlanning(trajectory_pb); ADEBUG << "Planning succeeded:" << trajectory_pb.header().DebugString(); } else { @@ -237,12 +241,15 @@ void Planning::RunOnce() { void Planning::Stop() {} -bool Planning::Plan(const bool is_on_auto_mode, const double publish_time, +bool Planning::Plan(const bool is_on_auto_mode, + const double current_time_stamp, + const double planning_cycle_time, ADCTrajectory* trajectory_pb) { - double vehicle_state_time = VehicleState::instance()->timestamp(); + const auto& stitching_trajectory = TrajectoryStitcher::compute_stitching_trajectory( - is_on_auto_mode, vehicle_state_time, last_publishable_trajectory_); + is_on_auto_mode, current_time_stamp, planning_cycle_time, + last_publishable_trajectory_); frame_->SetPlanningStartPoint(stitching_trajectory.back()); @@ -309,7 +316,7 @@ bool Planning::Plan(const bool is_on_auto_mode, const double publish_time, publishable_trajectory.prepend_trajectory_points( stitching_trajectory.begin(), stitching_trajectory.end() - 1); - publishable_trajectory.set_header_time(vehicle_state_time); + publishable_trajectory.set_header_time(current_time_stamp); publishable_trajectory.populate_trajectory_protobuf(trajectory_pb); diff --git a/modules/planning/planning.h b/modules/planning/planning.h index 457b72ff7e710645a0b00a12e782561889902839..98b9753f583cc6bb39ff3b53e15d605df03e288e 100644 --- a/modules/planning/planning.h +++ b/modules/planning/planning.h @@ -77,7 +77,8 @@ class Planning : public apollo::common::ApolloApp { * @param trajectory_pb the computed planning trajectory */ bool Plan(const bool is_on_auto_mode, - const double publish_time, + const double current_time_stamp, + const double planning_cycle_time, ADCTrajectory* trajectory_pb); const Frame* GetFrame() const; diff --git a/modules/planning/trajectory_stitcher/trajectory_stitcher.cc b/modules/planning/trajectory_stitcher/trajectory_stitcher.cc index a12683529007a0d0edbbde80f87d02086fe3173b..d1c7451357ce8decf9cf62805d5c5107e0d6180e 100644 --- a/modules/planning/trajectory_stitcher/trajectory_stitcher.cc +++ b/modules/planning/trajectory_stitcher/trajectory_stitcher.cc @@ -51,7 +51,8 @@ std::vector compute_reinit_stitching_trajectory() { // 3. the position deviation from actual and target is too high std::vector TrajectoryStitcher::compute_stitching_trajectory( const bool is_on_auto_mode, - const double vehicle_state_time, + const double current_timestamp, + const double planning_cycle_time, const PublishableTrajectory& prev_trajectory) { if (!is_on_auto_mode) { @@ -68,7 +69,7 @@ std::vector TrajectoryStitcher::compute_stitching_trajectory( } const double veh_rel_time = - vehicle_state_time - prev_trajectory.header_time(); + current_timestamp - prev_trajectory.header_time(); std::size_t matched_index = prev_trajectory.query_nearest_point(veh_rel_time); @@ -96,7 +97,7 @@ std::vector TrajectoryStitcher::compute_stitching_trajectory( return compute_reinit_stitching_trajectory(); } - double forward_rel_time = veh_rel_time + FLAGS_forward_predict_time; + double forward_rel_time = veh_rel_time + planning_cycle_time; std::size_t forward_index = prev_trajectory.query_nearest_point(forward_rel_time); diff --git a/modules/planning/trajectory_stitcher/trajectory_stitcher.h b/modules/planning/trajectory_stitcher/trajectory_stitcher.h index eda6fc2cee70b0fccc72be9816e6ce75c76f95a2..12cc41949f1c1e66c2c5ac664a320b6cab4a8145 100644 --- a/modules/planning/trajectory_stitcher/trajectory_stitcher.h +++ b/modules/planning/trajectory_stitcher/trajectory_stitcher.h @@ -38,7 +38,8 @@ class TrajectoryStitcher { static std::vector compute_stitching_trajectory( const bool is_on_auto_mode, - const double vehicle_state_time, + const double current_timestamp, + const double planning_cycle_time, const PublishableTrajectory& prev_trajectory); };