Autoware源码分析——astar_search

1. 概要

本文是对autoware中core_planning文件夹中astar_search功能包的相关文件组成和具体源码的分析,承接上一篇关于astar_avoid的分析。

如果有理解有误的地方,望指正!

2. 文件结构

Autoware源码分析——astar_search_第1张图片

2.1 astar_avoid中与之有关的函数

astar_avoid.cpp中的planAvoidWaypoints调用了astar_search中的astar_.makePlan()等函数,目的就是执行hybrid A* 算法。

planAvoidWaypoints

该函数使用了astar_search中的相关函数:makePlan()initialize()reset()getPath()

该函数的大致流程就是:

  1. 定义一些用于判断条件的变量。
  2. 调用astar_search的这些函数以实现hybrid A* 算法。具体来说,先逐步更新终点位姿和索引信息(“索引”这个变量非常重要,还没有太明白其本质意思。),再初始化地图(也就是costmap信息)并执行makePlan函数,如果找到路径就发布路径点信息,如果找不到就reset程序。

2.2 头文件

astar_search使用了两个头文件,一个是astar_util.h, 作用是定义程序中所需要的一些“数据结构”(struct类)和一些用于计算的函数;另一个是astar_search.h, 用于定义AstarSearch这个类中的信息,比如构造函数、析构函数、相关公有或私有变量或方法。以下对这两个文件进行分析。

astar_util.h

  1. 算法中需要的数据结构定义
enum class STATUS : uint8_t
struct AstarNode...
struct WaveFrontNode...
struct NodeUpdate...
struct SimpleNode...
  1. 算法中需要用到的函数(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

  1. 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_;
  }
  1. 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的参数之后返回TrueFalse

该函数的源代码如下:

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. 函数的大致步骤

  1. 定义与系统时间相关的变量
  2. 判断算法使用的openlist是否为empty,如果是empty, 搜索失败;如果不是empty,就进行循环。
  3. 搜索过程开始:从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
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 v1 vector that holds objects of type T. Default initialization; v1 is empty.
vector v2(v1) v2 has a copy of each element in v1.
vector v2 = v1 Equivalent to v2(v1), v2 is a copy of the elements in v1.
vector v3(n, val) v3 has n elements with value val.
vector v4(n) v4 has n copies of a value-initialized object.
vector v5{a,b,c . . .} v5 has as many elements as there are initializers; elements are initialized by corresponding initializers.
vector 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

这属于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_frontpush_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的区别:

  1. 指针本身就是一个对象,可以指向多个不同的对象(引用只能绑定一个对象)。
  2. 指针无需在定义时赋初值。(引用类型需要赋初值)

指针本身是存放对象的地址(获取对象地址的方式是取址符&)。给指针赋值就是给指针变量本身一个新的值,即存放新的地址从而“指向”新的对象

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。(在传引用参数的过程中,不需要传递变量的地址)

你可能感兴趣的:(智能汽车)