Autoware planning模块学习笔记（二）：路径规划（1）
继续为大家讲解Autoware planning模块，这里详细分析下其路径规划的实现过程。具体的操作过程大家可以移步autoware入门教程-使用rosbag数据进行路径规划，里面介绍了加载路径点设定任务，启动路径规划和导航等的操作，后面的代码运行过程，博主在此为大家解读一下。
勾选waypoint_loader
因为功能选项仍然是在computing分页，我们依然查找src/autoware/utilities/runtime_manager/scripts/computing.yaml可知勾选waypoint_loader（防盗标记：zhengkunxian）后运行ROS包waypoint_maker内的waypoint_loader.launch
waypoint_loader.launch如下：
也就是启动waypoint_loader，waypoint_replanner和waypoint_marker_publisher三个节点
由waypoint_maker的CMakeLists.txt可知节点waypoint_loader的源文件是nodes/waypoint_loader/waypoint_loader_core.cpp和nodes/waypoint_loader/waypoint_loader_node.cpp，（防盗标记：zhengkunxian）节点waypoint_replanner的源文件是nodes/waypoint_replanner/waypoint_replanner.cpp和nodes/waypoint_replanner/waypoint_replanner_node.cpp，节点waypoint_marker_publisher的源文件是nodes/waypoint_marker_publisher/waypoint_marker_publisher.cpp
节点waypoint_loader
我们先看waypoint_loader节点，首先是其main函数，main函数在waypoint_loader_node.cpp中，节点的功能主要通过WaypointLoaderNode对象的run函数实现。
int main(int argc, char** argv)
{
ros::init(argc, argv, "waypoint_loader");
waypoint_maker::WaypointLoaderNode wln;
wln.run();
return 0;
}
先分析WaypointLoaderNode的构造函数，其调用了initPubSub函数。
WaypointLoaderNode::WaypointLoaderNode() : private_nh_("~")
{
initPubSub();
}
initPubSub函数中对multi_lane_csv_（头文件定义std::string multi_lane_csv_）根据launch文件赋予初始值，我们分析时采用默认值/tmp/driving_lane.csv，接着设置topic为/based/lane_waypoints_raw的消息发布者lane_pub_。
void WaypointLoaderNode::initPubSub()
{
private_nh_.param<std::string>("multi_lane_csv", multi_lane_csv_, "/tmp/driving_lane.csv");
// setup publisher
lane_pub_ = nh_.advertise<autoware_msgs::LaneArray>("/based/lane_waypoints_raw", 10, true);
}
再看waypoint_loader_core.cpp中的run函数。multi_file_path_在头文件waypoint_loader_core.h中的定义为std::vector< std::string > multi_file_path_;
void WaypointLoaderNode::run()
{
multi_file_path_.clear();
parseColumns(multi_lane_csv_, &multi_file_path_);
autoware_msgs::LaneArray lane_array;
createLaneArray(multi_file_path_, &lane_array);
lane_pub_.publish(lane_array);
output_lane_array_ = lane_array;
ros::spin();
}
parseColumns函数如下，其作用是什么呢？我们依旧通过在线C++编辑器进行分析。
void parseColumns(const std::string& line, std::vector<std::string>* columns)
{
std::istringstream ss(line);
std::string column;
while (std::getline(ss, column, ','))
{
while (1)
{
auto res = std::find(column.begin(), column.end(), ' ');
if (res == column.end())
{
break;
}
column.erase(res);
}
if (!column.empty())
{
columns->emplace_back(column);
}
}
}
从下面的运行结果可知，parseColumns(const std::string& line, std::vectorstd::string* columns)函数以“，”作为分隔符将字符串line分成若干段，并将其中的空格全部去除，依次储存至字符串向量columns中。
接着分析createLaneArray(multi_file_path_, &lane_array)的作用，很容易看出其将multi_file_path_中的元素包含的信息分别填入autoware_msgs::Lane对象中，再将autoware_msgs::Lane对象依次填入autoware_msgs::LaneArray。
void WaypointLoaderNode::createLaneArray(const std::vector<std::string>& paths, autoware_msgs::LaneArray* lane_array)
{
for (const auto& el : paths)
{
autoware_msgs::Lane lane;
createLaneWaypoint(el, &lane);
lane_array->lanes.emplace_back(lane);
}
}
具体分析一下createLaneWaypoint(el, &lane)抽取el中的哪些信息填入lane中？
void WaypointLoaderNode::createLaneWaypoint(const std::string& file_path, autoware_msgs::Lane* lane)
{
if (!verifyFileConsistency(file_path.c_str()))
{
ROS_ERROR("lane data is something wrong...");
return;
}
ROS_INFO("lane data is valid. publishing...");
FileFormat format = checkFileFormat(file_path.c_str());
std::vector<autoware_msgs::Waypoint> wps;
if (format == FileFormat::ver1)
{
loadWaypointsForVer1(file_path.c_str(), &wps);
}
else if (format == FileFormat::ver2)
{
loadWaypointsForVer2(file_path.c_str(), &wps);
}
else
{
loadWaypointsForVer3(file_path.c_str(), &wps);
}
lane->header.frame_id = "/map";
lane->header.stamp = ros::Time(0);
lane->waypoints = wps;
}
首先看verifyFileConsistency函数的作用，根据函数名，它的作用是“验证文件一致性”。首先ifstream以输入方式打开文件filename，如果打开失败那么就直接返回false；如果成功打开此文件，则执行checkFileFormat函数。
bool WaypointLoaderNode::verifyFileConsistency(const char* filename)
{
ROS_INFO("verify...");
std::ifstream ifs(filename);
if (!ifs)
{
return false;
}
FileFormat format = checkFileFormat(filename);
ROS_INFO("format: %d", static_cast<int>(format));
if (format == FileFormat::unknown)
{
ROS_ERROR("unknown file format");
return false;
}
std::string line;
std::getline(ifs, line); // remove first line
size_t ncol = format == FileFormat::ver1 ? 4 // x,y,z,velocity
:
format == FileFormat::ver2 ? 5 // x,y,z,yaw,velocity
:
countColumns(line);
while (std::getline(ifs, line)) // search from second line
{
if (countColumns(line) != ncol)
{
return false;
}
}
return true;
}
checkFileFormat函数如下，首先判断能不能打开filename这个文件，如果不能打开则返回FileFormat::unknown。FileFormat是定义在头文件waypoint_loader_core.h中的一种特别的 “枚举类”，其中unknown = -1。
FileFormat WaypointLoaderNode::checkFileFormat(const char* filename)
{
std::ifstream ifs(filename);
if (!ifs)
{
return FileFormat::unknown;
}
// get first line
std::string line;
std::getline(ifs, line);
// parse first line
std::vector<std::string> parsed_columns;
parseColumns(line, &parsed_columns);
// check if first element in the first column does not include digit
if (!std::any_of(parsed_columns.at(0).cbegin(), parsed_columns.at(0).cend(), isdigit))
{
return FileFormat::ver3;
}
// if element consists only digit
int num_of_columns = countColumns(line);
ROS_INFO("columns size: %d", num_of_columns);
return (num_of_columns == 3 ? FileFormat::ver1 // if data consists "x y z (velocity)"
:
num_of_columns == 4 ? FileFormat::ver2 // if data consists "x y z yaw (velocity)
:
FileFormat::unknown);
}
接着看std::getline(ifs, line)的作用：istream& getline ( istream &is , string &str , char delim )中is是进行读入操作的输入流，str被用来存储读入的内容，delim是终结符，遇到该字符停止读取操作，不写的话默认为回车。通过下面的验证示例可以发现，通过getline函数读取了driving_lane.csv文件的第一行，并储存至字符串变量line中。
接着执行parseColumns函数，这个函数的作用前面已经分析过了，以“，”作为分隔符将字符串line分成若干段，并将其中的空格全部去除，依次储存至字符串向量中。后面借助any_of函数判断所读取的文件内的第一行中的第一个元素是否不包含数字，不包含（意思是第一行记录的是标题）则返回FileFormat::ver3。若包含数字（意思是第一行记录的就是数据）则继续调用countColumns函数，获取第一行所包含的元素个数，以“，”作为元素之间的分隔符。若个数为3，则返回FileFormat::ver1；若个数为4，则返回FileFormat::ver2；其他情形则返回FileFormat::unknown。
size_t countColumns(const std::string& line)
{
std::istringstream ss(line);
size_t ncol = 0;
std::string column;
while (std::getline(ss, column, ','))
{
++ncol;
}
return ncol;
}
countColumns函数功能验证：
分析完checkFileFormat函数，回到调用它的verifyFileConsistency函数。如果checkFileFormat函数返回的是FileFormat::unknown，则return false。否则在verifyFileConsistency函数内也借助getline函数读取一次filename文件内的数据，getline函数具备一个特性就是被调用一次后下次调用就会自动读取下一行。具体可见下面的验证，值得注意的是，在不同函数内被调用的时候是不会互相影响的。按照源代码中的注释，其用意是以备后面调用的时候直接从第二行的数据开始。为什么呢？因为verifyFileConsistency函数内size_t ncol = format = = FileFormat::ver1 ? 4 : format = = FileFormat::ver2 ? 5 : countColumns(line);在checkFileFormat函数返回FileFormat::ver3情形时（也就是存在需要另外解释的数据）需要另外确定一致性检查（每行列数）的标准，因此此时读取的第一行数据已经被使用了，应该直接从第二行开始判断其是否都和第一行的列数一致。
至此，我们知道了verifyFileConsistency函数所谓的“验证文件一致性”的具体含义，其实就是检查每行记录的数据个数（列数）是否一致。中途总结一下，一致性检查未通过的情形包括：（1）无法打开文件，可能文件不存在；（2）格式检查后为FileFormat::unknown，而FileFormat::unknown包含这两种情形（i，无法打开文件，按理说这里只是保险期间又加了一道验证是否能打开文件的命令；ii，读取文件第一行发现包含数据，但是在后续判断这一行数据数据个数的时候发现其既不是3也不是4，这其实都是Autoware预设的数据储存格式，其中3代表data consists “x y z (velocity)”，4代表data consists “x y z yaw (velocity)”）。这里有个疑问，为什么checkFileFormat函数里面3列定义为FileFormat::ver1，而到了verifyFileConsistency函数里却将FileFormat::ver1的列数设为4,并且如果后面行的列数不为4就认为一致性不通过；对于FileFormat::ver2也是类似的问题，为什么checkFileFormat函数内的4列到了verifyFileConsistency函数就成了5列？是因为括号内的velocity吗？但是文件还是那个文件没变呀？只能回校后实车跑一下看看为啥了（3）检查每一行数据的列数，发现与第一行不一致。
我们继续往回捋，返回调用verifyFileConsistency函数的createLaneWaypoint函数，如果一致性检查未通过，则报错提醒并返回。否则继续执行checkFileFormat函数，判断存储数据的具体操作如下：
std::vector<autoware_msgs::Waypoint> wps;
if (format == FileFormat::ver1)
{
loadWaypointsForVer1(file_path.c_str(), &wps);
}
else if (format == FileFormat::ver2)
{
loadWaypointsForVer2(file_path.c_str(), &wps);
}
else
{
loadWaypointsForVer3(file_path.c_str(), &wps);
}
我们以FileFormat::ver1这一情形中所调用的loadWaypointsForVer1函数为例，其他情形流程上也是差不多的。重复的内容不再赘述了，loadWaypointsForVer1里面调用了parseWaypointForVer1
void WaypointLoaderNode::loadWaypointsForVer1(const char* filename, std::vector<autoware_msgs::Waypoint>* wps)
{
std::ifstream ifs(filename);
if (!ifs)
{
return;
}
std::string line;
std::getline(ifs, line); // Remove first line
while (std::getline(ifs, line))
{
autoware_msgs::Waypoint wp;
parseWaypointForVer1(line, &wp);
wps->emplace_back(wp);
}
size_t last = wps->size() - 1;
for (size_t i = 0; i < wps->size(); ++i)
{
if (i != last)
{
double yaw = atan2(wps->at(i + 1).pose.pose.position.y - wps->at(i).pose.pose.position.y,
wps->at(i + 1).pose.pose.position.x - wps->at(i).pose.pose.position.x);
wps->at(i).pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
}
else
{
wps->at(i).pose.pose.orientation = wps->at(i - 1).pose.pose.orientation;
}
}
}
parseWaypointForVer1函数如下，其实就是将输入的这一行line中的数据依次由string转化为double类型后存入autoware_msgs::Waypoint类变量wp中。将每一行数据都分别存入autoware_msgs::Waypoint类变量并依次填入autoware_msgs::Waypoint向量std::vector<autoware_msgs::Waypoint> wps中。接着根据相邻两个轨迹点计算航向角。
void WaypointLoaderNode::parseWaypointForVer1(const std::string& line, autoware_msgs::Waypoint* wp)
{
std::vector<std::string> columns;
parseColumns(line, &columns);
wp->pose.pose.position.x = std::stod(columns[0]);
wp->pose.pose.position.y = std::stod(columns[1]);
wp->pose.pose.position.z = std::stod(columns[2]);
wp->twist.twist.linear.x = kmph2mps(std::stod(columns[3]));
}
轨迹点都存储好后，最后再回到调用loadWaypointsForVer1函数的createLaneWaypoint函数。将std::vector<autoware_msgs::Waypoint> wps存入autoware_msgs::Lane lane中，加上帧id和时间戳，那么一条道路也就是一个文件内储存的所有路径点都被用来建立一个autoware_msgs::Lane lane了，for循环继续读取其他文件，重复上面的操作，最后所有的lane构成了lane集合autoware_msgs::LaneArray lane_array。接着lane_pub_ = nh_.advertise<autoware_msgs::LaneArray>("/based/lane_waypoints_raw", 10, true)广播lane_array给其他ROS节点。
写在本节最后
我是不是挺罗嗦的呀？感觉自己讲得太细了，写多了也难免会有些疏漏。但是疫情期间呆在家里不写点东西根本看不进去代码，家里确实没有学校里学习科研氛围浓厚，只能通过写作刺激自己看代码了。其实这是一种目标奖励的机制，不断写写写，就能获得持续的成就感，这些就当作日记一样的东西了(*/ω＼*)
最后祝愿伟大祖国早日击败疫情！我们这些在家的学子虽无力在一线抗疫，但是也在尽己所能为国家的科技进步填砖加瓦。科教盛，国家胜！
