diff --git a/modules/planning/common/path/discretized_path.cc b/modules/planning/common/path/discretized_path.cc index e9f253019af76acd3a2cb5c7fb08672b713b29e5..b6d8aa439d11f825045da335100b460a9174417b 100644 --- a/modules/planning/common/path/discretized_path.cc +++ b/modules/planning/common/path/discretized_path.cc @@ -49,7 +49,7 @@ common::PathPoint DiscretizedPath::evaluate(const double param) const { return util::interpolate(*(it_lower - 1), *it_lower, param); } -double DiscretizedPath::param_length() const { +double DiscretizedPath::length() const { if (path_points_.empty()) { return 0.0; } @@ -93,11 +93,7 @@ int DiscretizedPath::query_closest_point(const double param) const { } } -std::vector *DiscretizedPath::mutable_path_points() { - return &path_points_; -} - -const std::vector &DiscretizedPath::path_points() const { +const std::vector &DiscretizedPath::points() const { return path_points_; } diff --git a/modules/planning/common/path/discretized_path.h b/modules/planning/common/path/discretized_path.h index 69ffd43fd59e1f10db0d9d20f110faca96c10211..1555f82ecb1dea7a979c61b2cc75c4fbde7412a8 100644 --- a/modules/planning/common/path/discretized_path.h +++ b/modules/planning/common/path/discretized_path.h @@ -40,7 +40,7 @@ class DiscretizedPath : public Path { common::PathPoint evaluate(const double param) const override; - double param_length() const override; + double length() const override; common::PathPoint start_point() const override; @@ -50,9 +50,7 @@ class DiscretizedPath : public Path { int query_closest_point(const double param) const; - std::vector *mutable_path_points(); - - const std::vector &path_points() const; + const std::vector &points() const; std::uint32_t num_of_points() const; diff --git a/modules/planning/common/path/frenet_frame_path.cc b/modules/planning/common/path/frenet_frame_path.cc index ea3543e74d48d16cb786fe5be244fd74406a8ec4..aa94bf02f49da1ba82d0cdc0b04ac1606802dfda 100644 --- a/modules/planning/common/path/frenet_frame_path.cc +++ b/modules/planning/common/path/frenet_frame_path.cc @@ -32,19 +32,22 @@ FrenetFramePath::FrenetFramePath( points_ = std::move(sl_points); } -void FrenetFramePath::set_frenet_points( +void FrenetFramePath::set_points( const std::vector &points) { points_ = points; } -std::vector *FrenetFramePath::mutable_points() { - return &points_; -} - const std::vector &FrenetFramePath::points() const { return points_; } +double FrenetFramePath::length() const { + if (points_.empty()) { + return 0.0; + } + return points_.back().s() - points_.front().s(); +} + std::uint32_t FrenetFramePath::number_of_points() const { return points_.size(); } diff --git a/modules/planning/common/path/frenet_frame_path.h b/modules/planning/common/path/frenet_frame_path.h index 4d51697ba8138a0bf7235251007493e86249015e..92eff5e7a1ea6aab836e1dc8ec9d623fdaa3d121 100644 --- a/modules/planning/common/path/frenet_frame_path.h +++ b/modules/planning/common/path/frenet_frame_path.h @@ -34,10 +34,10 @@ class FrenetFramePath { explicit FrenetFramePath(std::vector sl_points); virtual ~FrenetFramePath() = default; - void set_frenet_points(const std::vector &points); - std::vector *mutable_points(); + void set_points(const std::vector &points); const std::vector &points() const; std::uint32_t number_of_points() const; + double length() const; const common::FrenetFramePoint &point_at(const std::uint32_t index) const; common::FrenetFramePoint &point_at(const std::uint32_t index); diff --git a/modules/planning/common/path/path.h b/modules/planning/common/path/path.h index 62c25da74db0d74d6e45f09d74c5907bae21eaae..c33a11fd74e4945ba4df1a39a24f104239e97f84 100644 --- a/modules/planning/common/path/path.h +++ b/modules/planning/common/path/path.h @@ -34,7 +34,7 @@ class Path { virtual common::PathPoint evaluate(const double param) const = 0; - virtual double param_length() const = 0; + virtual double length() const = 0; virtual common::PathPoint start_point() const = 0; diff --git a/modules/planning/common/path/path_data.cc b/modules/planning/common/path/path_data.cc index d655634ee80190028daf50d4f6a8ab81ba227491..d850e512c252d97a380ab6d8cf995fcaeff1fbd7 100644 --- a/modules/planning/common/path/path_data.cc +++ b/modules/planning/common/path/path_data.cc @@ -31,22 +31,22 @@ namespace apollo { namespace planning { -void PathData::set_path(const DiscretizedPath &path) { path_ = path; } +void PathData::set_discretized_path(const DiscretizedPath &path) { + discretized_path_ = path; +} void PathData::set_frenet_path(const FrenetFramePath &frenet_path) { frenet_path_ = frenet_path; } -DiscretizedPath *PathData::mutable_path() { return &path_; } - -void PathData::set_path_points( +void PathData::set_discretized_path( const std::vector &path_points) { - path_.set_points(path_points); + discretized_path_.set_points(path_points); } -const DiscretizedPath &PathData::path() const { return path_; } - -FrenetFramePath *PathData::mutable_frenet_frame_path() { return &frenet_path_; } +const DiscretizedPath &PathData::discretized_path() const { + return discretized_path_; +} const FrenetFramePath &PathData::frenet_frame_path() const { return frenet_path_; @@ -54,7 +54,7 @@ const FrenetFramePath &PathData::frenet_frame_path() const { bool PathData::get_path_point_with_path_s( const double s, common::PathPoint *const path_point) const { - *path_point = path_.evaluate_linear_approximation(s); + *path_point = discretized_path_.evaluate_linear_approximation(s); return true; } @@ -72,7 +72,7 @@ bool PathData::get_path_point_with_ref_s( auto it_lower = std::lower_bound(frenet_points.begin(), frenet_points.end(), ref_s, comp); if (it_lower == frenet_points.begin()) { - *path_point = path_.path_points().front(); + *path_point = discretized_path_.points().front(); } else { // std::uint32_t index_lower = (std::uint32_t)(it_lower - // frenet_points.begin()); @@ -93,7 +93,7 @@ bool PathData::get_path_point_with_ref_s( std::string PathData::DebugString() const { std::ostringstream sout; sout << "[" << std::endl; - const auto &path_points = path_.path_points(); + const auto &path_points = discretized_path_.points(); for (std::size_t i = 0; i < path_points.size() && i < static_cast(FLAGS_trajectory_point_num_for_debug); diff --git a/modules/planning/common/path/path_data.h b/modules/planning/common/path/path_data.h index c9b6e9be419db8711fe130b71b64a2a4c6a1a321..51db7503b30fd5440cf424fb9fb9731119773e5f 100644 --- a/modules/planning/common/path/path_data.h +++ b/modules/planning/common/path/path_data.h @@ -33,17 +33,13 @@ class PathData { public: PathData() = default; - void set_path(const DiscretizedPath &path); + void set_discretized_path(const DiscretizedPath &path); - void set_path_points(const std::vector &path_points); + void set_discretized_path(const std::vector &path_points); void set_frenet_path(const FrenetFramePath &frenet_path); - DiscretizedPath *mutable_path(); - - const DiscretizedPath &path() const; - - FrenetFramePath *mutable_frenet_frame_path(); + const DiscretizedPath &discretized_path() const; const FrenetFramePath &frenet_frame_path() const; @@ -57,7 +53,7 @@ class PathData { // TODO(fanhaoyang) add check if the path data is valid private: - DiscretizedPath path_; + DiscretizedPath discretized_path_; FrenetFramePath frenet_path_; }; diff --git a/modules/planning/common/planning_data.cc b/modules/planning/common/planning_data.cc index dfe3ae392c987715a5882ca0e05f5c7cdc544a7b..24a1d90e27fce47e3b99236dafc28e4879ad1fd6 100644 --- a/modules/planning/common/planning_data.cc +++ b/modules/planning/common/planning_data.cc @@ -88,13 +88,15 @@ bool PlanningData::aggregate(const double time_resolution, common::PathPoint path_point; // TODO(all): temp fix speed point s out of path point bound, need further // refine later - if (speed_point.s() > path_data_.path().param_length()) { + if (speed_point.s() > path_data_.discretized_path().length()) { break; } - QUIT_IF( - !path_data_.get_path_point_with_path_s(speed_point.s(), &path_point), - false, ERROR, "Fail to get path data with s %f, path total length %f", - speed_point.s(), path_data_.path().param_length()); + if (!path_data_.get_path_point_with_path_s(speed_point.s(), &path_point)) { + AERROR << "Fail to get path data with s " << speed_point.s() + << "path total length " << path_data_.discretized_path().length(); + ; + return false; + } common::TrajectoryPoint trajectory_point; trajectory_point.mutable_path_point()->CopyFrom(path_point); diff --git a/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc b/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc index e2007b1006c795356dd4c6020d3b36663e63d3fd..f8e23234b37878891e732858852d883f430c1cb3 100644 --- a/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc +++ b/modules/planning/optimizer/dp_poly_path/dp_road_graph.cc @@ -125,8 +125,7 @@ bool DpRoadGraph::find_tunnel(const ReferenceLine &reference_line, } path_points.push_back(std::move(path_point)); } - DiscretizedPath discretized_path(path_points); - path_data->set_path(discretized_path); + path_data->set_discretized_path(path_points); return true; } diff --git a/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc b/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc index 06d5a73e985eded0db9112e5ba3f02dd39607766..c7784b8d60c4d251e8e7de45e6fd5fcfe3283494 100644 --- a/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc +++ b/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc @@ -86,7 +86,7 @@ TEST_F(DpRoadGraphTest, speed_road_graph) { bool result = road_graph.find_tunnel(*reference_line_, &decision_data_, &path_data_); EXPECT_TRUE(result); - EXPECT_EQ(706, path_data_.path().num_of_points()); + EXPECT_EQ(706, path_data_.discretized_path().num_of_points()); EXPECT_EQ(706, path_data_.frenet_frame_path().number_of_points()); EXPECT_FLOAT_EQ(69.9, path_data_.frenet_frame_path().points().back().s()); EXPECT_FLOAT_EQ(0.0, path_data_.frenet_frame_path().points().back().l()); diff --git a/modules/planning/optimizer/dp_st_speed/dp_st_boundary_mapper.cc b/modules/planning/optimizer/dp_st_speed/dp_st_boundary_mapper.cc index 2ac0d11e59214b86422f1babbb21a609315702b3..86c978f0562a0d464b6f7ed04f99e4522dcc9716 100644 --- a/modules/planning/optimizer/dp_st_speed/dp_st_boundary_mapper.cc +++ b/modules/planning/optimizer/dp_st_speed/dp_st_boundary_mapper.cc @@ -49,9 +49,9 @@ Status DpStBoundaryMapper::get_graph_boundary( return Status(ErrorCode::PLANNING_ERROR, msg); } - if (path_data.path().num_of_points() < 2) { + if (path_data.discretized_path().num_of_points() < 2) { AERROR << "Fail to get params since path has " - << path_data.path().num_of_points() << " points."; + << path_data.discretized_path().num_of_points() << " points."; return Status(ErrorCode::PLANNING_ERROR, "Fail to get params"); } @@ -103,7 +103,8 @@ Status DpStBoundaryMapper::map_obstacle_with_trajectory( // lower and upper bound for st boundary std::vector lower_points; std::vector upper_points; - const std::vector& veh_path = path_data.path().path_points(); + const std::vector& veh_path = + path_data.discretized_path().points(); for (std::uint32_t i = 0; i < obstacle.prediction_trajectories().size(); ++i) { PredictionTrajectory pred_traj = obstacle.prediction_trajectories()[i]; @@ -181,7 +182,8 @@ Status DpStBoundaryMapper::map_obstacle_without_trajectory( const double planning_distance, const double planning_time, std::vector* const boundary) const { // Static obstacle only have yield option - const std::vector& veh_path = path_data.path().path_points(); + const std::vector& veh_path = + path_data.discretized_path().points(); ::apollo::common::math::Box2d obs_box = obstacle.BoundingBox(); 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 e3ae4e222187d1e45687450c2c0ef6a7bfc3697a..f96a7a00493bb6b30cb794147f8df016e4b38486 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 @@ -65,9 +65,10 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data, return Status(ErrorCode::PLANNING_ERROR, "Not inited."); } + const double path_length = path_data.discretized_path().length(); // step 1 get boundaries - double planning_distance = std::min(dp_st_speed_config_.total_path_length(), - path_data.path().param_length()); + double planning_distance = + std::min(dp_st_speed_config_.total_path_length(), path_length); std::vector boundaries; common::TrajectoryPoint initial_planning_point = DataCenter::instance() ->current_frame() @@ -100,8 +101,7 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data, return Status(ErrorCode::PLANNING_ERROR, msg); } - StGraphData st_graph_data(boundaries, init_point, speed_limit, - path_data.path().param_length()); + StGraphData st_graph_data(boundaries, init_point, speed_limit, path_length); DpStGraph st_graph(dp_st_speed_config_); if (!st_graph.search(st_graph_data, decision_data, speed_data).ok()) { diff --git a/modules/planning/optimizer/optimizer_test_base.cc b/modules/planning/optimizer/optimizer_test_base.cc index a0155d3e2749e79f8b37f1f1dbbb74a742c6944b..82fa4a20e529bcc68252e6ab670cd91f532f113d 100644 --- a/modules/planning/optimizer/optimizer_test_base.cc +++ b/modules/planning/optimizer/optimizer_test_base.cc @@ -85,7 +85,7 @@ void OptimizerTestBase::export_path_data(const PathData& path_data, std::ofstream ofs(filename); ofs << "s, l, dl, ddl, x, y, z" << std::endl; const auto& frenet_path = path_data.frenet_frame_path(); - const auto& discrete_path = path_data.path(); + const auto& discrete_path = path_data.discretized_path(); if (frenet_path.number_of_points() != discrete_path.num_of_points()) { AERROR << "frenet_path and discrete path have different number of points"; return; diff --git a/modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc b/modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc index 4ee2e469972bc93f79d8fe863e0d69ea62d6cc1a..7bda6c5bee9e19f5b1e49d38bd034dfbff9cf9e9 100644 --- a/modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc +++ b/modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc @@ -134,7 +134,7 @@ bool QpSplinePathGenerator::generate(const ReferenceLine& reference_line, path_points.push_back(std::move(path_point)); s += s_resolution; } - path_data->set_path_points(path_points); + path_data->set_discretized_path(path_points); return true; } diff --git a/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_boundary_mapper.cc b/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_boundary_mapper.cc index 3a7d84b917ffd84edfcba18ad03a076dc27b9204..be68c884482b4f1f7ff59b54d0a244faf4639c4f 100644 --- a/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_boundary_mapper.cc +++ b/modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_boundary_mapper.cc @@ -67,10 +67,10 @@ Status QpSplineStBoundaryMapper::get_graph_boundary( return Status(ErrorCode::PLANNING_ERROR, msg); } - if (path_data.path().num_of_points() < 2) { + if (path_data.discretized_path().num_of_points() < 2) { AERROR << "Fail to get params because of too few path points. path points " "size: " - << path_data.path().num_of_points() << "."; + << path_data.discretized_path().num_of_points() << "."; return Status(ErrorCode::PLANNING_ERROR, "Fail to get params because of too few path points"); } @@ -250,7 +250,7 @@ Status QpSplineStBoundaryMapper::map_obstacle_with_prediction_trajectory( bool skip = true; std::vector boundary_points; - const auto& adc_path_points = path_data.path().path_points(); + const auto& adc_path_points = path_data.discretized_path().points(); if (obstacle.prediction_trajectories().size() == 0) { AWARN << "Obstacle (id = " << obstacle.Id() << ") has NO prediction trajectory."; @@ -271,7 +271,7 @@ Status QpSplineStBoundaryMapper::map_obstacle_with_prediction_trajectory( obstacle.Length() * st_boundary_config().expending_coeff(), obstacle.Width() * st_boundary_config().expending_coeff()); int64_t low = 0; - int64_t high = static_cast(path_data.path().num_of_points()) - 1; + int64_t high = adc_path_points.size() - 1; bool find_low = false; bool find_high = false; while (low < high) { 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 36ecbf3c8c5b5d6e1fb24aef7bcbe03a00d7eb70..591a16e024ab2e4bd4a204101fe0d7d0347d0473 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 @@ -71,7 +71,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data, } double total_length = std::min(qp_spline_st_speed_config_.total_path_length(), - path_data.path().param_length()); + path_data.discretized_path().length()); // step 1 get boundaries std::vector boundaries; @@ -102,7 +102,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data, QpSplineStGraph st_graph(qp_spline_st_speed_config_, veh_param); StGraphData st_graph_data(boundaries, init_point, speed_limits, - path_data.path().param_length()); + path_data.discretized_path().length()); if (st_graph.search(st_graph_data, path_data, speed_data) != Status::OK()) { return Status(ErrorCode::PLANNING_ERROR, "Failed to search graph with dynamic programming!"); diff --git a/modules/planning/optimizer/st_graph/st_boundary_mapper.cc b/modules/planning/optimizer/st_graph/st_boundary_mapper.cc index cf9bff3b373360c1db24e7a501f146ce2e0072de..87e8ba2eb57d70e63eb1c4638de273d729378a41 100644 --- a/modules/planning/optimizer/st_graph/st_boundary_mapper.cc +++ b/modules/planning/optimizer/st_graph/st_boundary_mapper.cc @@ -66,10 +66,10 @@ double StBoundaryMapper::get_area( return fabs(area); } -bool StBoundaryMapper::check_overlap( - const PathPoint& path_point, - const apollo::common::VehicleParam& params, - const apollo::common::math::Box2d& obs_box, const double buffer) const { +bool StBoundaryMapper::check_overlap(const PathPoint& path_point, + const apollo::common::VehicleParam& params, + const apollo::common::math::Box2d& obs_box, + const double buffer) const { const double mid_to_rear_center = params.length() / 2.0 - params.front_edge_to_center(); const double x = @@ -103,7 +103,8 @@ Status StBoundaryMapper::get_speed_limits( } std::vector speed_limits; - for (const auto& point : path_data.path().path_points()) { + const auto& path_points = path_data.discretized_path().points(); + for (const auto& point : path_points) { speed_limits.push_back(_st_boundary_config.maximal_speed()); for (auto& lane : lanes) { if (lane->is_on_lane({point.x(), point.y()})) { @@ -117,7 +118,7 @@ Status StBoundaryMapper::get_speed_limits( } } - if (planning_distance > path_data.path().path_points().back().s()) { + if (planning_distance > path_points.back().s()) { const std::string msg = "path length cannot be less than planning_distance"; AERROR << msg; return Status(ErrorCode::PLANNING_ERROR, msg); @@ -127,7 +128,6 @@ Status StBoundaryMapper::get_speed_limits( const double unit_s = planning_distance / matrix_dimension_s; std::uint32_t i = 0; std::uint32_t j = 1; - const auto& path_points = path_data.path().path_points(); while (i < matrix_dimension_s && j < path_points.size()) { const auto& point = path_points[j]; const auto& pre_point = path_points[j - 1]; @@ -153,7 +153,6 @@ Status StBoundaryMapper::get_speed_limits( std::fmin(curr_speed_limit, std::fmin(_st_boundary_config.maximal_speed(), speed)); } - curr_speed_limit *= _st_boundary_config.speed_multiply_buffer(); curr_speed_limit = std::fmax(curr_speed_limit, _st_boundary_config.lowest_speed()); diff --git a/modules/planning/planner/em/em_planner.cc b/modules/planning/planner/em/em_planner.cc index 3d8ba4fc5b6eaebd3ecc4588e8b16d844057d424..20af6033dcdb8987a6d425de3d88a68c21c93d00 100644 --- a/modules/planning/planner/em/em_planner.cc +++ b/modules/planning/planner/em/em_planner.cc @@ -84,7 +84,7 @@ Status EMPlanner::Init(const PlanningConfig& config) { } Status EMPlanner::Plan(const TrajectoryPoint& start_point, - ADCTrajectory* trajectory_pb) { + ADCTrajectory* trajectory_pb) { DataCenter* data_center = DataCenter::instance(); Frame* frame = data_center->current_frame(); @@ -109,12 +109,12 @@ Status EMPlanner::Plan(const TrajectoryPoint& start_point, CHECK(OptimizerType_Parse(optimizer->name(), &type)); if (type == DP_POLY_PATH_OPTIMIZER || type == QP_SPLINE_PATH_OPTIMIZER) { const auto& path_points = - planning_data->path_data().path().path_points(); + planning_data->path_data().discretized_path().points(); auto* optimized_path = trajectory_pb->mutable_debug()->mutable_planning_data()->add_path(); optimized_path->set_name(optimizer->name()); - optimized_path->mutable_path_point()->CopyFrom({path_points.begin(), - path_points.end()}); + optimized_path->mutable_path_point()->CopyFrom( + {path_points.begin(), path_points.end()}); } } } @@ -135,8 +135,8 @@ Status EMPlanner::Plan(const TrajectoryPoint& start_point, reference_line->set_name("planning_reference_line"); const auto& reference_points = planning_data->reference_line().reference_points(); - reference_line->mutable_path_point()->CopyFrom({reference_points.begin(), - reference_points.end()}); + reference_line->mutable_path_point()->CopyFrom( + {reference_points.begin(), reference_points.end()}); } return Status::OK();