（三）学习笔记autoware源码core_planning（waypoint

1.waypoint_loader_node

#include <ros/ros.h>#include "waypoint_loader_core.h"int main(int argc, char** argv)
{ros::init(argc, argv, "waypoint_loader");waypoint_maker::WaypointLoaderNode wln;wln.run();return 0;
}
/*首先是其main函数，main函数在 waypoint_loader_node.cpp中，节点的功能主要通过WaypointLoaderNode对象的run函数实现。*/

2.waypoint_loader_core

#include "waypoint_loader_core.h"namespace waypoint_maker
{
// Constructor
WaypointLoaderNode::WaypointLoaderNode() : private_nh_("~")
{initPubSub();
}// Destructor
WaypointLoaderNode::~WaypointLoaderNode()
{
}void WaypointLoaderNode::initPubSub()
{private_nh_.param<std::string>("multi_lane_csv", multi_lane_csv_, "/tmp/driving_lane.csv");// setup publisherlane_pub_ = nh_.advertise<autoware_msgs::LaneArray>("/based/lane_waypoints_raw", 10, true);
}/*initPubSub函数中对multi_lane_csv_赋值,我们分析时采用默认值"/tmp/ driving.lane.csv"，接着设置对应话题"/based/lane_waypoints_raw"的消息发布者lane_pub_.*///run函数读取存储的轨迹点文件数据，并发布至话题"/based/lane_waypoints_raw"。
void WaypointLoaderNode::run()
{multi_file_path_.clear();parseColumns(multi_lane_csv_, &multi_file_path_);//parseColumns函数以“,”作为分隔符将字符串 line分成若干段，并将其中的空格全部去除,依次储存至字符串向量columns 中。autoware_msgs::LaneArray lane_array;createLaneArray(multi_file_path_, &lane_array);//createLaneArray函数将paths中各个本地路径对应文件中包含的信息分别填入 lane中，再将lane依次填入lane_array。lane_pub_.publish(lane_array);output_lane_array_ = lane_array;ros::spin();
}//createLaneArray函数将paths中各个本地路径对应文件中包含的信息分别填入 lane中，再将lane依次填入lane_array。
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函数进行分析可知，其主要作用是首先检查文件file path中数据是否合规，接着读取文件内的数据存入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;
}//loadWaypointsForVer1函数将filename中每一行数据都分别存入 Waypoint类型数据变量并依次填入 Waypoint向量wps中,
//接着根据相邻两个轨迹点的信息计算航向角。
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 linewhile (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类型后存入wp 中。
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]));// kmph2mps单位转换函数/*1.stoi()将字符串转换为整型2.stoll()将字符串转换为long long3.stof()将字符串转换为float型4.stod()将字符串转换为double型*/
}void WaypointLoaderNode::loadWaypointsForVer2(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 linewhile (std::getline(ifs, line)){autoware_msgs::Waypoint wp;parseWaypointForVer2(line, &wp);wps->emplace_back(wp);}
}void WaypointLoaderNode::parseWaypointForVer2(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->pose.pose.orientation = tf::createQuaternionMsgFromYaw(std::stod(columns[3]));//tf::createQuaternionMsgFromYaw空间坐标变换函数wp->twist.twist.linear.x = kmph2mps(std::stod(columns[4]));
}void WaypointLoaderNode::loadWaypointsForVer3(const char* filename, std::vector<autoware_msgs::Waypoint>* wps)
{std::ifstream ifs(filename);if (!ifs){return;}std::string line;std::getline(ifs, line);  // get first linestd::vector<std::string> contents;parseColumns(line, &contents);// std::getline(ifs, line);  // remove second linewhile (std::getline(ifs, line)){autoware_msgs::Waypoint wp;parseWaypointForVer3(line, contents, &wp);wps->emplace_back(wp);}
}void WaypointLoaderNode::parseWaypointForVer3(const std::string& line, const std::vector<std::string>& contents,autoware_msgs::Waypoint* wp)
{std::vector<std::string> columns;parseColumns(line, &columns);std::unordered_map<std::string, std::string> map;for (size_t i = 0; i < contents.size(); i++){map[contents.at(i)] = columns.at(i);}wp->pose.pose.position.x = std::stod(map["x"]);//stod()将字符串转换为double型wp->pose.pose.position.y = std::stod(map["y"]);wp->pose.pose.position.z = std::stod(map["z"]);wp->pose.pose.orientation = tf::createQuaternionMsgFromYaw(std::stod(map["yaw"]));wp->twist.twist.linear.x = kmph2mps(std::stod(map["velocity"]));wp->change_flag = std::stoi(map["change_flag"]);//stoi()将字符串转换为整型wp->wpstate.steering_state = (map.find("steering_flag") != map.end()) ? std::stoi(map["steering_flag"]) : 0;wp->wpstate.accel_state = (map.find("accel_flag") != map.end()) ? std::stoi(map["accel_flag"]) : 0;wp->wpstate.stop_state = (map.find("stop_flag") != map.end()) ? std::stoi(map["stop_flag"]) : 0;wp->wpstate.event_state = (map.find("event_flag") != map.end()) ? std::stoi(map["event_flag"]) : 0;
}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);//getline函数来读取文件数据输入, 以及如何按行处理文件内容。// 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);
}
/*
首先判断能不能打开filename这个文件，如果不能打开则返回 FileFormat:;unknown。
FileFormat是定义在头文件 waypoint_loader_core.h 中的一种特别的“枚举类”，其中，unknown = -1。
接着分析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。
*///verifyFileConsistency函数的作用是“验证文件一致性”。首先ifstream以输入方式打开文件 filename，如果打开失败那么就直接返回false;如果成功打开此文件，则执行checkFileFormat 函数。
bool WaypointLoaderNode::verifyFileConsistency(const char* filename)
{ROS_INFO("verify...");std::ifstream ifs(filename);if (!ifs){return false;}
/*如果checkFileFormat函数返回的是FileFormat:.unknown，则返回.false。
否则在verifyFileConsistency函数内也借助getline函数读取一次filename文件内的数据，
getline函数具备一个特性就是被调用一次后下次调用就会自动读取下一行。
值得注意的是，getline函数在不同函数内被调用的时候是不会互相影响的。
*/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 linesize_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;
/*verifyFileConsistency函数先调用一次getline 函数赋值ncol，之后再调用getline函数直接读取第二行的数据。
调用两次 getline函数是因为verifyFileConsistency函数内size_t ncol = format = = FileFormat::ver1 ? 4 : format = = FileFormat:ver2 ? 5 :countColumns (line)；
在. checkFileFormat函数返回FileFormat::ver3情形时(也就是存在需要另外解释的数据)需要另外确定一致性检查的标准(每行列数)，
而再次调用getline 函数可以直接从第二行开始判断后续各行数据是否都和第一行的列数一致。
至此，我们知道了verifyFileConsistency函数所谓的“验证文件一致性”的具体含义，其实就是检查每行记录的数据个数(列数)是否一致。
*/}//parseColumns函数以“,”作为分隔符将字符串 line分成若干段，并将其中的空格全部去除,依次储存至字符串向量columns 中。
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(), ' ');//begin 返回首元素的地址，end 返回尾元素的下一个地址。if (res == column.end()){break;}column.erase(res);/* c.erase(elem)删除与elem相等的所有元素，返回被移除的元素个数。c.erase(pos)移除迭代器pos所指位置元素，无返回值。c.erase(beg,end)移除区间[beg,end)所有元素，无返回值。 */}if (!column.empty()){columns->emplace_back(column);}}
}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;
}
/*
C++引入了ostringstream、istringstream、stringstream这三个类，要使用他们创建对象就必须包含<sstream>这个头文件。
istringstream类用于执行C++风格的串流的输入操作。
ostringstream类用于执行C风格的串流的输出操作。
strstream类同时可以支持C风格的串流的输入输出操作。istringstream::istringstream(string str);它的作用是从string对象str中读取字符*/
}  // waypoint_maker

参考书目《Autoware与自动驾驶技术》

（三）学习笔记autoware源码core_planning（waypoint_loader)相关推荐

（二）学习笔记autoware源码core_planning（waypoint_extractor)
#include <ros/ros.h> #include <tf/transform_datatypes.h> #include <autoware_msgs/Lane ...
（一）学习笔记autoware源码core_planning（waypoint_saver)
#include <ros/ros.h> #include <geometry_msgs/PoseStamped.h> #include <visualization_m ...
sheng的学习笔记-Vector源码分析
概述 Vector底层也是数组,跟ArrayList很像(先看下ArrayList,再看Vector会很轻松),ArrayList可参考下文,并且由于效率低,已经被淘汰了,大概瞅瞅得了 sheng的学 ...
PixHawk学习笔记之源码浅析——mc_pos_control.cpp——task_main
注意:基于"Firmware-1.6.0rc1" 献上固件源码分享链接:https://pan.baidu.com/s/1kUPocmF 密码:j55a 自己边学边写的,一定有错, ...
yolov1-v5学习笔记及源码解读
目录深度学习网络分类评价指标原理 yolov1 yolov2 yolov3 yolov4 yolov5 源码解读(v3为例) 深度学习网络分类深度学习经典检测方法通常分为 two-stage ...
狂神说SpringCloud学习笔记(附带源码和笔记)
狂神说Spring Cloud Netflix笔记-01(服务注册与发现) 狂神说Spring Cloud Netflix笔记-02(Eureka集群的搭建 ) 狂神说Spring Cloud Net ...
android源码编译简书,android学习笔记之源码编译
编译环境 1.需要Ubuntu 64bit,建议Ubuntu14.04 64-bit 2.安装openJDK7 $ sudo apt-get update $ sudo apt-get install ...
as工程放到源码编译_方舟编译器学习笔记2 源码编译
根据方舟官方文档编译了方舟编译器的源码,在这里简单谈谈其源码的编译过程: 1.操作系统环境: 64位版本的Ubuntu(官方推荐Ubuntu 16.04).我自己本身就有Ubuntu 16.04的虚拟 ...
dubbo学习笔记一源码编译
前面学习了netty和rocketmq,当然前面的文章还会继续更新,继续往下写 2016 没几天了,我打算写下dubbo 2017 继续深入源码,大家有啥问题都可以一起来讨论源码搭建下载源码同 ...

（三）学习笔记autoware源码core_planning（waypoint_loader)

（三）学习笔记autoware源码core_planning（waypoint_loader)相关推荐

最新文章

热门文章