1. 描述
主要想测试一下 frenet_frame_path，源文件在这里
modules/planning/common/path/frenet_frame_path.h
modules/planning/common/path/frenet_frame_path.cc
里面有些 msg 的定义是 proto 的，我就写成结构体了，方便测试，如下
2. 代码
其中
- GetNearestPoint() 是查找 frenet_frame_path 上距离 SLBoundary 最近的点
- EvaluateByS() 是通过查找 s，获取对应的 FrenetFramePoint
#include <iostream>
#include <vector>
#include <string>
#include <queue>
#include <memory>
#include <algorithm>
#include <cmath>
// #include "opencv2/highgui.hpp"
// #include "opencv2/opencv.hpp"
#include <gtest/gtest.h>
using namespace std;
namespace apollo {
namespace common {
struct FrenetFramePoint{
double s;
double l;
double dl;
double ddl;
};
struct SLBoundary {
double start_s;
double end_s;
double start_l;
double end_l;
};
}
}
namespace apollo {
namespace planning {
class FrenetFramePath : public std::vector<apollo::common::FrenetFramePoint> {
public:
FrenetFramePath() = default;
explicit FrenetFramePath(const std::vector<apollo::common::FrenetFramePoint> &points);
double Length() 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()]
*/
common::FrenetFramePoint GetNearestPoint(const apollo::common::SLBoundary &sl) const;
private:
static bool LowerBoundComparator(const apollo::common::FrenetFramePoint &p,
const double s) {
return p.s < s;
}
static bool UpperBoundComparator(const double s,
const apollo::common::FrenetFramePoint &p) {
return s < p.s;
}
};
} // namespace planning
} // namespace apollo
namespace apollo {
namespace common {
namespace math {
/**
* @brief Linear interpolation between two points of type T.
* @param x0 The coordinate of the first point.
* @param t0 The interpolation parameter of the first point.
* @param x1 The coordinate of the second point.
* @param t1 The interpolation parameter of the second point.
* @param t The interpolation parameter for interpolation.
* @param x The coordinate of the interpolated point.
* @return Interpolated point.
*/
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) {
cout << "input time difference is too small";
return x0;
}
const double r = (t - t0) / (t1 - t0);
const T x = x0 + r * (x1 - x0);
return x;
}
} // namespace math
} // namespace common
} // namespace apollo
namespace apollo {
namespace planning {
using apollo::common::FrenetFramePoint;
FrenetFramePath::FrenetFramePath(const std::vector<FrenetFramePoint>& points)
: std::vector<FrenetFramePoint>(points) {
}
double FrenetFramePath::Length() const {
if (empty()) {
return 0.0;
}
return back().s - front().s;
}
FrenetFramePoint FrenetFramePath::GetNearestPoint(const apollo::common::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) {
if (it->l >= sl.start_l && it->l <= sl.end_l) {
return *it;
} else if (it->l > sl.end_l) {
double diff = it->l - sl.end_l;
if (diff < min_dist) {
min_dist = diff;
min_it = it;
}
} 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(), 1);
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.s = s;
p.l = apollo::common::math::lerp(p0.l, s0, p1.l, s1, s);
p.dl = apollo::common::math::lerp(p0.dl, s0, p1.dl, s1, s);
p.ddl = apollo::common::math::lerp(p0.ddl, s0, p1.ddl, s1, s);
return p;
}
} // namespace planning
} // namespace apollo
// | 一一一一一一一一一一一一一一一一一一
// | | |
// | 一一一一一一一一一一一一一一一一一一
// |
// | × ×
// | × × ×
// |
//一一一|一一一一一一×一一一一一一一一一×一一一一
// | 1 2 3 4 5 6 7 8 9
// | × ×
// | ×
// |
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.s = s[i];
point.l = l[i];
point.dl = dl[i];
point.ddl = ddl[i];
}
path_.reset(new FrenetFramePath(sl_points));
}
void SetUp() {
InitFrenetFramePath(); }
std::unique_ptr<FrenetFramePath> path_;
};
TEST_F(FrenetFramePathTest, GetNearestPoint) {
cout << "path_->Length():" << path_->Length() << endl;
auto p = path_->EvaluateByS(2.5);
cout << p.s << "," << p.l << "," << p.dl << "," << p.ddl << endl;
apollo::common::SLBoundary sl_boundary;
{
// at the beginning
sl_boundary.start_s = -2;
sl_boundary.end_s = -1;
sl_boundary.start_l = 0;
sl_boundary.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);
cout << nearest.s << "," << nearest.l << "," << nearest.dl << "," << nearest.ddl << endl;
}
{
// at the end
sl_boundary.start_s = 11;
sl_boundary.end_s = 12;
sl_boundary.start_l = 0;
sl_boundary.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);
cout << nearest.s << "," << nearest.l << "," << nearest.dl << "," << nearest.ddl << endl;
}
{
// in the middle, left side
sl_boundary.start_s = 1;
sl_boundary.end_s = 9;
sl_boundary.start_l = 3;
sl_boundary.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);
cout << nearest.s << "," << nearest.l << "," << nearest.dl << "," << nearest.ddl << endl;
}
{
// in the middle, right side
sl_boundary.start_s = 1;
sl_boundary.end_s = 9;
sl_boundary.start_l = -4;
sl_boundary.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);
cout << nearest.s << "," << nearest.l << "," << nearest.dl << "," << nearest.ddl << endl;
}
{
// in the middle, cross
sl_boundary.start_s = 1;
sl_boundary.end_s = 9;
sl_boundary.start_l = -1;
sl_boundary.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);
cout << nearest.s << "," << nearest.l << "," << nearest.dl << "," << nearest.ddl << endl;
}
}
} // namespace planning
} // namespace apollo
结果
seivl@seivl-Default-string:~/me/代码/规划/bbox_test/build_$ ./bbox_test
Running main() from /home/seivl/gtest/googletest/googletest/src/gtest_main.cc
[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from FrenetFramePathTest
[ RUN ] FrenetFramePathTest.GetNearestPoint
path_->Length():9
2.5,1.5,-0.5,0
1,1,1,0
10,2,0,0
2,2,0,0
6,-2,0,0
4,0,0,0
[ OK ] FrenetFramePathTest.GetNearestPoint (0 ms)
[----------] 1 test from FrenetFramePathTest (0 ms total)
[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (0 ms total)
[ PASSED ] 1 test.