Apollo 10.0
自动驾驶开放平台
Public 成员函数 | 所有成员列表
apollo::planning::SmootherUtil类 参考
apollo::planning::SmootherUtil 的协作图:

Public 成员函数

 SmootherUtil (const std::string &filename)
 
bool Smooth ()
 
void Export (const std::string &filename)
 

详细描述

在文件 smoother_util.cc 第 46 行定义.

构造及析构函数说明

◆ SmootherUtil()

apollo::planning::SmootherUtil::SmootherUtil ( const std::string &  filename)
inlineexplicit

在文件 smoother_util.cc 第 48 行定义.

48 : filename_(filename) {
49 std::ifstream ifs(filename.c_str(), std::ifstream::in);
50 std::string point_str;
51 while (std::getline(ifs, point_str)) {
52 size_t idx = point_str.find(',');
53 if (idx == std::string::npos) {
54 continue;
55 }
56 auto x_str = point_str.substr(0, idx);
57 auto y_str = point_str.substr(idx + 1);
58 raw_points_.emplace_back(std::stod(x_str), std::stod(y_str));
59 }
60 ACHECK(cyber::common::GetProtoFromFile(FLAGS_smoother_config_filename,
61 &config_))
62 << "Failed to read smoother config file: "
63 << FLAGS_smoother_config_filename;
64 }
ACHECK
#define ACHECK(cond)
Definition log.h:80
apollo::cyber::common::GetProtoFromFile
bool GetProtoFromFile(const std::string &file_name, google::protobuf::Message *message)
Parses the content of the file specified by the file_name as a representation of protobufs,...
Definition file.cc:132

成员函数说明

◆ Export()

void apollo::planning::SmootherUtil::Export ( const std::string &  filename)
inline

在文件 smoother_util.cc 第 141 行定义.

141 {
142 std::ofstream ofs(filename.c_str());
143 if (ofs.fail()) {
144 AERROR << "Fail to open file " << filename;
145 return;
146 }
147 ofs.precision(6);
148 double s = 0.0;
149 // skip the first point and the last point
150 for (size_t i = 1; i + 1 < ref_points_.size(); ++i) {
151 const auto& point = ref_points_[i];
152 ofs << std::fixed << "{\"kappa\": " << point.kappa() << ", \"s\": " << s
153 << ", \"theta\": " << point.heading() << ", \"x\":" << point.x()
154 << ", \"y\":" << point.y() << ", \"dkappa\":" << point.dkappa()
155 << "}";
156 s += DistanceXY(point, ref_points_[i + 1]);
157 }
158 ofs.close();
159 AINFO << "Smoothed result saved to " << filename;
160 }
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42
apollo::common::util::DistanceXY
double DistanceXY(const U &u, const V &v)
calculate the distance beteween Point u and Point v, which are all have member function x() and y() i...
Definition util.h:97

◆ Smooth()

bool apollo::planning::SmootherUtil::Smooth ( )
inline

在文件 smoother_util.cc 第 66 行定义.

66 {
67 if (raw_points_.size() <= 2) {
68 AERROR << "the original point size is " << raw_points_.size();
69 return false;
70 }
71 size_t i = 1;
72 {
73 std::vector<ReferencePoint> ref_points;
74 double s = 0.0;
75 for (; s < FLAGS_smooth_length && i < raw_points_.size(); ++i) {
76 LineSegment2d segment(raw_points_[i - 1], raw_points_[i]);
77 ref_points.emplace_back(MapPathPoint(raw_points_[i], segment.heading()),
78 0.0, 0.0);
79 s += segment.length();
80 }
81 ReferenceLine init_ref(ref_points);
82 // Prefer "std::make_unique" to direct use of "new".
83 // Reference "https://herbsutter.com/gotw/_102/" for details.
84 auto smoother_ptr =
85 std::make_unique<QpSplineReferenceLineSmoother>(config_);
86 auto anchors =
87 CreateAnchorPoints(init_ref.reference_points().front(), init_ref);
88 smoother_ptr->SetAnchorPoints(anchors);
89 ReferenceLine smoothed_init_ref;
90 if (!smoother_ptr->Smooth(init_ref, &smoothed_init_ref)) {
91 AERROR << "smooth initial reference line failed";
92 return false;
93 }
94 ref_points_ = smoothed_init_ref.reference_points();
95 }
96 for (; i < raw_points_.size(); ++i) {
97 double s = 0.0;
98 size_t j = ref_points_.size() - 1;
99 while (j > 0 && s < FLAGS_smooth_length / 2.0) {
100 s += DistanceXY(ref_points_[j - 1], ref_points_[j]);
101 --j;
102 }
103 ReferenceLine prev_half_ref(ref_points_.begin() + j, ref_points_.end());
104 ref_points_.erase(ref_points_.begin() + j, ref_points_.end());
105 common::SLPoint sl;
106 prev_half_ref.XYToSL(raw_points_[i], &sl);
107 while (sl.s() <= prev_half_ref.Length() && i + 1 < raw_points_.size()) {
108 prev_half_ref.XYToSL(raw_points_[i + 1], &sl);
109 ++i;
110 }
111 s = 0.0;
112 j = i;
113 auto ref_points = prev_half_ref.reference_points();
114 while (j + 1 < raw_points_.size() && s < FLAGS_smooth_length / 2.0) {
115 Vec2d vec = raw_points_[j + 1] - raw_points_[j];
116 s += vec.Length();
117 ref_points.emplace_back(MapPathPoint(raw_points_[j], vec.Angle()), 0.0,
118 0.0);
119 ++j;
120 }
121 i = j;
122 ReferenceLine local_ref(ref_points);
123 auto anchors = CreateAnchorPoints(ref_points.front(), local_ref);
124 // Prefer "std::make_unique" to direct use of "new".
125 // Reference "https://herbsutter.com/gotw/_102/" for details.
126 auto smoother_ptr =
127 std::make_unique<QpSplineReferenceLineSmoother>(config_);
128 smoother_ptr->SetAnchorPoints(anchors);
129 ReferenceLine smoothed_local_ref;
130 if (!smoother_ptr->Smooth(local_ref, &smoothed_local_ref)) {
131 AERROR << "Failed to smooth reference line";
132 return false;
133 }
134 ref_points_.insert(ref_points_.end(),
135 smoothed_local_ref.reference_points().begin(),
136 smoothed_local_ref.reference_points().end());
137 }
138 return true;
139 }
该类的文档由以下文件生成: