diff --git a/modules/planning/lattice/behavior_decider/behavior_decider.cc b/modules/planning/lattice/behavior_decider/behavior_decider.cc index 0fc37a244071ab8eeabe8fee9d332091c7339488..6da9407411cc0f78f221f10a479e8fdf6a92c6d1 100644 --- a/modules/planning/lattice/behavior_decider/behavior_decider.cc +++ b/modules/planning/lattice/behavior_decider/behavior_decider.cc @@ -47,14 +47,14 @@ PlanningTarget BehaviorDecider::Analyze( CHECK(frame != nullptr); CHECK_GT(discretized_reference_line.size(), 0); - PlanningTarget ret; + PlanningTarget planning_target; if (ScenarioManager::instance()->ComputeWorldDecision( - frame, reference_line_info, &ret) != 0) { + frame, reference_line_info, &planning_target) != 0) { AERROR << "ComputeWorldDecision error!"; } for (const auto& reference_point : discretized_reference_line) { - ret.mutable_discretized_reference_line() + planning_target.mutable_discretized_reference_line() ->add_discretized_reference_line_point() ->CopyFrom(reference_point); } @@ -64,29 +64,21 @@ PlanningTarget BehaviorDecider::Analyze( FLAGS_planning_upper_speed_limit, path_time_neighborhood_); - std::vector sample_bounds = condition_filter.QuerySampleBounds(); + // AddSampleBounds(condition_filter, &planning_target); - static int decision_cycles = 0; - if (FLAGS_enable_lattice_st_image_dump) { - apollo::planning_internal::LatticeStTraining st_data; - double timestamp = init_planning_point.relative_time(); - std::string st_img_name = "DecisionCycle_" + - std::to_string(decision_cycles) + "_" + - std::to_string(timestamp); - if (condition_filter.GenerateLatticeStPixels(&st_data, timestamp, - st_img_name)) { - ADEBUG << "Created_lattice_st_image_named = " << st_img_name - << "_for_timestamp = " << timestamp - << " num_colored_pixels = " << st_data.pixel_size(); - planning_internal::Debug* ptr_debug = - reference_line_info->mutable_debug(); - ptr_debug->mutable_planning_data()->mutable_lattice_st_image()->CopyFrom( - st_data); - } + AddNeighborPoints(condition_filter, &planning_target); + + return planning_target; +} + +void BehaviorDecider::AddSampleBounds( + const ConditionFilter& condition_filter, + PlanningTarget* const planning_target) { + std::vector sample_bounds = condition_filter.QuerySampleBounds(); + for (const auto& sample_bound : sample_bounds) { + planning_target->add_sample_bound()->CopyFrom(sample_bound); } - decision_cycles += 1; - // Debug SampleBound if (sample_bounds.empty()) { ADEBUG << "Sample_bounds empty"; } else { @@ -94,11 +86,45 @@ PlanningTarget BehaviorDecider::Analyze( ADEBUG << "Sample_bound: " << sample_bound.ShortDebugString(); } } +} - for (const auto& sample_bound : sample_bounds) { - ret.add_sample_bound()->CopyFrom(sample_bound); +void BehaviorDecider::AddNeighborPoints( + const ConditionFilter& condition_filter, + PlanningTarget* const planning_target) { + std::vector neighbor_points = + condition_filter.QueryNeighborPoints(); + for (const auto& neighbor_point : neighbor_points) { + planning_target->add_neighbor_point()->CopyFrom(neighbor_point); + } + + if (neighbor_points.empty()) { + ADEBUG << "Sample_bounds empty"; + } else { + for (const SamplePoint& neighbor_point : neighbor_points) { + ADEBUG << "Neighbor point: " << neighbor_point.ShortDebugString(); + } + } +} + +void BehaviorDecider::DumpLatticeImage(const int index, + const common::TrajectoryPoint& init_planning_point, + const ConditionFilter& condition_filter, + ReferenceLineInfo* const reference_line_info) { + apollo::planning_internal::LatticeStTraining st_data; + double timestamp = init_planning_point.relative_time(); + std::string st_img_name = "DecisionCycle_" + + std::to_string(index) + "_" + + std::to_string(timestamp); + if (condition_filter.GenerateLatticeStPixels(&st_data, timestamp, + st_img_name)) { + ADEBUG << "Created_lattice_st_image_named = " << st_img_name + << "_for_timestamp = " << timestamp + << " num_colored_pixels = " << st_data.pixel_size(); + planning_internal::Debug* ptr_debug = + reference_line_info->mutable_debug(); + ptr_debug->mutable_planning_data()->mutable_lattice_st_image()->CopyFrom( + st_data); } - return ret; } } // namespace planning diff --git a/modules/planning/lattice/behavior_decider/behavior_decider.h b/modules/planning/lattice/behavior_decider/behavior_decider.h index 055d5572556a03216b6f01b7124b1ae34e9261c8..c1d823f94a627955553e5cec1eb0f3cd6c7648a1 100644 --- a/modules/planning/lattice/behavior_decider/behavior_decider.h +++ b/modules/planning/lattice/behavior_decider/behavior_decider.h @@ -29,6 +29,7 @@ #include "modules/planning/common/frame.h" #include "modules/planning/common/reference_line_info.h" #include "modules/planning/common/trajectory/discretized_trajectory.h" +#include "modules/planning/lattice/behavior_decider/condition_filter.h" #include "modules/planning/lattice/behavior_decider/path_time_graph.h" #include "modules/planning/proto/lattice_structure.pb.h" #include "modules/planning/proto/planning.pb.h" @@ -51,6 +52,19 @@ class BehaviorDecider { const std::vector& discretized_reference_line); private: + void AddSampleBounds( + const ConditionFilter& condition_filter, + PlanningTarget* const plannint_target); + + void AddNeighborPoints( + const ConditionFilter& condition_filter, + PlanningTarget* const plannint_target); + + void DumpLatticeImage(const int index, + const common::TrajectoryPoint& init_planning_point, + const ConditionFilter& condition_filter, + ReferenceLineInfo* const reference_line_info); + std::shared_ptr path_time_neighborhood_; }; diff --git a/modules/planning/lattice/behavior_decider/condition_filter.cc b/modules/planning/lattice/behavior_decider/condition_filter.cc index 828b74f4c431c18090c00f827b22cb9b4a7324ca..6893d47cf34cccbf97cde0e13e73e01b06d1e179 100644 --- a/modules/planning/lattice/behavior_decider/condition_filter.cc +++ b/modules/planning/lattice/behavior_decider/condition_filter.cc @@ -159,7 +159,7 @@ PathTimePointPair ConditionFilter::QueryPathTimeObstacleIntervals( return block_interval; } -std::vector ConditionFilter::QuerySamplePoints() const { +std::vector ConditionFilter::QueryNeighborPoints() const { std::vector sample_points; for (const auto& path_time_obstacle : ptr_path_time_neighborhood_->GetPathTimeObstacles()) { @@ -250,7 +250,7 @@ std::vector ConditionFilter::UniformTimeStamps( // Compute pixel img for lattice st bool ConditionFilter::GenerateLatticeStPixels( apollo::planning_internal::LatticeStTraining* st_data, double timestamp, - std::string st_img_name) { + std::string st_img_name) const { int num_rows = 250; int num_cols = 160; double s_step = 100.0 / static_cast(num_rows); @@ -297,7 +297,7 @@ bool ConditionFilter::GenerateLatticeStPixels( return true; } -bool ConditionFilter::WithinObstacleSt(double s, double t) { +bool ConditionFilter::WithinObstacleSt(double s, double t) const { const auto& path_time_obstacles = ptr_path_time_neighborhood_->GetPathTimeObstacles(); diff --git a/modules/planning/lattice/behavior_decider/condition_filter.h b/modules/planning/lattice/behavior_decider/condition_filter.h index fe4233111aac1c902cfd5d679d706d92aa27fca2..495a1195e1e893a3e6fc4b9d455132ea0740d173 100644 --- a/modules/planning/lattice/behavior_decider/condition_filter.h +++ b/modules/planning/lattice/behavior_decider/condition_filter.h @@ -45,16 +45,16 @@ class ConditionFilter { std::vector QuerySampleBounds() const; - std::vector QuerySamplePoints() const; + std::vector QueryNeighborPoints() const; std::vector> QueryPathTimeObstacleIntervals(const double t) const; bool GenerateLatticeStPixels( apollo::planning_internal::LatticeStTraining* st_data, double timestamp, - std::string st_img_name); + std::string st_img_name) const; - bool WithinObstacleSt(double s, double t); + bool WithinObstacleSt(double s, double t) const; private: // Return true only if t is within the range of time slot, diff --git a/modules/planning/lattice/trajectory_generator/end_condition_sampler.cc b/modules/planning/lattice/trajectory_generator/end_condition_sampler.cc index 8539395474697aa9a9f364ede57019a5469c5601..c44d0a5d4c7b373143c42a9f9c23b25c68493eaf 100644 --- a/modules/planning/lattice/trajectory_generator/end_condition_sampler.cc +++ b/modules/planning/lattice/trajectory_generator/end_condition_sampler.cc @@ -147,5 +147,19 @@ EndConditionSampler::SampleLonEndConditionsForPathTimeBounds( return end_s_conditions; } +std::vector, double>> +EndConditionSampler::SampleLonEndConditionsForNeighborPoints( + const PlanningTarget& planning_target) const { + std::vector, double>> end_s_conditions; + for (const SamplePoint& neighbor_point : planning_target.neighbor_point()) { + double s = neighbor_point.path_time_point().s(); + double v = neighbor_point.v(); + std::array end_state = {s, v, 0.0}; + end_s_conditions.push_back({end_state, + neighbor_point.path_time_point().t()}); + } + return end_s_conditions; +} + } // namespace planning } // namespace apollo diff --git a/modules/planning/lattice/trajectory_generator/end_condition_sampler.h b/modules/planning/lattice/trajectory_generator/end_condition_sampler.h index 729d89eee9f2e42199bdcd060708fb84befe4f03..8985891395617ebb1d5345d8909ac950a0639e36 100644 --- a/modules/planning/lattice/trajectory_generator/end_condition_sampler.h +++ b/modules/planning/lattice/trajectory_generator/end_condition_sampler.h @@ -54,6 +54,10 @@ class EndConditionSampler { SampleLonEndConditionsForPathTimeBounds( const PlanningTarget& planning_target) const; + std::vector, double>> + SampleLonEndConditionsForNeighborPoints( + const PlanningTarget& planning_target) const; + private: std::array init_s_; diff --git a/modules/planning/lattice/trajectory_generator/trajectory1d_generator.cc b/modules/planning/lattice/trajectory_generator/trajectory1d_generator.cc index 516dfcf8a6eb1dbb4547e21cc7246115bd10d1e0..d6a8d8f146f513359882c845a86e46180f1303df 100644 --- a/modules/planning/lattice/trajectory_generator/trajectory1d_generator.cc +++ b/modules/planning/lattice/trajectory_generator/trajectory1d_generator.cc @@ -123,6 +123,26 @@ void Trajectory1dGenerator::GenerateSpeedProfilesForPathTimeBound( } } +void Trajectory1dGenerator::GenerateSpeedProfilesForNeighborPoints( + const PlanningTarget& planning_target, + std::vector>* ptr_lon_trajectory_bundle) const { + std::vector, double>> end_conditions = + end_condition_sampler_->SampleLonEndConditionsForNeighborPoints( + planning_target); + + for (const auto& end_condition : end_conditions) { + std::shared_ptr lattice_traj_ptr( + new LatticeTrajectory1d( + std::shared_ptr(new QuinticPolynomialCurve1d( + init_lon_state_, end_condition.first, end_condition.second)))); + + lattice_traj_ptr->set_target_position(end_condition.first[0]); + lattice_traj_ptr->set_target_velocity(end_condition.first[1]); + lattice_traj_ptr->set_target_time(end_condition.second); + ptr_lon_trajectory_bundle->push_back(lattice_traj_ptr); + } +} + void Trajectory1dGenerator::GenerateLongitudinalTrajectoryBundle( const PlanningTarget& planning_target, std::vector>* ptr_lon_trajectory_bundle) const { @@ -130,9 +150,11 @@ void Trajectory1dGenerator::GenerateLongitudinalTrajectoryBundle( GenerateSpeedProfilesForCruising(planning_target.cruise_speed(), ptr_lon_trajectory_bundle); - // - GenerateSpeedProfilesForPathTimeBound(planning_target, - ptr_lon_trajectory_bundle); + // GenerateSpeedProfilesForPathTimeBound(planning_target, + // ptr_lon_trajectory_bundle); + + GenerateSpeedProfilesForNeighborPoints(planning_target, + ptr_lon_trajectory_bundle); if (planning_target.has_stop_point()) { GenerateSpeedProfileForStopping(planning_target.stop_point(), diff --git a/modules/planning/lattice/trajectory_generator/trajectory1d_generator.h b/modules/planning/lattice/trajectory_generator/trajectory1d_generator.h index a84173cca1b830f6de1117174bb76edcbfb8addf..7177e7adfc62c8e2221e75fbb14ba8ffbe04880e 100644 --- a/modules/planning/lattice/trajectory_generator/trajectory1d_generator.h +++ b/modules/planning/lattice/trajectory_generator/trajectory1d_generator.h @@ -59,6 +59,10 @@ class Trajectory1dGenerator { const PlanningTarget& planning_target, std::vector>* ptr_lon_trajectory_bundle) const; + void GenerateSpeedProfilesForNeighborPoints( + const PlanningTarget& planning_target, + std::vector>* ptr_lon_trajectory_bundle) const; + void GenerateLongitudinalTrajectoryBundle( const PlanningTarget& planning_target, std::vector>* ptr_lon_trajectory_bundle) const; diff --git a/modules/planning/proto/lattice_structure.proto b/modules/planning/proto/lattice_structure.proto index fcb31a2644dd3824a3596f6f48ec96e67868623c..ed24533e63d34eb329664b3e80f1b50d0d8713ca 100644 --- a/modules/planning/proto/lattice_structure.proto +++ b/modules/planning/proto/lattice_structure.proto @@ -46,6 +46,7 @@ message SampleBound { message PlanningTarget { optional DiscretizedReferenceLine discretized_reference_line = 1; repeated SampleBound sample_bound = 2; - optional double stop_point = 3; - optional double cruise_speed = 4; + repeated SamplePoint neighbor_point = 3; + optional double stop_point = 4; + optional double cruise_speed = 5; }