1. 描述
测试一下 discretized_path，源文件在这里
modules/planning/common/path/discretized_path.h
modules/planning/common/path/discretized_path.cc
里面 PathPoint 的定义是 proto 的，我就写成结构体了，方便测试
struct PathPoint {
// coordinates
double x;
double y;
double z;
// direction on the x-y plane
double theta;
// curvature on the x-y planning
double kappa;
// accumulated distance from beginning of the path
double s;
// derivative of kappa w.r.t s.
double dkappa;
// derivative of derivative of kappa w.r.t s.
double ddkappa;
// The lane ID where the path point is on
string lane_id;
// derivative of x and y w.r.t parametric parameter t in CosThetareferenceline
double x_derivative;
double y_derivative;
};
2. 代码
其中
- Evaluate() 是通过查找 s，获取对应的 PathPoint 点的信息
#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 PathPoint {
// coordinates
double x;
double y;
double z;
// direction on the x-y plane
double theta;
// curvature on the x-y planning
double kappa;
// accumulated distance from beginning of the path
double s;
// derivative of kappa w.r.t s.
double dkappa;
// derivative of derivative of kappa w.r.t s.
double ddkappa;
// The lane ID where the path point is on
string lane_id;
// derivative of x and y w.r.t parametric parameter t in CosThetareferenceline
double x_derivative;
double y_derivative;
};
}
}
namespace apollo {
namespace common {
namespace math {
constexpr double kMathEpsilon = 1e-10;
double NormalizeAngle(const double angle) {
double a = std::fmod(angle + M_PI, 2.0 * M_PI);
if (a < 0.0) {
a += (2.0 * M_PI);
}
return a - M_PI;
}
double slerp(const double a0, const double t0, const double a1, const double t1,
const double t) {
if (std::abs(t1 - t0) <= kMathEpsilon) {
// AERROR << "input time difference is too small";
return NormalizeAngle(a0);
}
const double a0_n = NormalizeAngle(a0);
const double a1_n = NormalizeAngle(a1);
double d = a1_n - a0_n;
if (d > M_PI) {
d = d - 2 * M_PI;
} else if (d < -M_PI) {
d = d + 2 * M_PI;
}
const double r = (t - t0) / (t1 - t0);
const double a = a0_n + d * r;
return NormalizeAngle(a);
}
PathPoint InterpolateUsingLinearApproximation(const PathPoint &p0,
const PathPoint &p1,
const double s) {
double s0 = p0.s;
double s1 = p1.s;
// CHECK_LE(s0, s1);
PathPoint path_point;
double weight = (s - s0) / (s1 - s0);
double x = (1 - weight) * p0.x + weight * p1.x;
double y = (1 - weight) * p0.y + weight * p1.y;
double theta = slerp(p0.theta, p0.s, p1.theta, p1.s, s);
double kappa = (1 - weight) * p0.kappa + weight * p1.kappa;
double dkappa = (1 - weight) * p0.dkappa + weight * p1.dkappa;
double ddkappa = (1 - weight) * p0.ddkappa + weight * p1.ddkappa;
path_point.x = x;
path_point.y = y;
path_point.theta = theta;
path_point.kappa = kappa;
path_point.dkappa = dkappa;
path_point.ddkappa = ddkappa;
path_point.s = s;
return path_point;
}
} // namespace math
} // namespace common
} // namespace apollo
namespace apollo {
namespace planning {
class DiscretizedPath : public std::vector<apollo::common::PathPoint> {
public:
DiscretizedPath() = default;
explicit DiscretizedPath(const std::vector<apollo::common::PathPoint>& path_points);
double Length() const;
apollo::common::PathPoint Evaluate(const double path_s) const;
protected:
std::vector<apollo::common::PathPoint>::const_iterator QueryLowerBound(
const double path_s) const;
};
} // namespace planning
} // namespace apollo
namespace apollo {
namespace planning {
using apollo::common::PathPoint;
DiscretizedPath::DiscretizedPath(const std::vector<PathPoint> &path_points)
: std::vector<PathPoint>(path_points) {
}
double DiscretizedPath::Length() const {
if (empty()) {
return 0.0;
}
return back().s - front().s;
}
PathPoint DiscretizedPath::Evaluate(const double path_s) const {
auto it_lower = QueryLowerBound(path_s);
if (it_lower == begin()) {
return front();
}
if (it_lower == end()) {
return back();
}
return common::math::InterpolateUsingLinearApproximation(*(it_lower - 1),
*it_lower, path_s);
}
std::vector<PathPoint>::const_iterator DiscretizedPath::QueryLowerBound(
const double path_s) const {
auto func = [](const PathPoint &tp, const double path_s) {
return tp.s < path_s;
};
return std::lower_bound(begin(), end(), path_s, func);
}
} // namespace planning
} // namespace apollo
namespace apollo {
namespace common {
namespace util {
PathPoint MakePathPoint(const double x, const double y, const double z,
const double theta, const double kappa,
const double dkappa, const double ddkappa) {
PathPoint path_point;
path_point.x = x;
path_point.y = y;
path_point.z = z;
path_point.theta = theta;
path_point.kappa = kappa;
path_point.dkappa = dkappa;
path_point.ddkappa = ddkappa;
return path_point;
}
} // namespace util
} // namespace common
} // namespace apollo
// |
// |
// | *
// | *
// | *
// | *
//一一一*一一一一一一一一一一一一
// |
// |
namespace apollo {
namespace planning {
using apollo::common::PathPoint;
using apollo::common::util::MakePathPoint;
TEST(DiscretizedPathTest, basic_test) {
PathPoint p1 = MakePathPoint(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
p1.s = 0.0;
PathPoint p2 = MakePathPoint(1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0);
p2.s = std::sqrt(1.0 + 1.0) + p1.s;
PathPoint p3 = MakePathPoint(2.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0);
p3.s = std::sqrt(1.0 + 1.0) + p2.s;
PathPoint p4 = MakePathPoint(3.0, 3.0, 0.0, 0.0, 0.0, 0.0, 0.0);
p4.s = std::sqrt(1.0 + 1.0) + p3.s;
std::vector<PathPoint> path_points{
p1, p2, p3, p4};
DiscretizedPath discretized_path(path_points);
EXPECT_EQ(discretized_path.size(), 4);
EXPECT_DOUBLE_EQ(discretized_path.Length(), std::sqrt(1.0 + 1.0) * 3.0);
auto eval_p1 = discretized_path.Evaluate(0.0);
// EXPECT_DOUBLE_EQ(eval_p1.s(), 0.0);
// EXPECT_DOUBLE_EQ(eval_p1.x(), 0.0);
// EXPECT_DOUBLE_EQ(eval_p1.y(), 0.0);
auto eval_p2 = discretized_path.Evaluate(0.3 * std::sqrt(2.0));
// EXPECT_DOUBLE_EQ(eval_p2.s(), 0.3 * std::sqrt(2.0));
// EXPECT_DOUBLE_EQ(eval_p2.x(), 0.3);
// EXPECT_DOUBLE_EQ(eval_p2.y(), 0.3);
auto eval_p3 = discretized_path.Evaluate(1.8);
// EXPECT_DOUBLE_EQ(eval_p3.s(), 1.8);
// EXPECT_DOUBLE_EQ(eval_p3.x(), (1.0 + 0.8) / std::sqrt(2));
// EXPECT_DOUBLE_EQ(eval_p3.y(), (1.0 + 0.8) / std::sqrt(2));
auto eval_p4 = discretized_path.Evaluate(2.5);
// EXPECT_DOUBLE_EQ(eval_p4.s(), 2.5);
// EXPECT_DOUBLE_EQ(eval_p4.x(), (2.0 + 0.5) / std::sqrt(2));
// EXPECT_DOUBLE_EQ(eval_p4.y(), (2.0 + 0.5) / std::sqrt(2));
auto eval_p5 = discretized_path.Evaluate(std::sqrt(1.0 + 1.0));
cout << eval_p5.x << endl;
discretized_path.clear();
// EXPECT_EQ(discretized_path.size(), 0);
}
} // 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 DiscretizedPathTest
[ RUN ] DiscretizedPathTest.basic_test
1
[ OK ] DiscretizedPathTest.basic_test (0 ms)
[----------] 1 test from DiscretizedPathTest (0 ms total)
[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (0 ms total)
[ PASSED ] 1 test.