提交 7c67350d 编写于 作者: Z Zhang Liangliang 提交者: Jiangtao Hu

removed veh_param in st_boundary_mapper.

上级 84c3af60
......@@ -37,9 +37,8 @@ using apollo::common::PathPoint;
using apollo::common::Status;
DpStBoundaryMapper::DpStBoundaryMapper(
const StBoundaryConfig& st_boundary_config,
const ::apollo::common::config::VehicleParam& veh_param)
: StBoundaryMapper(st_boundary_config, veh_param) {}
const StBoundaryConfig& st_boundary_config)
: StBoundaryMapper(st_boundary_config) {}
Status DpStBoundaryMapper::get_graph_boundary(
const common::TrajectoryPoint& initial_planning_point,
......@@ -131,7 +130,6 @@ Status DpStBoundaryMapper::map_obstacle_with_trajectory(
std::uint32_t high_index = veh_path.size() - 1;
bool find_low = false;
bool find_high = false;
while (high_index >= low_index) {
if (find_high && find_low) {
break;
......@@ -202,7 +200,6 @@ Status DpStBoundaryMapper::map_obstacle_without_trajectory(
std::uint32_t high_index = veh_path.size() - 1;
bool find_low = false;
bool find_high = false;
while (high_index >= low_index) {
if (find_high && find_low) {
break;
......
......@@ -32,8 +32,7 @@ namespace planning {
class DpStBoundaryMapper : public StBoundaryMapper {
public:
DpStBoundaryMapper(const StBoundaryConfig& st_boundary_config,
const ::apollo::common::config::VehicleParam& veh_param);
DpStBoundaryMapper(const StBoundaryConfig& st_boundary_config);
// TODO: combine two interfaces together to provide a st graph data type
apollo::common::Status get_graph_boundary(
......
......@@ -62,10 +62,8 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
const ReferenceLine& reference_line,
DecisionData* const decision_data,
SpeedData* const speed_data) {
const auto& veh_param = VehicleConfigHelper::GetConfig().vehicle_param();
// step 1 get boundaries
DpStBoundaryMapper st_mapper(st_boundary_config_, veh_param);
DpStBoundaryMapper st_mapper(st_boundary_config_);
double planning_distance = std::min(dp_st_speed_config_.total_path_length(),
path_data.path().param_length());
std::vector<StGraphBoundary> boundaries;
......
......@@ -44,8 +44,8 @@ using Box2d = apollo::common::math::Box2d;
using Vec2d = apollo::common::math::Vec2d;
QpSplineStBoundaryMapper::QpSplineStBoundaryMapper(
const StBoundaryConfig& st_boundary_config, const VehicleParam& veh_param)
: StBoundaryMapper(st_boundary_config, veh_param) {}
const StBoundaryConfig& st_boundary_config)
: StBoundaryMapper(st_boundary_config) {}
Status QpSplineStBoundaryMapper::get_graph_boundary(
const common::TrajectoryPoint& initial_planning_point,
......
......@@ -35,9 +35,7 @@ namespace planning {
class QpSplineStBoundaryMapper : public StBoundaryMapper {
public:
QpSplineStBoundaryMapper(
const StBoundaryConfig& st_boundary_config,
const apollo::common::config::VehicleParam& veh_param);
QpSplineStBoundaryMapper(const StBoundaryConfig& st_boundary_config);
apollo::common::Status get_graph_boundary(
const common::TrajectoryPoint& initial_planning_point,
......
......@@ -67,13 +67,12 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
AERROR << "Please call Init() before Process.";
return Status(ErrorCode::PLANNING_ERROR, "Not init.");
}
double total_length = std::min(qp_spline_st_speed_config_.total_path_length(),
path_data.path().param_length());
// step 1 get boundaries
const auto& veh_param =
common::config::VehicleConfigHelper::GetConfig().vehicle_param();
QpSplineStBoundaryMapper st_mapper(st_boundary_config_, veh_param);
QpSplineStBoundaryMapper st_mapper(st_boundary_config_);
std::vector<StGraphBoundary> boundaries;
if (st_mapper.get_graph_boundary(
init_point, *decision_data, path_data, reference_line,
......@@ -96,6 +95,8 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
}
// step 2 perform graph search
const auto& veh_param =
common::config::VehicleConfigHelper::GetConfig().vehicle_param();
QpSplineStGraph st_graph(qp_spline_st_speed_config_, veh_param);
StGraphData st_graph_data(boundaries, init_point, speed_limits,
......
......@@ -57,6 +57,7 @@ cc_library(
],
deps = [
":st_graph_boundary",
"//modules/common/configs:vehicle_config_helper",
"//modules/common/configs/proto:vehicle_config_proto",
"//modules/common/proto:path_point_proto",
"//modules/common/status",
......
......@@ -38,10 +38,8 @@ using apollo::common::Status;
using apollo::hdmap::HDMap;
using apollo::localization::Pose;
StBoundaryMapper::StBoundaryMapper(
const StBoundaryConfig& st_boundary_config,
const apollo::common::config::VehicleParam& veh_param)
: _st_boundary_config(st_boundary_config), _veh_param(veh_param) {}
StBoundaryMapper::StBoundaryMapper(const StBoundaryConfig& st_boundary_config)
: _st_boundary_config(st_boundary_config) {}
double StBoundaryMapper::get_area(
const std::vector<STPoint>& boundary_points) const {
......@@ -174,10 +172,5 @@ const StBoundaryConfig& StBoundaryMapper::st_boundary_config() const {
return _st_boundary_config;
}
const apollo::common::config::VehicleParam& StBoundaryMapper::vehicle_param()
const {
return _veh_param;
}
} // namespace planning
} // namespace apollo
......@@ -23,12 +23,12 @@
#include <vector>
#include "modules/common/configs/proto/vehicle_config.pb.h"
#include "modules/common/proto/path_point.pb.h"
#include "modules/common/status/status.h"
#include "modules/localization/proto/pose.pb.h"
#include "modules/planning/proto/st_boundary_config.pb.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/map/hdmap/hdmap.h"
#include "modules/planning/common/decision_data.h"
#include "modules/planning/common/path/path_data.h"
......@@ -42,8 +42,8 @@ namespace planning {
class StBoundaryMapper {
public:
StBoundaryMapper(const apollo::planning::StBoundaryConfig& st_boundary_config,
const apollo::common::config::VehicleParam& veh_param);
StBoundaryMapper(
const apollo::planning::StBoundaryConfig& st_boundary_config);
virtual ~StBoundaryMapper() = default;
......@@ -60,12 +60,13 @@ class StBoundaryMapper {
const std::uint32_t matrix_dimension_s, const double default_speed_limit,
SpeedLimit* const speed_limit_data);
protected:
const apollo::planning::StBoundaryConfig& st_boundary_config() const;
const apollo::common::config::VehicleParam& vehicle_param() const;
const apollo::common::config::VehicleParam& vehicle_param() const {
return common::config::VehicleConfigHelper::GetConfig().vehicle_param();
}
protected:
double get_area(const std::vector<STPoint>& boundary_points) const;
bool check_overlap(const apollo::common::PathPoint& path_point,
const apollo::common::config::VehicleParam& params,
const apollo::common::math::Box2d& obs_box,
......@@ -73,7 +74,6 @@ class StBoundaryMapper {
private:
apollo::planning::StBoundaryConfig _st_boundary_config;
apollo::common::config::VehicleParam _veh_param;
};
} // namespace planning
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册