Autoware源码分析——astar_avoid
概要
本文是对autoware中waypoint_planner功能包的相关文件组成和具体节点的分析。由于程序比较复杂，我认为还存在一些不完整的地方，之后也会继续分析，继续更新。在看代码的过程中也学习了一些之前不清楚的ROS和C++的要点和ROS和autoware的一些具体消息类型（这部分在文末）。
有任何错误或不足之处，望指正！
1. waypoint_planner功能包的概述及整体结构
1.1 功能包概述
waypoint_planner功能包用于生成到目标的道路点waypoints的集合。它与lane_planner的区别在于它发布的是一个道路点而不是道路点的数组。
1.2 文件结构
从下图可以看出，waypoint planner的源文件src中由两个子功能包组成。一个是astar_avoid，另一个是velocity_set，astar_avoid文件夹中最重要的就是astar_avoid.cpp文件。
2. astar_avoid介绍
2.1 功能概述
autoware自带readme文件中提到，这里的astar_avoid使用的算法是基于hybrid A* 的，相关算法在astar_search的功能包里。
astar_avoid存在两种模式，一种是relay模式，第二种是avoidance模式，前者不能实现避障的功能，只是根据当前车辆的位姿生成waypoints；后者可以根据hybrid A* 算法中的内部状态转移的规则进行避障。
2.2 所使用到的文件
运行该算法依靠的文件是astar_avoid.launch文件，该文件实现的两个功能，第一个功能是设置一些astar_avoid程序中构造函数和A* search程序中的参数；第二个功能是启动astar_avoid节点。
2.3 astar_avoid.cpp分析
i. 所含部分
该文件基本可以分为5个部分，分别是：类AstarAvoid的构造函数、类的析构函数、回调函数callbacks、“主函数”（AstarAvoid::run()）、“主函数”所用到的子函数定义。
以上五个部分所使用的库均在astar_avoid.h文件中定义。该头文件中定义了必要的公有变量（用于avoidance模式下的状态）、私有“变量”（包括ROS中的节点句柄NodeHandle、订阅者和发布者；定义启动节点需要设置的参数parameters，线程所用到的类；程序内部使用的变量）、回调函数和私有成员函数原型的定义。
ii. 具体包含的函数分析
AstarAvoid::AstarAvoid()
这部分是该类的构造函数，做了三件事：
- 赋给程序用到的参数一些默认值
- 设置节点句柄所用的parameters
- 定义了唯一一个发布者，即
safety_waypoints_pub_ = nh_.advertise - 定义若干订阅者
（advertise(), subsribe()函数见第三部分）
AstarAvoid::~AstarAvoid()
第二部分是该类的析构函数，这里只有一行代码：publish_thread_.join();，这里join()的目的是在类消失的时候，阻塞主函数，让该线程继续运行，运行结束之后，再运行主函数。(C++11多线程)
回调函数callbacks
...
// functions, callback
void costmapCallback(const nav_msgs::OccupancyGrid& msg);
void currentPoseCallback(const geometry_msgs::PoseStamped& msg);
void currentVelocityCallback(const geometry_msgs::TwistStamped& msg);
void baseWaypointsCallback(const autoware_msgs::Lane& msg);
void closestWaypointCallback(const std_msgs::Int32& msg);
void obstacleWaypointCallback(const std_msgs::Int32& msg);
...
通过astar_avoid.h中的回调函数原型定义可以看出，如果该程序接受到了这几类消息，那么在ros::spinOnce()之后，这些回调函数就会被执行一次（通常ros::spinOnce()在一个循环内部，所以说执行一次）比如costmapCallback函数接收含有消息类型nav_msgs::OccupancyGrid的消息之后，将其存储到变量costmap_中，对其地图的原点位姿消息转换到TF坐标系中（tf::poseMsgToTF(costmap_.info.origin, local2costmap_);），并将costmap初始化标志位置为true，也就是costmap_initialized_ = true;。
个人认为以上这些回调函数的目的是为了用于程序判断程序是否已初始化完成。
AstarAvoid::run()
为什么这里是“主函数”？先看astar_avoid_node.cpp文件的代码：
...
#include "waypoint_planner/astar_avoid/astar_avoid.h"
int main(int argc, char** argv)
{
ros::init(argc, argv, "astar_avoid");
AstarAvoid node;
node.run();
return 0;
}
其中最主要的语句就是node.run()，它将执行AstarAvoid::run()这个类的方法，所以这里类比为“主函数”。
该函数的基本流程
- 定义或重置一些Index和时间，并检查程序是否初始化完成，如果失败，则继续等待要订阅的相关消息。
...
void AstarAvoid::run()
{
// check topics
state_ = AstarAvoid::STATE::INITIALIZING;
while (ros::ok())
{
ros::spinOnce();
if (checkInitialized())
{
break;
}
ROS_WARN("Waiting for subscribing topics...");
ros::Duration(1.0).sleep();
}
...
- 做主循环开始之前的一些准备工作
....
// main loop
int end_of_avoid_index = -1;//初始化用于终止发布路径点的索引
ros::WallTime start_plan_time = ros::WallTime::now();//设置规划开始的时间。
ros::WallTime start_avoid_time = ros::WallTime::now();
// reset obstacle index
obstacle_waypoint_index_ = -1;//初始化用于表示障碍物的索引
// relaying mode at startup
state_ = AstarAvoid::STATE::RELAYING;
// start publish thread
publish_thread_ = std::thread(&AstarAvoid::publishWaypoints, this);//开启第二个线程。
...
- 循环中的第一个阶段：relay mode（默认模式）
...
while (ros::ok())
{
ros::spinOnce();
// relay mode
if (!enable_avoidance_)
{
rate_->sleep();
continue;
}
...
- 循环中的第二部分：在满足相应条件之后开启avoidance模式（这段代码比较长，这里不做粘贴了）
首先，定义了两个用于后续判断任务的变量：受否发现障碍物bool found_obstacle，速度是否超过规定的速度bool avoid_velocity，bool replan（用于判断是否需要重新规划）和bool reached（用于判断是否已经到了终点）。在avoidance模式下，总共有四种状态，分别是relaying, stopping, planning和avoiding，本人总结了以下转移的过程，如下图所示：（线段中的字符表示判断条件）
状态转移中用到的其他子函数
...
// functions
bool checkInitialized();
bool planAvoidWaypoints(int& end_of_avoid_index);
void mergeAvoidWaypoints(const nav_msgs::Path& path, int& end_of_avoid_index);
void publishWaypoints();
tf::Transform getTransform(const std::string& from, const std::string& to);
int getLocalClosestWaypoint(const autoware_msgs::Lane& waypoints, const geometry_msgs::Pose& pose, const int& search_size);
};
...
（由于另一个功能包A* search还没有仔细分析，后续将不断对这部分进行更新）
代码较短的部分我就粘贴在这里。
a. bool AstarAvoid::checkInitialized
该函数用于检测初始化的条件是否满足。最初initialized的值是false。从以下的程序可以看出，relay mode需要满足的条件比avoidance mode要少。后者还需要检查costmap（或者说就是occupancy grid map）和current_velocity是否初始化完成。
bool AstarAvoid::checkInitialized()
{
bool initialized = false;
// check for relay mode
//这里的relay mode需要满足四个条件。
initialized = (current_pose_initialized_ && closest_waypoint_initialized_ && base_waypoints_initialized_ &&
(closest_waypoint_index_ >= 0));
// check for avoidance mode, additionally
if (enable_avoidance_)
{
initialized = (initialized && (current_velocity_initialized_ && costmap_initialized_));
}
return initialized;
}
（b和c都用到了A* search包中的函数，这里之后更新。）
b. bool AstarAvoid::planAvoidWaypoints
c. void mergeAvoidWaypoints
d. void publishWaypoints
该函数的目的是单独创建一个发布安全的道路点waypoints的线程。这个发布者也是astar_avoid中的唯一一个发布者，从中也可以直观看出该程序的功能就是经过一番运算，发布车辆可以安全行驶的道路点。
该函数的两大步骤就是:
- 根据状态选择当前的waypoints, 也就是
current_waypoints的设置。
switch (state_)
{
case AstarAvoid::STATE::RELAYING:
current_waypoints = base_waypoints_;
break;
case AstarAvoid::STATE::STOPPING:
// do nothing, keep current waypoints
break;
case AstarAvoid::STATE::PLANNING:
// do nothing, keep current waypoints
break;
case AstarAvoid::STATE::AVOIDING:
current_waypoints = avoid_waypoints_;
break;
default:
current_waypoints = base_waypoints_;
break;
}
- 把waypoints“压”到数组中，即道路点的发布
这里用到了vector中的push_back()函数，解释见第三部分。
...
if (safety_waypoints.waypoints.size() > 0)
{
safety_waypoints_pub_.publish(safety_waypoints);
}
...
e. tf::Transform getTransform
这一部分使用了ROS中的tf功能包。这个函数的目的就是通过tf::transform_listener，用try...except语句找到两个带有frame id的数据之间的变换，找到之后会返回这个变换，否则报错。
tf::Transform AstarAvoid::getTransform(const std::string& from, const std::string& to)
{
tf::StampedTransform stf;
try
{
tf_listener_.lookupTransform(from, to, ros::Time(0), stf);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
}
return stf;
}
f. int getLocalClosestWaypoint
（该函数的步骤也后续更新）
3. 分析过程中的问题解决
由于是第一次看如此复杂的工程，期间遇到了一些记不太清楚和不太明白的编程要点，这部分对这些问题进行汇总，便于之后的查阅。
3.1 C++和ROS语法相关的一些要点
I. ROS：
ROS相关的问题大部分都可以在官方的wiki上找到答案。
：
a. 订阅者和发布者相关的函数
http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29
http://wiki.ros.org/roscpp/Overview/Publishers%20and%20Subscribers#Subscribing_to_a_Topic
发布者的advertise()函数
该函数会先与ros master进行通信，注册相关参数。常见语法如下：
ros::Publisher pub = node_handle.advertise<message_type>(topic_name, queue_size);
形参 (parameters) 是: message_type, topic_name, queue_size（队列的大小一般为1000）
该函数的返回值是ros::Publisher对象，其有两个目的：
- 之后使用对象中的publish()方法进行消息发布；
- 当消息超出范围之后，该对象会自动停止发布。
定义完该对象之后，在后续的主函数中使用自定义或ROS的标准消息类型（message-type）定义具体的消息（message content），通过pub.publish(msg)即可，格式如下：
message-type msg;
msg.xxx=xxx;
关于advertise中的第三个参数，可以参考以下图片。该参数为true的时候，该话题的上一个消息会被保存并且发给新的订阅者。
b. subscriber的写法
根据ROS官方wiki给出的教程：
callback:NodeHandle::subscribe() returns a ros::Subscriber object. When the Subscriber object is destructed, it will automatically unsubscribe from the chatter topic.
上文说明subscribe()函数返回的是Subscriber对象，以下是该函数的定义：
ros::Subscriber subscribe(const std::string& topic, uint32_t queue_size, <callback, which may involve multiple arguments>,
const ros::TransportHints& transport_hints = ros::TransportHints());
如果回调函数是在某个类中进行定义，则这里的
current_velocity_sub_ = nh_.subscribe("current_velocity", 1, &AstarAvoid::currentVelocityCallback, this);
一开始并不太理解this，这里的this就是指向AstarAvoid该类的指针。
整个订阅者的简要过程
- 初始化ROS节点
- 订阅某个话题（topics）
ros::spinOnce()会等待这些消息的到来- 消息接收之后，执行回调函数。
c. nodehandler中的.param函数
不理解的语句如下：
private_nh_.param<bool>("enable_avoidance", enable_avoidance_, false);
括号内参数的含义:
（查找过程：在网页http://docs.ros.org/melodic/api/roscpp/html/classros_1_1NodeHandle.html中搜索ros::NodeHandle::param）
相关定义为：
template<typename T >
bool ros::NodeHandle::param (const std::string& param_name, T& param_val, const T& default_val)
This method tries to retrieve the indicated parameter value from the parameter server, storing the result in param_val. If the value cannot be retrieved from the server, default_val is used instead.
可以看出，param()函数的目的就是从参数服务器中取出key为param_name的值，并将其存放到param_val中，如果服务器中不存在此参数，则使用默认值。本节中的例子可以解释为：从参数服务器中取出key为enable_avoidance的值，将其存放到enable_avoidance_中，如果没有取出，则使用false作为该参数的默认值。
d. rate_->sleep();
相关链接：http://docs.ros.org/diamondback/api/rostime/html/classros_1_1Rate.html
该语句的目的就是一个周期内的剩余时间均处于sleep状态。计时是从上一次睡眠，重置或调用constructor开始。
e. ros::ok()
ros::ok()返回false的情况有：
（该情况下，所有的ROS调用都会失败）
a. 终端输入ctrl-C
b. 相同节点名称的节点将当前节点踢出网络（kick off）
c. ros::shutdown()被调用
d. 所有的ros::NodeHandles被消除。
f. ros::spinOnce()
相关链接： http://wiki.ros.org/roscpp/Overview/Callbacks%20and%20Spinning
该语句属于单线程（single-threaded spinning）。
还有一种方式是是ros::spin()，这条语句的目的就是循环使用ros::spinOnce()，也就是所有的回调函数在其内部被调用，直到调用ros::shutdown()或ctrl-C出现，程序终止。
加入ros::spinOnce()之后，ROS把订阅相关（收到Message）的回调函数推到一个队列（queues/spinning）中，ROS不会立即调用它。
g. ros::Duration(1.0).sleep()和ros::WallTime
ros::Duration(1.0).sleep()
相关链接：http://wiki.ros.org/roscpp/Overview/Time
例子：
ros::Duration(0.5).sleep(); // sleep for half a second
ros::WallTime
Wall Time可以理解为墙上时间，墙上挂着的时间没有人改变的了，永远在往前走；ROS Time可以被人为修改，你可以暂停它，可以加速，
可以减速，但是Wall Time不可以。
wall time 的接口与Time类的接口一样
h. tf listener相关
相关链接：http://docs.ros.org/diamondback/api/tf/html/c++/classtf_1_1TransformListener.html
lookupTransform的格式可以参考以下链接：
http://docs.ros.org/diamondback/api/tf/html/c++/classtf_1_1Transformer.html#ac01a9f8709a828c427f1a5faa0ced42b
II. C++相关
C++的官方文档链接：http://www.cplusplus.com/reference/
a. 析构函数destructor
参考链接：https://www.runoob.com/cplusplus/cpp-constructor-destructor.html
类的析构函数是类的一种特殊的成员函数，它会在每次删除所创建的对象时执行。
析构函数的名称与类的名称是完全相同的，只是在前面加了个波浪号（~）作为前缀，它不会返回任何值，也不能带有任何参数。析构函数有助于在跳出程序（比如关闭文件、释放内存等）前释放资源。
b. thread和mutex:
#include 以上两个include是C++11关于多线程任务的新标准。
多线程是多任务处理的一种特殊形式，多任务处理允许让电脑同时运行两个或两个以上的程序。一般情况下，两种类型的多任务处理：基于进程和基于线程。
- 基于进程的多任务处理是程序的并发执行。
- 基于线程的多任务处理是同一程序的片段的并发执行
创建线程的方法：std::thread name(threadFunction)，join()是让阻塞当前主线程，等待所有的子线程执行完毕之后主线程才能继续执行或退出。
mutex是用来保证线程同步的，防止不同的线程同时操作同一个共享数据。
std::mutex mutex_;
std::lock_guard<std::mutex> lock(mutex_);
c. C++ 关键字this
相关链接：https://www.runoob.com/cplusplus/cpp-this-pointer.html
在 C++ 中，每一个对象都能通过this指针来访问自己的地址。this指针是所有成员函数的隐含参数。只有成员函数才有this 指针。
d. static_cast
相关链接：https://www.cnblogs.com/QG-whz/p/4509710.html
static_cast是一个强制类型转换操作符。强制类型转换，也称为显式转换。
例如，static_cast 就是将某个数据类型强制转化为int
e. C++ 中的push_back函数
use a vector member named push_back to add elements at runtime. The push_back operation takes a value and “pushes” that value as a new last element onto the “back” of the vector.
f. C++ 中的auto数据类型和for循环中的冒号写法
- auto可以在声明变量的时候根据变量初始值的类型自动为此变量选择匹配的类型。
- C++ 11中for循环的新写法
相关链接：https://blog.csdn.net/qq_39856931/article/details/106764565
int array[10] = { 54, 23, 78, 9, 15, 18, 63, 33, 87, 66 };
for (auto a : array) {
cout << a << " "; //输出：54 23 78 9 15 18 63 33 87 66
}
可以解释为a是array中的某个元素，将每次遍历array得到的结果赋值给a。
g. .at(i)操作：返回vector中下标为i的元素
vector（T是某种类型）的at函数接收一个vector::size_type类型的参数n（一种无符号整型类型），返回容器位置为n的元素的引用（注意第一个位置为0）。
3.2 该程序所需要的消息类型（可能不太全，可以自行查阅相关msgs文件）
ROS的msgs文件的路径在：/opt/ros/kinetic/share/，或者通过ROS WIKI进行搜索。
autoware的相关msgs文件路径在：~/autoware.ai/src/autoware/msessages/（版本1.12.0）
1. nav_msgs::OccupancyGrid
# This represents a 2-D grid map, in which each cell represents the probability of
# occupancy.
Header header
#MetaData for the map
MapMetaData info
# The map data, in row-major order, starting with (0,0). Occupancy
# probabilities are in the range [0,100]. Unknown is -1.
int8[] data
# compact message def：
std_msgs/Header header
nav_msgs/MapMetaData info
int8[] data
子类型定义：
std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
string frame_id
nav_msgs/MapMetaData
# This hold basic information about the characterists of the OccupancyGrid
# The time at which the map was loaded
time map_load_time
# The map resolution [m/cell]
float32 resolution
# Map width [cells]
uint32 width
# Map height [cells]
uint32 height
# The origin of the map [m, m, rad]. This is the real-world pose of the
# cell (0,0) in the map.
geometry_msgs/Pose origin
geometry_msgs/Pose
geometry_msgs/Point position
geometry_msgs/Quaternion orientation
geometry_msgs/Quaternion
float64 x
float64 y
float64 z
float64 w
geometry_msgs::PoseStamped
# A Pose with reference coordinate frame and timestamp
std_msgs/Header header
geometry_msgs/Pose pose
geometry_msgs::TwistStamped
# A twist with reference coordinate frame and timestamp
std_msgs/Header header
geometry_msgs/Twist twist
其中的子类型定义:
geometry_msgs::Twist（代表车辆的速度）
# This expresses velocity in free space broken into its linear and angular parts.
geometry_msgs/Vector3 linear
geometry_msgs/Vector3 angular
geometry_msgs/Vector3
float64 x
float64 y
float64 z
autoware_msgs::Lane
Header header
int32 increment
int32 lane_id
Waypoint[] waypoints
uint32 lane_index
float32 cost
float32 closest_object_distance
float32 closest_object_velocity
bool is_blocked
autoware_msgs::Waypoint
# global id
int32 gid
# local id
int32 lid
geometry_msgs/PoseStamped pose
geometry_msgs/TwistStamped twist
DTLane dtlane
int32 change_flag
WaypointState wpstate
uint32 lane_id
uint32 left_lane_id
uint32 right_lane_id
uint32 stop_line_id
float32 cost
float32 time_cost
# Lane Direction
# FORWARD = 0
# FORWARD_LEFT = 1
# FORWARD_RIGHT = 2
# BACKWARD = 3
# BACKWARD_LEFT = 4
# BACKWARD_RIGHT = 5
# STANDSTILL = 6
uint32 direction
本文实在有点长，见谅