From bdc20c45233dc740788a4e29cc2d989095eb32f1 Mon Sep 17 00:00:00 2001 From: Zhang Liangliang Date: Wed, 2 Aug 2017 11:18:56 -0700 Subject: [PATCH] changes in st_boundary and mapper. --- .../dp_st_speed/dp_st_speed_optimizer.cc | 4 +-- .../qp_spline_st_speed/qp_spline_st_graph.cc | 21 ++++++------ .../qp_spline_st_speed/qp_spline_st_graph.h | 4 +-- .../qp_spline_st_speed_optimizer.cc | 4 +-- .../optimizer/st_graph/st_boundary_mapper.cc | 6 ++-- .../optimizer/st_graph/st_boundary_mapper.h | 4 +-- .../st_graph/st_boundary_mapper_impl.cc | 2 +- .../st_graph/st_boundary_mapper_impl.h | 2 +- .../optimizer/st_graph/st_graph_boundary.cc | 32 ++++++++----------- .../optimizer/st_graph/st_graph_boundary.h | 6 ++-- 10 files changed, 40 insertions(+), 45 deletions(-) diff --git a/modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc b/modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc index 91bcdd54c8..417248e35a 100644 --- a/modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc +++ b/modules/planning/optimizer/dp_st_speed/dp_st_speed_optimizer.cc @@ -72,7 +72,7 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data, std::min(dp_st_speed_config_.total_path_length(), path_length); std::vector boundaries; if (!boundary_mapper_ - .get_graph_boundary(init_point, *decision_data, path_data, + .GetGraphBoundary(init_point, *decision_data, path_data, reference_line, dp_st_speed_config_.total_path_length(), dp_st_speed_config_.total_time(), &boundaries) @@ -86,7 +86,7 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data, // step 2 perform graph search SpeedLimit speed_limit; if (!boundary_mapper_ - .get_speed_limits(common::VehicleState::instance()->pose(), + .GetSpeedLimits(common::VehicleState::instance()->pose(), Frame::PncMap(), path_data, planning_distance, dp_st_speed_config_.matrix_dimension_s(), dp_st_speed_config_.max_speed(), &speed_limit) diff --git a/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc b/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc index a907377278..4737bd8e25 100644 --- a/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc +++ b/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc @@ -37,9 +37,10 @@ QpSplineStGraph::QpSplineStGraph( const QpSplineStSpeedConfig& qp_spline_st_speed_config, const apollo::common::VehicleParam& veh_param) : qp_spline_st_speed_config_(qp_spline_st_speed_config), - time_resolution_(qp_spline_st_speed_config_.total_time() / - qp_spline_st_speed_config_.number_of_discrete_graph_t()), - evaluation_time_resolution_( + t_knots_resolution_( + qp_spline_st_speed_config_.total_time() / + qp_spline_st_speed_config_.number_of_discrete_graph_t()), + t_evaluated_resolution_( qp_spline_st_speed_config_.total_time() / qp_spline_st_speed_config_.number_of_evaluated_graph_t()) @@ -51,7 +52,7 @@ void QpSplineStGraph::Init() { for (uint32_t i = 0; i <= qp_spline_st_speed_config_.number_of_discrete_graph_t(); ++i) { t_knots_.push_back(curr_t); - curr_t += time_resolution_; + curr_t += t_knots_resolution_; } // init evaluated t positions @@ -59,7 +60,7 @@ void QpSplineStGraph::Init() { for (uint32_t i = 0; i <= qp_spline_st_speed_config_.number_of_evaluated_graph_t(); ++i) { t_evaluated_.push_back(curr_t); - curr_t += evaluation_time_resolution_; + curr_t += t_evaluated_resolution_; } // init spline generator @@ -106,15 +107,16 @@ Status QpSplineStGraph::Search(const StGraphData& st_graph_data, speed_data->Clear(); const Spline1d& spline = spline_generator_->spline(); - double time_resolution = qp_spline_st_speed_config_.output_time_resolution(); + double t_output_resolution = + qp_spline_st_speed_config_.output_time_resolution(); double time = 0.0; - while (time < qp_spline_st_speed_config_.total_time() + time_resolution) { + while (time < qp_spline_st_speed_config_.total_time() + t_output_resolution) { double s = spline(time); double v = spline.derivative(time); double a = spline.second_order_derivative(time); double da = spline.third_order_derivative(time); speed_data->add_speed_point(s, time, v, a, da); - time += time_resolution; + time += t_output_resolution; } return Status::OK(); @@ -228,7 +230,8 @@ Status QpSplineStGraph::ApplyKernel( for (uint32_t i = 0; i <= qp_spline_st_speed_config_.number_of_discrete_graph_t(); ++i) { s_vec.push_back(dist_ref); - dist_ref += time_resolution_ * speed_limit.get_speed_limit_by_s(dist_ref); + dist_ref += + t_knots_resolution_ * speed_limit.get_speed_limit_by_s(dist_ref); } // TODO: change reference line kernel to configurable version spline_kernel->add_reference_line_kernel_matrix(t_knots_, s_vec, 1); diff --git a/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h b/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h index 7220306438..a95768b82e 100644 --- a/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h +++ b/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.h @@ -90,12 +90,12 @@ class QpSplineStGraph { std::unique_ptr spline_generator_ = nullptr; // time resolution - double time_resolution_ = 0.0; + double t_knots_resolution_ = 0.0; // knots std::vector t_knots_; - double evaluation_time_resolution_ = 0.0; + double t_evaluated_resolution_ = 0.0; // evaluated points std::vector t_evaluated_; }; diff --git a/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc b/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc index b6ed5ccd85..ef3130e07d 100644 --- a/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc +++ b/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc @@ -74,7 +74,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data, // step 1 get boundaries std::vector boundaries; if (!boundary_mapper_ - .get_graph_boundary( + .GetGraphBoundary( init_point, *decision_data, path_data, reference_line, qp_spline_st_speed_config_.total_path_length(), qp_spline_st_speed_config_.total_time(), &boundaries) @@ -86,7 +86,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data, SpeedLimit speed_limits; const auto& pose = apollo::common::VehicleState::instance()->pose(); const auto* pnc_map = frame_->PncMap(); - if (boundary_mapper_.get_speed_limits(pose, pnc_map, path_data, total_length, + if (boundary_mapper_.GetSpeedLimits(pose, pnc_map, path_data, total_length, qp_spline_st_speed_config_.total_time(), qp_spline_st_speed_config_.max_speed(), &speed_limits) != Status::OK()) { diff --git a/modules/planning/optimizer/st_graph/st_boundary_mapper.cc b/modules/planning/optimizer/st_graph/st_boundary_mapper.cc index 9532c20ce5..5de3ae45c2 100644 --- a/modules/planning/optimizer/st_graph/st_boundary_mapper.cc +++ b/modules/planning/optimizer/st_graph/st_boundary_mapper.cc @@ -83,7 +83,7 @@ bool StBoundaryMapper::check_overlap(const PathPoint& path_point, return obs_box.HasOverlap(adc_box); } -Status StBoundaryMapper::get_speed_limits( +Status StBoundaryMapper::GetSpeedLimits( const Pose& pose, const PncMap* pnc_map, const PathData& path_data, const double planning_distance, const std::uint32_t matrix_dimension_s, const double default_speed_limit, SpeedLimit* const speed_limit_data) { @@ -93,14 +93,14 @@ Status StBoundaryMapper::get_speed_limits( adc_point.set_x(adc_position.x()); adc_point.set_y(adc_position.y()); adc_point.set_z(adc_position.z()); - // std::vector lanes; + std::vector> lanes; int ret = pnc_map->HDMap()->get_lanes(adc_point, 1.0, &lanes); if (ret != 0) { AERROR << "Fail to get lanes for point [" << adc_position.x() << ", " << adc_position.y() << "]."; return Status(ErrorCode::PLANNING_ERROR, - "StBoundaryMapper::get_speed_limits"); + "StBoundaryMapper::GetSpeedLimits"); } std::vector speed_limits; diff --git a/modules/planning/optimizer/st_graph/st_boundary_mapper.h b/modules/planning/optimizer/st_graph/st_boundary_mapper.h index f57b68910a..bbaa391f76 100644 --- a/modules/planning/optimizer/st_graph/st_boundary_mapper.h +++ b/modules/planning/optimizer/st_graph/st_boundary_mapper.h @@ -47,14 +47,14 @@ class StBoundaryMapper { virtual ~StBoundaryMapper() = default; bool Init(const std::string& config_file); - virtual apollo::common::Status get_graph_boundary( + virtual apollo::common::Status GetGraphBoundary( const common::TrajectoryPoint& initial_planning_point, const DecisionData& decision_data, const PathData& path_data, const ReferenceLine& reference_line, const double planning_distance, const double planning_time, std::vector* const boundary) const = 0; - virtual apollo::common::Status get_speed_limits( + virtual apollo::common::Status GetSpeedLimits( const apollo::localization::Pose& pose, const apollo::hdmap::PncMap* map, const PathData& path_data, const double planning_distance, const std::uint32_t matrix_dimension_s, const double default_speed_limit, diff --git a/modules/planning/optimizer/st_graph/st_boundary_mapper_impl.cc b/modules/planning/optimizer/st_graph/st_boundary_mapper_impl.cc index 8e04457bbc..11ba05ce49 100644 --- a/modules/planning/optimizer/st_graph/st_boundary_mapper_impl.cc +++ b/modules/planning/optimizer/st_graph/st_boundary_mapper_impl.cc @@ -49,7 +49,7 @@ using Box2d = apollo::common::math::Box2d; using Vec2d = apollo::common::math::Vec2d; using VehicleConfigHelper = apollo::common::VehicleConfigHelper; -Status StBoundaryMapperImpl::get_graph_boundary( +Status StBoundaryMapperImpl::GetGraphBoundary( const common::TrajectoryPoint& initial_planning_point, const DecisionData& decision_data, const PathData& path_data, const ReferenceLine& reference_line, const double planning_distance, diff --git a/modules/planning/optimizer/st_graph/st_boundary_mapper_impl.h b/modules/planning/optimizer/st_graph/st_boundary_mapper_impl.h index d04f851604..a4a4092c51 100644 --- a/modules/planning/optimizer/st_graph/st_boundary_mapper_impl.h +++ b/modules/planning/optimizer/st_graph/st_boundary_mapper_impl.h @@ -35,7 +35,7 @@ namespace planning { class StBoundaryMapperImpl : public StBoundaryMapper { public: - apollo::common::Status get_graph_boundary( + apollo::common::Status GetGraphBoundary( const common::TrajectoryPoint& initial_planning_point, const DecisionData& decision_data, const PathData& path_data, const ReferenceLine& reference_line, const double planning_distance, diff --git a/modules/planning/optimizer/st_graph/st_graph_boundary.cc b/modules/planning/optimizer/st_graph/st_graph_boundary.cc index dd82ec33cb..478810b81f 100644 --- a/modules/planning/optimizer/st_graph/st_graph_boundary.cc +++ b/modules/planning/optimizer/st_graph/st_graph_boundary.cc @@ -29,42 +29,36 @@ namespace planning { using Vec2d = common::math::Vec2d; -StGraphBoundary::StGraphBoundary(const std::vector& points) { - CHECK_GE(points.size(), 4); - for (const auto& point : points) { - points_.emplace_back(point.t(), point.s()); - } - BuildFromPoints(); +StGraphBoundary::StGraphBoundary(const std::vector& points) + : Polygon2d(std::vector(points.begin(), points.end())) { + CHECK_EQ(points.size(), 4) + << "StGraphBoundary must have exactly four points. Input points size: " + << points.size(); } StGraphBoundary::StGraphBoundary( const std::vector<::apollo::common::math::Vec2d>& points) : Polygon2d(points) { - CHECK_GE(points.size(), 4); + CHECK_EQ(points.size(), 4) + << "StGraphBoundary must have exactly four points. Input points size: " + << points.size(); } bool StGraphBoundary::IsPointInBoundary( const StGraphPoint& st_graph_point) const { - ::apollo::common::math::Vec2d vec2d = {st_graph_point.point().t(), - st_graph_point.point().s()}; - return IsPointIn(vec2d); + return IsPointInBoundary(st_graph_point.point()); } bool StGraphBoundary::IsPointInBoundary(const STPoint& st_point) const { - ::apollo::common::math::Vec2d vec2d = {st_point.t(), st_point.s()}; - return IsPointIn(vec2d); + return IsPointIn(st_point); } -const ::apollo::common::math::Vec2d StGraphBoundary::point( - const uint32_t index) const { - CHECK_LT(index, points_.size()); +Vec2d StGraphBoundary::point(const uint32_t index) const { + CHECK_LT(index, points_.size()) << "Index[" << index << "] is out of range."; return points_[index]; } -const std::vector<::apollo::common::math::Vec2d>& StGraphBoundary::points() - const { - return points_; -} +const std::vector& StGraphBoundary::points() const { return points_; } bool StGraphBoundary::is_empty() const { return points_.empty(); } diff --git a/modules/planning/optimizer/st_graph/st_graph_boundary.h b/modules/planning/optimizer/st_graph/st_graph_boundary.h index 8f67d56cd3..2f9504d815 100644 --- a/modules/planning/optimizer/st_graph/st_graph_boundary.h +++ b/modules/planning/optimizer/st_graph/st_graph_boundary.h @@ -40,8 +40,6 @@ class StGraphBoundary : public common::math::Polygon2d { FOLLOW, YIELD, OVERTAKE, - SIDEPASSFOLLOW, - SIDEPASSLEAD, }; explicit StGraphBoundary(const std::vector& points); @@ -53,7 +51,7 @@ class StGraphBoundary : public common::math::Polygon2d { bool IsPointInBoundary(const StGraphPoint& st_graph_point) const; bool IsPointInBoundary(const STPoint& st_point) const; - const common::math::Vec2d point(const uint32_t index) const; + common::math::Vec2d point(const uint32_t index) const; const std::vector& points() const; BoundaryType boundary_type() const; @@ -82,4 +80,4 @@ class StGraphBoundary : public common::math::Polygon2d { } // end namespace planning } // end namespace apollo -#endif // BAIDU_ADU_PLANNING_OTIMIZER_COMMON_ST_GRAPH_BOUNDARY_H_ +#endif // BAIDU_ADU_PLANNING_OPTIMIZER_COMMON_ST_GRAPH_BOUNDARY_H_ -- GitLab