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

#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 publisher
    larray_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_maker

int main(int argc, char **argv)
{
  ros::init(argc, argv, "waypoint_extractor");
  waypoint_maker::WaypointExtractor we;
  ros::spin();
  return 0;
}

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