提交 e1d2f4ea 编写于 作者: Z Zhang Liangliang 提交者: Liangliang Zhang

Planning: restructured codes for two types of smoother (qp_spline and spiral).

上级 c459326d
......@@ -294,30 +294,25 @@ bool Frame::CreateReferenceLineFromRouting(
return false;
}
QpSplineReferenceLineSmoother smoother;
std::unique_ptr<ReferenceLineSmoother> smoother;
std::vector<double> init_t_knots;
Spline2dSolver spline_solver(init_t_knots, smoother_config_.spline_order());
smoother.Init(smoother_config_, &spline_solver);
SpiralReferenceLineSmoother spiral_smoother;
double max_spiral_smoother_dev = 0.1;
spiral_smoother.Init(max_spiral_smoother_dev);
if (FLAGS_enable_spiral_reference_line) {
double max_deviation = 0.1;
smoother.reset(new SpiralReferenceLineSmoother(max_deviation));
} else {
smoother.reset(
new QpSplineReferenceLineSmoother(smoother_config_, &spline_solver));
}
for (const auto &each_segments : route_segments) {
hdmap::Path hdmap_path;
hdmap::PncMap::CreatePathFromLaneSegments(each_segments, &hdmap_path);
if (FLAGS_enable_smooth_reference_line) {
ReferenceLine reference_line;
if (FLAGS_enable_spiral_reference_line) {
if (!spiral_smoother.Smooth(ReferenceLine(hdmap_path),
&reference_line)) {
AERROR << "Failed to smooth reference_line with spiral smoother";
}
} else {
if (!smoother.Smooth(ReferenceLine(hdmap_path), &reference_line)) {
AERROR << "Failed to smooth reference line";
continue;
}
if (!smoother->Smooth(ReferenceLine(hdmap_path), &reference_line)) {
AERROR << "Failed to smooth reference line";
continue;
}
reference_lines->emplace_back(std::move(reference_line));
segments->emplace_back(each_segments);
......
......@@ -35,20 +35,11 @@
namespace apollo {
namespace planning {
void QpSplineReferenceLineSmoother::Init(const std::string& config_file,
Spline2dSolver* const spline_solver) {
if (!common::util::GetProtoFromFile(config_file, &smoother_config_)) {
AERROR << "failed to load config file " << config_file;
return;
}
spline_solver_ = spline_solver;
}
void QpSplineReferenceLineSmoother::Init(
QpSplineReferenceLineSmoother::QpSplineReferenceLineSmoother(
const QpSplineReferenceLineSmootherConfig& config,
Spline2dSolver* const spline_solver) {
smoother_config_ = config;
spline_solver_ = spline_solver;
Spline2dSolver* const spline_solver)
: smoother_config_(config), spline_solver_(spline_solver) {
CHECK_NOTNULL(spline_solver);
}
void QpSplineReferenceLineSmoother::Clear() {
......
......@@ -36,17 +36,13 @@
namespace apollo {
namespace planning {
class QpSplineReferenceLineSmoother : ReferenceLineSmoother {
class QpSplineReferenceLineSmoother : public ReferenceLineSmoother {
public:
QpSplineReferenceLineSmoother() = default;
QpSplineReferenceLineSmoother(
const QpSplineReferenceLineSmootherConfig& config,
Spline2dSolver* const spline_solver);
virtual ~QpSplineReferenceLineSmoother() = default;
void Init(const std::string& config_file,
Spline2dSolver* const spline_solver) override;
void Init(const QpSplineReferenceLineSmootherConfig& refline_smooth_config,
Spline2dSolver* const spline_solver) override;
bool Smooth(const ReferenceLine& raw_reference_line,
ReferenceLine* const smoothed_reference_line) override;
......
......@@ -42,10 +42,6 @@ class QpSplineReferenceLineSmootherTest : public ::testing::Test {
AERROR << "failed to find lane " << lane_id << " from map " << map_file;
return;
}
QpSplineReferenceLineSmootherConfig config;
smoother_.Init(config,
&spline_solver_); // use the default value in config.
std::vector<ReferencePoint> ref_points;
const auto& points = lane_info_ptr->points();
const auto& headings = lane_info_ptr->headings();
......@@ -64,10 +60,11 @@ class QpSplineReferenceLineSmootherTest : public ::testing::Test {
"modules/planning/testdata/garage_map/base_map.txt";
hdmap::HDMap hdmap_;
QpSplineReferenceLineSmoother smoother_;
common::math::Vec2d vehicle_position_;
std::vector<double> t_knots_;
Spline2dSolver spline_solver_{t_knots_, 5};
QpSplineReferenceLineSmootherConfig config_;
QpSplineReferenceLineSmoother smoother_{config_, &spline_solver_};
std::unique_ptr<ReferenceLine> reference_line_;
hdmap::LaneInfoConstPtr lane_info_ptr = nullptr;
};
......
......@@ -145,12 +145,14 @@ bool ReferenceLineProvider::CreateReferenceLineFromRouting() {
}
}
QpSplineReferenceLineSmoother smoother;
smoother.Init(smoother_config_, spline_solver_.get());
SpiralReferenceLineSmoother spiral_smoother;
double max_spiral_smoother_dev = 0.1;
spiral_smoother.Init(max_spiral_smoother_dev);
std::unique_ptr<ReferenceLineSmoother> smoother;
if (FLAGS_enable_spiral_reference_line) {
double max_deviation = 0.1;
smoother.reset(new SpiralReferenceLineSmoother(max_deviation));
} else {
smoother.reset(new QpSplineReferenceLineSmoother(smoother_config_,
spline_solver_.get()));
}
std::vector<ReferenceLine> reference_lines;
std::vector<hdmap::RouteSegments> segments;
......@@ -160,15 +162,9 @@ bool ReferenceLineProvider::CreateReferenceLineFromRouting() {
if (FLAGS_enable_smooth_reference_line) {
ReferenceLine raw_reference_line(hdmap_path);
ReferenceLine reference_line;
if (FLAGS_enable_spiral_reference_line) {
if (!spiral_smoother.Smooth(raw_reference_line, &reference_line)) {
AERROR << "Failed to smooth reference_line with spiral smoother";
}
} else {
if (!smoother.Smooth(raw_reference_line, &reference_line)) {
AERROR << "Failed to smooth reference line";
continue;
}
if (!smoother->Smooth(raw_reference_line, &reference_line)) {
AERROR << "Failed to smooth reference line";
continue;
}
bool is_valid_reference_line = true;
......
......@@ -21,16 +21,7 @@
#ifndef MODULES_PLANNING_REFERENCE_LINE_REFERENCE_LINE_SMOOTHER_H_
#define MODULES_PLANNING_REFERENCE_LINE_REFERENCE_LINE_SMOOTHER_H_
#include <memory>
#include <string>
#include <vector>
#include "modules/planning/proto/planning.pb.h"
#include "modules/planning/proto/qp_spline_reference_line_smoother_config.pb.h"
#include "modules/planning/math/smoothing_spline/spline_2d_solver.h"
#include "modules/planning/reference_line/reference_line.h"
#include "modules/planning/reference_line/reference_point.h"
namespace apollo {
namespace planning {
......@@ -38,12 +29,9 @@ namespace planning {
class ReferenceLineSmoother {
public:
ReferenceLineSmoother() = default;
virtual ~ReferenceLineSmoother() = default;
virtual void Init(const std::string&, Spline2dSolver* const) {}
virtual void Init(const QpSplineReferenceLineSmootherConfig&,
Spline2dSolver* const) {}
virtual void Init(const double max_point_deviation_distance) {}
virtual bool Smooth(const ReferenceLine&, ReferenceLine* const) = 0;
};
......
......@@ -34,6 +34,12 @@
namespace apollo {
namespace planning {
SpiralReferenceLineSmoother::SpiralReferenceLineSmoother(
const double max_point_deviation)
: max_point_deviation_(max_point_deviation) {
CHECK(max_point_deviation >= 0.0);
}
bool SpiralReferenceLineSmoother::Smooth(
const ReferenceLine& raw_reference_line,
ReferenceLine* const smoothed_reference_line) {
......@@ -171,11 +177,6 @@ bool SpiralReferenceLineSmoother::Smooth(
status == Ipopt::Solved_To_Acceptable_Level;
}
void SpiralReferenceLineSmoother::Init(const double max_point_deviation) {
CHECK(max_point_deviation >= 0.0);
max_point_deviation_ = max_point_deviation;
}
std::vector<common::PathPoint> SpiralReferenceLineSmoother::to_path_points(
const double start_x, const double start_y, const double start_s,
const double theta0, const double kappa0, const double dkappa0,
......
......@@ -34,13 +34,12 @@
namespace apollo {
namespace planning {
class SpiralReferenceLineSmoother : ReferenceLineSmoother {
class SpiralReferenceLineSmoother : public ReferenceLineSmoother {
public:
SpiralReferenceLineSmoother() = default;
explicit SpiralReferenceLineSmoother(
const double max_point_deviation_distance);
virtual ~SpiralReferenceLineSmoother() = default;
void Init(const double max_point_deviation_distance) override;
bool Smooth(const ReferenceLine& raw_reference_line,
ReferenceLine* const smoothed_reference_line) override;
......
......@@ -72,7 +72,6 @@ class StBoundaryMapperTest : public ::testing::Test {
const std::string map_file =
"modules/planning/testdata/garage_map/base_map.txt";
hdmap::HDMap hdmap_;
QpSplineReferenceLineSmoother smoother_;
common::math::Vec2d vehicle_position_;
std::unique_ptr<ReferenceLine> reference_line_;
hdmap::LaneInfoConstPtr lane_info_ptr = nullptr;
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册