Autoware源码分析——astar_search
1. 概要
本文是对autoware中core_planning文件夹中astar_search功能包的相关文件组成和具体源码的分析，承接上一篇关于astar_avoid的分析。
如果有理解有误的地方，望指正!
2. 文件结构
2.1 astar_avoid中与之有关的函数
astar_avoid.cpp中的planAvoidWaypoints调用了astar_search中的astar_.makePlan()等函数，目的就是执行hybrid A* 算法。
planAvoidWaypoints
该函数使用了astar_search中的相关函数：makePlan()、initialize()、reset()和getPath()。
该函数的大致流程就是：
- 定义一些用于判断条件的变量。
- 调用astar_search的这些函数以实现hybrid A* 算法。具体来说，先逐步更新终点位姿和索引信息（“索引”这个变量非常重要，还没有太明白其本质意思。），再初始化地图（也就是costmap信息）并执行
makePlan函数，如果找到路径就发布路径点信息，如果找不到就reset程序。
2.2 头文件
astar_search使用了两个头文件，一个是astar_util.h, 作用是定义程序中所需要的一些“数据结构”（struct类）和一些用于计算的函数；另一个是astar_search.h, 用于定义AstarSearch这个类中的信息，比如构造函数、析构函数、相关公有或私有变量或方法。以下对这两个文件进行分析。
astar_util.h
- 算法中需要的数据结构定义
enum class STATUS : uint8_t
struct AstarNode...
struct WaveFrontNode...
struct NodeUpdate...
struct SimpleNode...
- 算法中需要用到的函数（inline）
inline double calcDistance(double x1, double y1, double x2, double y2)
inline double modifyTheta(double theta)
inline geometry_msgs::Pose transformPose(const geometry_msgs::Pose& pose, const tf::Transform& tf)
inline WaveFrontNode getWaveFrontNode(int x, int y, double cost)
inline geometry_msgs::Point calcRelativeCoordinate(geometry_msgs::Pose pose, tf::Point point)
inline double calcDiffOfRadian(double a, double b)
inline geometry_msgs::Pose xytToPoseMsg(double x, double y, double theta)
astar_search.h
- public方法
...
class AstarSearch
{
friend class TestClass;
public:
AstarSearch();
~AstarSearch();
void initialize(const nav_msgs::OccupancyGrid& costmap);
bool makePlan(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& goal_pose);
void reset();
const nav_msgs::Path& getPath() const
{
return path_;
}
- private变量或方法
private:
//以下为private methods:
void createStateUpdateTable();
bool search();
void poseToIndex(const geometry_msgs::Pose& pose, int* index_x, int* index_y, int* index_theta);
void pointToIndex(const geometry_msgs::Point& point, int* index_x, int* index_y);
bool isOutOfRange(int index_x, int index_y);
void setPath(const SimpleNode& goal);
bool setStartNode(const geometry_msgs::Pose& start_pose);
bool setGoalNode(const geometry_msgs::Pose& goal_pose);
bool isGoal(double x, double y, double theta);
bool isObs(int index_x, int index_y);
bool detectCollision(const SimpleNode& sn);
bool calcWaveFrontHeuristic(const SimpleNode& sn);
bool detectCollisionWaveFront(const WaveFrontNode& sn);
私有变量可以分为如下模块：
// 1. ros param
// 2. base configs
// 3. robot configs (TODO: obtain from vehicle_info)
// 4. search configs
// 5. costmap configs
// 6. hybrid astar variables
// 7. costmap as occupancy grid
// 8. pose in costmap frame
// 9. result path
用到的消息类型和其对应的内容有如下:
nav_msgs::Path:
#An array of poses that represents a Path for a robot to follow
Header header
geometry_msgs/PoseStamped[] poses
geometry_msgs/PoseStamped:
# A Pose with reference coordinate frame and timestamp
std_msgs/Header header
geometry_msgs/Pose pose
可以看出，路径信息是这些位姿点构成的数组。
autoware_msgs::Lane：
见上一篇关于astar_avoid的源码分析
2.3 cpp文件
astar_util.cpp
//license 信息
#include "astar_search/astar_util.h"
WaveFrontNode::WaveFrontNode() : index_x(0), index_y(0), hc(0)
{
}
WaveFrontNode::WaveFrontNode(int x, int y, double cost) : index_x(x), index_y(y), hc(cost)
{
}
SimpleNode::SimpleNode()
{
}
SimpleNode::SimpleNode(int x, int y, int theta, double gc, double hc)
: index_x(x), index_y(y), index_theta(theta), cost(gc + hc)
{
}
astar_search.cpp
该cpp文件就是对上述同名.h文件的扩展。其模块之间的结构都是一样的：构造函数，析构函数，方法函数。
3. 源码分析
3.1 AstarSearch::createStateUpdateTable()
所用到的数据结构（定义来自astar_util.h）
struct NodeUpdate
{
double shift_x;
double shift_y;
double rotation;
double step;
int index_theta;
bool curve;
bool back;
};
该函数做了以下几件事：
定义每次状态更新的“精度”
首先，车辆的状态空间是 ( X , Y , Θ ) (X,Y,\Theta) (X,Y,Θ)，程序中用到的角度步长为dtheta，根据角度步长定义每一步的最小弧长（Minimum moving distance with one state update）；根据定义的旋转半径更新圆弧轨迹的圆心位置:
double right_circle_center_x = minimum_turning_radius_ * std::sin(theta);
double right_circle_center_y = minimum_turning_radius_ * -std::cos(theta);
double left_circle_center_x = -right_circle_center_x;
double left_circle_center_y = -right_circle_center_y;
根据不同模式更新对应的坐标
前向：包含三段，forward, forward right, forward left。
后向：也包含三段，backward, backward right, backward left。
3.2 AstarSearch::makePlan()
输入输出：该函数接收到一些消息类型为geometry_msgs::Pose的参数之后返回True或False。
该函数的源代码如下：
bool AstarSearch::makePlan(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& goal_pose)
{
if (!setStartNode(start_pose))
{
// ROS_WARN_STREAM("Invalid start pose");
return false;
}
if (!setGoalNode(goal_pose))
{
// ROS_WARN_STREAM("Invalid goal pose");
return false;
}
return search();
}
由此可见，该函数做的事情就是根据判断条件的满足与否来执行search()这个核心函数。
3.3 AstarSearch::search()
I. 函数的大致步骤
- 定义与系统时间相关的变量
- 判断算法使用的openlist是否为empty，如果是empty, 搜索失败；如果不是empty，就进行循环。
- 搜索过程开始：从openlist中取出最小损失的节点，然后从这个损失最小的节点开始扩张，并更新相关的列表信息。
II. 详细的搜索过程
a. 取出最小损失的节点（Pop minimum cost node from openlist）
SimpleNode的定义
struct SimpleNode
{
int index_x;
int index_y;
int index_theta;
double cost;
bool operator>(const SimpleNode& right) const
{
return cost > right.cost;
}
SimpleNode();
SimpleNode(int x, int y, int theta, double gc, double hc);
};
定义的数据结构就是优先级队列，再通过优先级队列中的弹出方法将损失最小的点取出。
b. 从这个损失最小的节点开始扩张
该步骤用到的数据结构：
struct AstarNode
{
double x, y, theta; // Coordinate of each node
STATUS status = STATUS::NONE; // NONE, OPEN, CLOSED or OBS
double gc = 0; // Actual cost
double hc = 0; // heuristic cost
double move_distance = 0; // actual move distance
bool back; // true if the current direction of the vehicle is back
AstarNode* parent = NULL; // parent node
};
接下来的步骤就是：
a. 将该节点添加到CLOSED列表中，之后判断该点是否为终点，如果是重点就调用setPath()设置这条路径并返回true。如果不是终点，继续进行算法。
b. 节点扩张的过程
- 更新下一个状态的信息-next state
- 增加反向损失（Increase reverse cost）
- 计算下一个状态的索引index
- 检查该状态的index是否有效：用函数
isOutOfRange(next_sn.index_x, next_sn.index_y) || detectCollision(next_sn)进行检查。如果为条件为true, 重新开始下一次循环。否则继续执行以下语句。 - 检查使用的损失函数的类型并相应地基于欧几里得距离更新损失值：
use_potential_heuristic_或者是
!use_wavefront_heuristic_ && !use_potential_heuristic_ - 判断节点AstarNode的状态：如果为
NONE，则将其状态改为OPEN，状态改变之后，更新该节点的信息；如果是OPEN or CLOSED，则检查条件next_gc < next_an->gc
是否满足，满足该条件之后，将节点状态设置为OPEN，之后更新该节点的信息并push到openlist中。
4. 关于ROS或C++语法的要点
C++的一些要点:
（已更新，大部分来自《C++ primer》）
友元（friend）
A class can allow another class or function to access its nonpublic members by making that class or function a friend.
类可以允许其他类或函数访问它的非公有成员，方法是令其他类或函数成为它的友元。
class AAA
{
friend class BBB; //BBB可以访问AAA类中的私有部分
...
}
The member functions of a friend class can access all the members, including the
nonpublic members, of the class granting friendship.(友元中的成员函数能够访问该类中非公有成员在内的所有成员)
Autoware中的代码中用到友元的部分：
class AstarSearch
{
friend class TestClass;
...
常量成员函数-const memeber function（void function(arg) const {} ）
其目的是修改this，让其指向const指针。（注意，this默认情况下是指向非const类类型，class type, 的const指针）。在类成员函数的声明和定义中，const的函数不能对其数据成员进行修改操作（只可读取，不能写入）。
总的来说，常量成员函数的作用就是const的对象、指向const对象的指针或引用不能引用非const的成员函数。
参考链接：https://blog.csdn.net/u013270326/article/details/78388305
inline函数——内联函数
A function specified as inline (usually) is expanded “in line” at each call.
内联函数的目的是减少函数的运行时(或者说函数调用时的)开销（runtime overhead），其原因在于函数调用的时候速度相对较慢（在函数调用之前，寄存器中的信息要被存储并在调用之后恢复；拷贝实参；程序跳到新的执行位置）
Registers are saved before the call and restored after the return; arguments may be copied; and the program branches to a new location.
内联机制用于优化频繁调用的规模较小、流程直接的函数
std::vector——C++中的容器container
多个vector的组合，<>括号内为自定义类型
A vector is a collection of objects, all of which have the same type. Every object in the collection has an associated index, which gives access to that object. （often referred to as a container）
#includeClass Template can be thought of as instructions to the compiler for generating classes or functions.
vector是一种类模板class template，它可以被看成是在生成类或函数的过程中发给编译器的“指令”。
实例化instantiation：编译器用模板生成类或函数的过程。
Instantiation: The process that the compiler uses to create classes or functions from templates.
例如：
vector<int> ivec;
vector<vector<string>> file; // vector whose elements are vectors
I. 定义和初始化vector对象方法
| 指令 | 解释 |
|---|---|
| vector |
vector that holds objects of type T. Default initialization; v1 is empty. |
| vector |
v2 has a copy of each element in v1. |
| vector |
Equivalent to v2(v1), v2 is a copy of the elements in v1. |
| vector |
v3 has n elements with value val. |
| vector |
v4 has n copies of a value-initialized object. |
| vector |
v5 has as many elements as there are initializers; elements are initialized by corresponding initializers. |
| vector |
Equivalent to v5{a,b,c . . . }. |
注意：值初始化value-initialization
值初始化中，只需要提供vector对象容纳的元素数量，可以略去初始值。C++的库会创建一个值初始化的元素初值，并将其赋给所有元素。初值由vector对象中存储的数据类型决定。例如：int类型初始值为0
II. 对vector对象进行操作——添加元素、其他操作等
对vector进行元素添加的一般方法是：先定义一个空的vector对象，再在运行时向其中添加具体值（push_back()函数）。
vector支持的其他操作：
| 操作 | 解释 |
|---|---|
| v.empty() | v不含任何元素，返回真；否则返回假 |
| v.size() | 返回v的元素个数 |
| v[n] | 返回v中第n个元素的引用 |
| v1=v2 | 将v2赋值给v1 |
| v1={a,b,c…} | - |
| v1==v2 | v1,v2相等的条件是：当且仅当它们的元素数量相同且对应位置的元素值都相同。 |
| v1 != v2 | - |
| <, <=, >, >= | - |
std::priority_queue<>和std::queue<>用法
两者均定义在queue文件中，#include 。
这属于C++中顺序容器适配器sequential container adaptors中的内容，其内容包括了：stack, queue, priority_queue。
例如：
The stack adaptor takes a sequential container (other than array or forward_list) and makes it operate as if it were a stack.
stack, queue默认是基于deque实现的，而priority_queue默认是基于vector实现的（也可以通过其他的容器来实现）
容器的操作：
I. emplace_back, emplace_front与push_back, push_front
相关链接：https://blog.csdn.net/p942005405/article/details/84764104
新标准C++11中引入emplace_front, emplace, emplace_back, 这些操作构造元素而不是拷贝元素。（分别对应push_front, insert和 push_back）, 其作用是将元素放在容器头部、指定位置之前、容器尾部。
可以说push_back()只接受一个元素，可以是值也可以是某个对象；而emplace_back()可以接收某个结构体中定义的多个元素。
push_back()会将拷贝的数据传给容器（先创建一个局部临时对象，再将其push到容器中），而emplace_back()直接在容器管理的内存空间中创建。
例子：
// uses the three-argument Sales_data constructor
c.emplace_back("978-0590353403", 25, 15.99);
// error: there is no version of push_back that takes three arguments
c.push_back("978-0590353403", 25, 15.99);
// ok: we create a temporary Sales_data object to pass to push_back
c.push_back(Sales_data("978-0590353403", 25, 15.99));
II. queue和priority_queue额外支持的操作
q.top()：（只适用于priority_queue）返回但不删除最高优先级元素。
q.pop(): 删除但不返回queue的首元素（front element）或者priority_queue的最高优先级（highest-priority）元素
q.front()：返回但不删除首元素或尾元素
q.back(): 只适用于queue
q.push(item)与q.emplace(args):在queue末尾或priority_queue恰当位置创建一个元素，其值为item或由args构造。
swap(a,b)和a.swap(b): 交换a和b的内容，a，b必须有相同的类型，包括底层容器类型也必须相同
内容补充：什么是priority_queue?
A priority_queue lets us establish a priority among the elements held in the queue, Newly added elements are placed ahead of all the elements with a lower priority.
(新加入的元素排在所有优先级比它低的已有元素之前)
hypot(a,b)——计算a,b和的均方根
The function returns what would be the square root of the sum of the squares of x and y (as per the Pythagorean theorem), but without incurring in undue overflow or underflow of intermediate values. Returns the hypotenuse of a right-angled triangle whose legs are x and y.
就是对两个数平方之和求根。
C++类中的多个构造函数（或构造函数的重载）
相关连接：https://blog.csdn.net/qq_28581077/article/details/81660383
C++的类的定义有两种方式：struct和class。
构造函数的重载允许我们用各种定义方法来初始化我们的对象。
例如：
Sales_data() = default;
Sales_data(const std::string &s): bookNo(s) { }
Sales_data(const std::string &s, unsigned n, double p): bookNo(s), units_sold(n), revenue(p*n) { }
Sales_data(std::istream &);
解释以上代码：
a. default
=default要求编译器生成默认的constructor
b.
Sales_data(const std::string &s): bookNo(s) { }
Sales_data(const std::string &s, unsigned n, double p): bookNo(s), units_sold(n), revenue(p*n) { }
冒号和花括号之间的代码，比如第一句中的bookNo(s)，这部分是构造函数初始值列表（constructor initializer list），作用是: specifies initial values for one or more data members of the object being created.（为一个或多个数据成员赋初始值）。每个名字后面紧跟其括号括起来（或者在构造函数内部，或花括号括起来）的成员初始值，比如bookNo(s)中s就是bookNo这个变量的初始值。之后用逗号分隔不同数据成员data members。
以上不完全的语句是：Sales_data(const std::string &s): bookNo(s) { }，该语句相当于Sales_data(const std::string &s): bookNo(s), units_sold(0), revenue(0){ }
M_PI是c++语言中标准库定义的宏。
resize()函数——用于改变容器container大小
首先，array不支持resize操作。
容器中的resize()
如果当前容器大小大于resize中的大小，容器后部的元素会被删除。如果容器大小大于resize中的大小，则将新元素添加到容器后部
list<int> ilist(10, 42); // ten ints: each has value 42
ilist.resize(15); // adds five elements of value 0 to the back of ilist
ilist.resize(25, -1); // adds ten elements of value -1 to the back of ilist
ilist.resize(5); // erases 20 elements from the back of ilist
相关操作的格式：
| 操作 | 解释 |
|---|---|
c.resize(n); |
Resize c so that it has n elements. If n < c.size(), the excess elements are discarded. If new elements must be added, they are value initialized |
c.resize(n, t); |
Resize c to have n elements. Any elements added have value t. |
std::reverse()
reverse(a.begin(),a.end())翻转字符串a
函数的重载overloaded function
Functions that have the same name but different parameter lists and that appear in the same scope are overloaded.
函数重载表示在同一个作用域内的几个函数名字相同但形参列表（形参类型或数量）不同的函数（如果形参列表相同，函数的返回类型不同，则不能称为重载。形参的名字并不影响形参列表的内容）
在该过程中，编译器会自己推断使用的是哪个函数（需要注意的是，main函数不能重载）
运算符的重载overloaded operator
1. 基本的定义方式：
Overloaded operators are functions with special names: the keyword operator followed by the symbol for the operator being defined.
关键字operator和其后的运算符号构成。定义方式例如：
operator+(...)
operator<(...)
重载运算符函数的形参数量与该运算符作用的operand数量一样。例如，在二元运算符的重载过程中，左侧operand是形参列表中的第一个，右operand是第二个参数。
只有已经存在的运算符，才能被重载。不能重载库中没有的运算符。运算符的优先级和结合律重载之后与原先的内置运算符保持一致。
调用时直接调用符号即可：
data1 + data2;
operator+(data1, data2);
某些运算符本身就定义了其operand的求值顺序。（如逻辑与，或和逗号运算符），这些运算符都不应该进行重载。
C++复合类型中的引用reference和指针pointer
例子:
// i is an int; p is a pointer to int; r is a reference to int
int i = 1024, *p = &i, &r = i;
a declaration is a base type （基本数据类型）followed by a list of declarators（声明符）.
a. 引用
引用为对象起了另一个名字（Alias）。（引用类型必须进行初始化）
A reference is not an object. Instead, a reference is just another name for an already existing object.
定义一个引用之后，对其进行的所有操作都是在与之绑定的对象上进行的。定义方式是在变量前加&
int ival = 1024;
int &refVal = ival; // refVal refers to (is another name for) ival
int &refVal2; // error: a reference must be initialized
定义引用的时候，并不是像一般赋值那样拷贝变量的值，而是将引用与被引绑定bind在一起，绑定之后不能重新绑定到另外一个对象。
给引用赋值也就是给他refers to的对象赋值。引用只能绑定在某个对象（例如由int类实例化的int变量）上。需要注意的是，不能定义引用的引用（a reference to a reference）。
int &refVal4 = 10; // error: initializer must be an object
double dval = 3.14;
int &refVal5 = dval; // error: initializer must be an int object
b. 指针pointer
指针也实现了对其他对象的简介引用。定义指针需要在变量前加* 。
pointer与reference的区别：
- 指针本身就是一个对象，可以指向多个不同的对象（引用只能绑定一个对象）。
- 指针无需在定义时赋初值。（引用类型需要赋初值）
指针本身是存放对象的地址（获取对象地址的方式是取址符&）。给指针赋值就是给指针变量本身一个新的值，即存放新的地址从而“指向”新的对象
int ival = 42;
int *p = &ival; // p holds the address of ival; p is a pointer to ival
第二句相当于把&ival传给p，再通过一次*解引用操作（ dereference operator * 用于访问对象的值，因为指针中存放的是对象的地址。）将其中的数取出。
(注意，指针的类型要与其所指对象严格匹配)。特殊的指针void*，用于存放任意对象的地址。好的编程习惯 是初始化所有指针。
指针的两种定义方式：
int *p1, *p2; // both p1 and p2 are pointers to int
int* p1, p2; // p1 is a pointer to int; p2 is an int
指向指针的指针（Pointers to Pointers）
int ival = 1024;
int *pi = &ival; // pi points to an int
int **ppi = π // ppi points to a pointer to an int
指向指针的指针用**表示。这里的**ppi相当于解析了两次，第一次解析出*pi中的ival地址，第二次解析出ival中的数值。
函数中的参数传递
Autoware中出现的代码:
poseToIndex(pose, index_x, index_y, &index_thet
a. passed by value
实参的值拷贝给形参，函数中对该形参所作的任何改动不会影响实参的值。
void reset(int *ip)
{
*ip = 0; // changes the value of the object to which ip points
ip = 0; // changes only the local copy of ip; the argument is unchanged
}
以上函数的调用过程:
int i = 42;
reset(&i); // changes i but not the address of i
cout << "i = " << i << endl; // printsi = 0
C语言中指针类型的形参经常用于访问函数外部的变量，而在C++中，建议使用引用类型的形参代替指针
b. passed by reference
例子:
// function that takes a reference to an int and sets the given object to zero
void reset(int &i) // i is just another name for the object passed to reset
{
i = 0; // changes the value of the object to which i refers
}
调用部分：
int j = 42;
reset(j); // j is passed by reference; the value in j is changed
cout << "j = " << j << endl; // printsj = 0
这里的函数中实参传递相当于&i = j,然后i = 0导致j = 0。（在传引用参数的过程中，不需要传递变量的地址）