Autoware源码分析——astar_search

最新推荐文章于 2024-06-07 17:31:29 发布

RavenRaaven

最新推荐文章于 2024-06-07 17:31:29 发布

阅读量1.9k

点赞数 7

分类专栏：智能汽车

本文链接：https://blog.csdn.net/qq_41871826/article/details/107752286

版权

智能汽车专栏收录该内容

14 篇文章 33 订阅

订阅专栏

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,\Theta)$ ，程序中用到的角度步长为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）

#include<vector>
using std::vector

Class 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<T> v1	vector that holds objects of type T. Default initialization; v1 is empty.
vector<T> v2(v1)	v2 has a copy of each element in v1.
vector<T> v2 = v1	Equivalent to v2(v1), v2 is a copy of the elements in v1.
vector<T> v3(n, val)	v3 has n elements with value val.
vector<T> v4(n)	v4 has n copies of a value-initialized object.
vector<T> v5{a,b,c . . .}	v5 has as many elements as there are initializers; elements are initialized by corresponding initializers.
vector<T> v5 = {a,b,c . . . }	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 <queue>。

这属于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

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 = &pi; // 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。（在传引用参数的过程中，不需要传递变量的地址）

RavenRaaven

关注

7
点赞
踩
32

收藏

觉得还不错? 一键收藏
0
评论
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该函数使用
复制链接

扫一扫