apollo自动驾驶进阶学习之：common:discretized_path.cc
发布时间
阅读量：
阅读量
文章目录
-
-
- discretized_path.h源文件
-
1.1代码实现细节
-
1.2相关知识模块
-
一、在C++中添加const用于...
-
- 函数返回值指定
-
- 参数传递控制
-
-
二、关于protected类型的成员函数设计
-
2、DiscretizedPath类定义
-
3、单元测试
-
Apollo代码实战及讲解(1)：planning/common/discretized
1、discretized_path.h头文件
1.1代码
namespace apollo {
namespace planning {
class DiscretizedPath : public std::vector<common::PathPoint> {
//继承自std::vector<common::PathPoint>
public:
DiscretizedPath() = default;
//默认构造函数
explicit DiscretizedPath(std::vector<common::PathPoint> path_points);
double Length() const;
//const作用：不允许改变成员变量
common::PathPoint Evaluate(const double path_s) const;
//const作用：同上
common::PathPoint EvaluateReverse(const double path_s) const;
//const作用：同上
protected:
//protected成员函数知识点详见1.2讲解
std::vector<common::PathPoint>::const_iterator QueryLowerBound(
const double path_s) const;
std::vector<common::PathPoint>::const_iterator QueryUpperBound(
const double path_s) const;
};
} // namespace planning
} // namespace apollo1.2知识点
一、在C++函数之后附加const修饰符以指定返回值为常量
建议对非静态成员函数后需添加const。
该方法中的$this指针被隐含地指定为常量引用。
（1）具有加入[const]修饰词的成员函数可被具有或无[const]修饰的对象调用
（2）未加入[const]修饰的成员函数仅可被具有[non-const]对象调用
二、protected成员函数
1.类的实例化对象不能访问protected成员函数和成员；
类中的每个成员函数均允许对其protected及其所属类型的所有成员函数和private其所属类型的所有成员函数进行访问。
3.只有在派生类中才可以通过派生类对象访问基类的protected成员。
2、DiscretizedPath类定义
#include "modules/planning/common/path/discretized_path.h"
#include <algorithm>
#include "cyber/common/log.h"
#include "modules/common/math/linear_interpolation.h"
namespace apollo {
namespace planning {
using apollo::common::PathPoint;
DiscretizedPath::DiscretizedPath(std::vector<PathPoint> path_points)
: std::vector<PathPoint>(std::move(path_points)) {}
//求path_points总的S长度
double DiscretizedPath::Length() const {
if (empty()) {
return 0.0;
}
return back().s() - front().s();
}
//根据给定的path_s的大小，插值求取path_s所在的x,y，theta，kappa，dkappa，ddkappa等信息
PathPoint DiscretizedPath::Evaluate(const double path_s) const {
ACHECK(!empty());
auto it_lower = QueryLowerBound(path_s);
if (it_lower == begin()) {
return front();
}
if (it_lower == end()) {
return back();
}
//参考linear_interpolation.cc
return common::math::InterpolateUsingLinearApproximation(*(it_lower - 1),
*it_lower, path_s);
}
//求第一个不满足Lambda表达式中比较条件的PathPoint的位置
std::vector<PathPoint>::const_iterator DiscretizedPath::QueryLowerBound(
const double path_s) const {
//C++中的Lambda表达式
auto func = [](const PathPoint &tp, const double path_s) {
return tp.s() < path_s;
};
return std::lower_bound(begin(), end(), path_s, func);
}
//根据给定的path_s的大小，插值求取path_s所在的x,y，theta，kappa，dkappa，ddkappa等信息，和上面的类似
PathPoint DiscretizedPath::EvaluateReverse(const double path_s) const {
ACHECK(!empty());
auto it_upper = QueryUpperBound(path_s);
if (it_upper == begin()) {
return front();
}
if (it_upper == end()) {
return back();
}
return common::math::InterpolateUsingLinearApproximation(*(it_upper - 1),
*it_upper, path_s);
}
std::vector<PathPoint>::const_iterator DiscretizedPath::QueryUpperBound(
const double path_s) const {
auto func = [](const double path_s, const PathPoint &tp) {
return tp.s() < path_s;
};
return std::upper_bound(begin(), end(), path_s, func);
}
} // namespace planning
} // namespace apollo3、单元测试
#include "modules/planning/common/path/discretized_path.h"
#include "cyber/common/log.h"
#include "gtest/gtest.h"
#include "modules/common/util/point_factory.h"
namespace apollo {
namespace planning {
using apollo::common::PathPoint;
using apollo::common::util::PointFactory;
TEST(DiscretizedPathTest, basic_test) {
const double s1 = 0.0;
const double s2 = s1 + std::sqrt(1.0 + 1.0);
const double s3 = s2 + std::sqrt(1.0 + 1.0);
const double s4 = s3 + std::sqrt(1.0 + 1.0);
PathPoint p1 = PointFactory::ToPathPoint(0.0, 0.0, 0.0, s1);
PathPoint p2 = PointFactory::ToPathPoint(1.0, 1.0, 0.0, s2);
PathPoint p3 = PointFactory::ToPathPoint(2.0, 2.0, 0.0, s3);
PathPoint p4 = PointFactory::ToPathPoint(3.0, 3.0, 0.0, s4);
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);
//长度=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));
discretized_path.clear();
EXPECT_EQ(discretized_path.size(), 0);
}
//reverse_case和上面的类似
TEST(DiscretizedPathTest, reverse_case) {
const double s1 = 0.0;
const double s2 = s1 - std::sqrt(1.0 + 1.0);
const double s3 = s2 - std::sqrt(1.0 + 1.0);
const double s4 = s3 - std::sqrt(1.0 + 1.0);
PathPoint p1 = PointFactory::ToPathPoint(0.0, 0.0, 0.0, s1);
PathPoint p2 = PointFactory::ToPathPoint(1.0, 1.0, 0.0, s2);
PathPoint p3 = PointFactory::ToPathPoint(2.0, 2.0, 0.0, s3);
PathPoint p4 = PointFactory::ToPathPoint(3.0, 3.0, 0.0, s4);
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.EvaluateReverse(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.EvaluateReverse(-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.EvaluateReverse(-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.EvaluateReverse(-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));
discretized_path.clear();
EXPECT_EQ(discretized_path.size(), 0);
}
} // namespace planning
} // namespace apollo全部评论 (0)
还没有任何评论哟~