apollo自动驾驶进阶学习之：common:frenet_frame_path.cc
发布时间
阅读量：
阅读量
文章目录
-
-
代码路径
-
1、discretized_path.h头文件
-
- 1.1代码
-
2、FrenetFramePath类定义
-
- FrenetFramePoint定义：
- SLBoundary定义：
-
3、单元测试
-
该HTML5多媒体组件用于嵌入B站视频播放器页面，并根据指定视频来源链接展示内容；其特点在于能够支持自动播放功能。
Apollo代码实战及讲解(2)：planning/common/frenet
代码路径
./apollo/modules/planning/common/path/frenet_frame_path.cc
1、discretized_path.h头文件
1.1代码
#pragma once
#include <utility>
#include <vector>
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/proto/sl_boundary.pb.h"
namespace apollo {
namespace planning {
//继承自std::vector<common::FrenetFramePoint>
class FrenetFramePath : public std::vector<common::FrenetFramePoint> {
public:
//默认构造函数
FrenetFramePath() = default;
explicit FrenetFramePath(std::vector<common::FrenetFramePoint> points);
//const作用：不允许本成员函数对成员变量进行更改
double Length() const;
//const作用：同上
common::FrenetFramePoint EvaluateByS(const double s) const;
/** * @brief Get the FrenetFramePoint that is within SLBoundary, or the one with
* smallest l() in SLBoundary's s range [start_s(), end_s()]
*/
//const作用：同上
common::FrenetFramePoint GetNearestPoint(const SLBoundary &sl) const;
private:
static bool LowerBoundComparator(const common::FrenetFramePoint &p,
const double s) {
return p.s() < s;
}
static bool UpperBoundComparator(const double s,
const common::FrenetFramePoint &p) {
return s < p.s();
}
};
} // namespace planning
} // namespace apollo2、FrenetFramePath类定义
#include "modules/planning/common/path/frenet_frame_path.h"
#include <algorithm>
#include <limits>
#include "cyber/common/log.h"
#include "modules/common/math/linear_interpolation.h"
namespace apollo {
namespace planning {
using apollo::common::FrenetFramePoint;
FrenetFramePath::FrenetFramePath(std::vector<FrenetFramePoint> points)
: std::vector<FrenetFramePoint>(std::move(points)) {}
//见下图1
double FrenetFramePath::Length() const {
if (empty()) {
return 0.0;
}
return back().s() - front().s();
}
//见下图2
FrenetFramePoint FrenetFramePath::GetNearestPoint(const SLBoundary& sl) const {
auto it_lower =
std::lower_bound(begin(), end(), sl.start_s(), LowerBoundComparator);
if (it_lower == end()) {
return back();
}
auto it_upper =
std::upper_bound(it_lower, end(), sl.end_s(), UpperBoundComparator);
double min_dist = std::numeric_limits<double>::max();
auto min_it = it_upper;
for (auto it = it_lower; it != it_upper; ++it) {
//图2-condition1
if (it->l() >= sl.start_l() && it->l() <= sl.end_l()) {
return *it;
//图2-condition2
} else if (it->l() > sl.end_l()) {
double diff = it->l() - sl.end_l();
if (diff < min_dist) {
min_dist = diff;
min_it = it;
}
//图2-condition3
} else {
double diff = sl.start_l() - it->l();
if (diff < min_dist) {
min_dist = diff;
min_it = it;
}
}
}
return *min_it;
}
//插值法求
FrenetFramePoint FrenetFramePath::EvaluateByS(const double s) const {
CHECK_GT(size(), 1U);
auto it_lower = std::lower_bound(begin(), end(), s, LowerBoundComparator);
if (it_lower == begin()) {
return front();
} else if (it_lower == end()) {
return back();
}
const auto& p0 = *(it_lower - 1);
const auto s0 = p0.s();
const auto& p1 = *it_lower;
const auto s1 = p1.s();
FrenetFramePoint p;
p.set_s(s);
p.set_l(common::math::lerp(p0.l(), s0, p1.l(), s1, s));
p.set_dl(common::math::lerp(p0.dl(), s0, p1.dl(), s1, s));
p.set_ddl(common::math::lerp(p0.ddl(), s0, p1.ddl(), s1, s));
return p;
}
} // namespace planning
} // namespace apollo
补充：线性插值代码
template <typename T>
T lerp(const T &x0, const double t0, const T &x1, const double t1,
const double t) {
if (std::abs(t1 - t0) <= 1.0e-6) {
AERROR << "input time difference is too small";
return x0;
}
const double r = (t - t0) / (t1 - t0);
const T x = x0 + r * (x1 - x0);
return x;
}FrenetFramePoint定义：
message FrenetFramePoint {
optional double s = 1;
optional double l = 2;
optional double dl = 3;
optional double ddl = 4;
}SLBoundary定义：
message SLBoundary {
optional double start_s = 1;
optional double end_s = 2;
optional double start_l = 3;
optional double end_l = 4;
repeated apollo.common.SLPoint boundary_point = 5;
}3、单元测试
本图列出test case中的几个案例，可以更清晰的了解本类的作用
#include "modules/planning/common/path/frenet_frame_path.h"
#include "gtest/gtest.h"
namespace apollo {
namespace planning {
using apollo::common::FrenetFramePoint;
class FrenetFramePathTest : public ::testing::Test {
public:
void InitFrenetFramePath() {
std::vector<double> s{1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
std::vector<double> l{1, 2, 1, 0, -1, -2, -1, 0, 1, 2};
std::vector<double> dl{1, 0, -1, 0, -1, 0, 1, 1, 1, 0};
std::vector<double> ddl{0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
std::vector<FrenetFramePoint> sl_points;
for (size_t i = 0; i < s.size(); ++i) {
sl_points.emplace_back();
FrenetFramePoint& point = sl_points.back();
point.set_s(s[i]);
point.set_l(l[i]);
point.set_dl(dl[i]);
point.set_ddl(ddl[i]);
}
path_.reset(new FrenetFramePath(std::move(sl_points)));
}
void SetUp() { InitFrenetFramePath(); }
std::unique_ptr<FrenetFramePath> path_;
};
TEST_F(FrenetFramePathTest, GetNearestPoint) {
SLBoundary sl_boundary;
{
// at the beginning
sl_boundary.set_start_s(-2);
sl_boundary.set_end_s(-1);
sl_boundary.set_start_l(0);
sl_boundary.set_end_l(1);
auto nearest = path_->GetNearestPoint(sl_boundary);
EXPECT_DOUBLE_EQ(nearest.s(), 1.0);
EXPECT_DOUBLE_EQ(nearest.l(), 1.0);
EXPECT_DOUBLE_EQ(nearest.dl(), 1.0);
EXPECT_DOUBLE_EQ(nearest.ddl(), 0.0);
}
{
// at the end
sl_boundary.set_start_s(11);
sl_boundary.set_end_s(12);
sl_boundary.set_start_l(0);
sl_boundary.set_end_l(1);
auto nearest = path_->GetNearestPoint(sl_boundary);
EXPECT_DOUBLE_EQ(nearest.s(), 10.0);
EXPECT_DOUBLE_EQ(nearest.l(), 2.0);
EXPECT_DOUBLE_EQ(nearest.dl(), 0.0);
EXPECT_DOUBLE_EQ(nearest.ddl(), 0.0);
}
{
// in the middle, left side
sl_boundary.set_start_s(1);
sl_boundary.set_end_s(9);
sl_boundary.set_start_l(3);
sl_boundary.set_end_l(4);
auto nearest = path_->GetNearestPoint(sl_boundary);
EXPECT_DOUBLE_EQ(nearest.s(), 2.0);
EXPECT_DOUBLE_EQ(nearest.l(), 2.0);
EXPECT_DOUBLE_EQ(nearest.dl(), 0.0);
EXPECT_DOUBLE_EQ(nearest.ddl(), 0.0);
}
{
// in the middle, right side
sl_boundary.set_start_s(1);
sl_boundary.set_end_s(9);
sl_boundary.set_start_l(-4);
sl_boundary.set_end_l(-3);
auto nearest = path_->GetNearestPoint(sl_boundary);
EXPECT_DOUBLE_EQ(nearest.s(), 6.0);
EXPECT_DOUBLE_EQ(nearest.l(), -2.0);
EXPECT_DOUBLE_EQ(nearest.dl(), 0.0);
EXPECT_DOUBLE_EQ(nearest.ddl(), 0.0);
}
{
// in the middle, cross
sl_boundary.set_start_s(1);
sl_boundary.set_end_s(9);
sl_boundary.set_start_l(-1);
sl_boundary.set_end_l(0);
auto nearest = path_->GetNearestPoint(sl_boundary);
EXPECT_DOUBLE_EQ(nearest.s(), 4.0);
EXPECT_DOUBLE_EQ(nearest.l(), 0.0);
EXPECT_DOUBLE_EQ(nearest.dl(), 0.0);
EXPECT_DOUBLE_EQ(nearest.ddl(), 0.0);
}
}全部评论 (0)
还没有任何评论哟~