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

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <autoware_msgs/LaneArray.h>
#include <iostream>
#include <fstream>
#include <vector>
/*
节点waypoint_extractor的主要作用:在其被关闭时将lane_navi节点规划出来的路径集合中的所有轨迹点数据按照路径分别存储在对应的本地文件内。
*/
namespace waypoint_maker
{
// Constructor
class WaypointExtractor
{
private:                               /*首先设置变量，注意lane_csv_默认值为"/tmp/driving _lane.csv"，后面会用到。接着订阅话题lane_topic_。*/ros::NodeHandle nh_, private_nh_;ros::Subscriber larray_sub_;std::string lane_csv_, lane_topic_;autoware_msgs::LaneArray lane_;       /*自定义信息格式*/
public:WaypointExtractor() : private_nh_("~"){init();}// Destructor~WaypointExtractor()        /*节点 waypoint_extractor运行停止时，调用 WaypointExtractor的析构函数～WaypointExtractor()，析构函数中调用deinit函数。*/{deinit();}double mps2kmph(double velocity_mps){return (velocity_mps * 60 * 60) / 1000;/*将m/s换成km/h*/}const std::string addFileSuffix(std::string file_path, std::string suffix){std::string output_file_path, tmp;std::string directory_path, filename, extension;tmp = file_path;const std::string::size_type idx_slash(tmp.find_last_of("/"));if (idx_slash != std::string::npos)//std::string::npos是一个常数，它等于size_type类型可以表示的最大值，用来表示一个不存在的位置{tmp.erase(0, idx_slash);}const std::string::size_type idx_dot(tmp.find_last_of("."));const std::string::size_type idx_dot_allpath(file_path.find_last_of("."));if (idx_dot != std::string::npos && idx_dot != tmp.size() - 1){file_path.erase(idx_dot_allpath, file_path.size() - 1);}file_path += suffix + ".csv";return file_path;}/* 其中const std::string::size_type idx_slash(tmp.find_last_of("/"))将字符串tmp中最后一个“/”的下标值赋值给idx_slash，之后通过idx_slash != std::string::npos判断字符串tmp中是否存在“/”，其中std::string::npos是一个常数，它等于size_type类型可以表示的最大值，用来表示一个不存在的位置。如果存在“/”，则删除下标“0”开始的连续idx_slash个字符。后面也是差不多的。总的而言，addFileSuffix函数的功能是将重复填充在字符串数组dst_multi_file_path中的元素lane_csv_（默认值为/tmp/driving_lane.csv）加上其在数组中的下标，比如排在数组第二位（下标为1）被更改为/tmp/driving_lane1.csv。*/void init(){private_nh_.param<std::string>("lane_csv", lane_csv_, "/tmp/driving_lane.csv");private_nh_.param<std::string>("lane_topic", lane_topic_, "/lane_waypoints_array");// setup publisherlarray_sub_ = nh_.subscribe(lane_topic_, 1, &WaypointExtractor::LaneArrayCallback, this);}void deinit(){if (lane_.lanes.empty()){return;}std::vector<std::string> dst_multi_file_path(lane_.lanes.size(), lane_csv_);if (lane_.lanes.size() > 1){for (auto& el : dst_multi_file_path){el = addFileSuffix(el, std::to_string(&el - &dst_multi_file_path[0]));}}/*上面for循环中的std::to_string(&el - &dst_multi_file_path[0])得到的是el在字符串向量dst_multi_file_path中的下标。修饰el的 auto关键字可以在声明变量的时候根据变量初始值的类型自动为此变量选择匹配的类型。通过图4-7可以验证，可知&el得到el的地址，to_string(&el-&dst_multi_file_path[0])得到el在字符串向量dst_multi_ filc_path中的下标(转换为字符串)。通过上面的分析可知，循环调用函数addFileSuffix时往其中传入字符串向量dst_multi_file_path 中的字符串和其对应的下标(转换为字符串形式)。*/saveLaneArray(dst_multi_file_path, lane_);}void LaneArrayCallback(const autoware_msgs::LaneArray::ConstPtr& larray)          /*函数LaneArrayCallback中将larray赋值给成员变量lane_.*/{if (larray->lanes.empty()){return;}lane_ = *larray;}void saveLaneArray(const std::vector<std::string>& paths,const autoware_msgs::LaneArray& lane_array){for (const auto& file_path : paths){const unsigned long idx = &file_path - &paths[0];std::ofstream ofs(file_path.c_str());ofs << "x,y,z,yaw,velocity,change_flag,steering_flag,accel_flag,stop_flag,event_flag" << std::endl;for (const auto& el : lane_array.lanes[idx].waypoints){const geometry_msgs::Point p = el.pose.pose.position;const double yaw = tf::getYaw(el.pose.pose.orientation);const double vel = mps2kmph(el.twist.twist.linear.x);const int states[] ={el.change_flag, el.wpstate.steering_state, el.wpstate.accel_state,el.wpstate.stop_state, el.wpstate.event_state};ofs << std::fixed << std::setprecision(4);ofs << p.x << "," << p.y << "," << p.z << "," << yaw << "," << vel;for (int i = 0; i < 5; ofs << "," << states[i++]){}ofs << std::endl;}}}/*将lane_array中不同的 lane中的轨迹点数据存到对应的.csv后缀文件中，如将lane_array.lanes[1].waypoints中所有轨迹点的"x,y,z,yaw,velocity,change_flag,steering_flag,accel_flag,stop_flag,event_flag"全都存到/tmp/driving_lane1.csv中。*/};}  // waypoint_makerint main(int argc, char **argv)
{ros::init(argc, argv, "waypoint_extractor");waypoint_maker::WaypointExtractor we;ros::spin();return 0;
}

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

（二）学习笔记autoware源码core_planning（waypoint_extractor)相关推荐

（三）学习笔记autoware源码core_planning（waypoint_loader)
1.waypoint_loader_node #include <ros/ros.h>#include "waypoint_loader_core.h"int main ...
（一）学习笔记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 ...
dubbo学习笔记一源码编译
前面学习了netty和rocketmq,当然前面的文章还会继续更新,继续往下写 2016 没几天了,我打算写下dubbo 2017 继续深入源码,大家有啥问题都可以一起来讨论源码搭建下载源码同 ...
Opencv学习笔记 - imread源码解读
一.打开图片流程分析 1.读取图片头,进行解码器的寻找 2.根据参数flags,确定图像通道和是否缩放 3.给解码器指定缩放参数和源 4.使用解码器读取图像的头,确保没有问题,失败则输出错误并返回 5 ...

（二）学习笔记autoware源码core_planning（waypoint_extractor)

（二）学习笔记autoware源码core_planning（waypoint_extractor)相关推荐

最新文章

热门文章