【 autoware(node:waypoint_extractor)数据保存到多个文件】

autoware(node:waypoint_extractor)数据保存到多个文件

---

一、node:waypoint_extractor的作用？可以学到？

将lane_navi节点规划出来的路径集合中的所有轨迹点数据按照路径分别保存到对应的文件中
保存一系列轨迹，一条轨迹一个文件，文件名自动排序

二、程序分析

1.主函数

执行WaypointExtractor构造函数

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

2.类

类的方法 即成员函数在类内实现
私有成员变量
ros::NodeHandle nh_, private_nh_; // 句柄
ros::Subscriber larray_sub_; // 订阅
std::string lane_csv_, lane_topic_;
autoware_msgs::LaneArray lane_;

// Constructor
class WaypointExtractor
{
private:
  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()
  {
    deinit();
  }

  double mps2kmph(double velocity_mps)
  {
    return (velocity_mps * 60 * 60) / 1000;
  }

  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)
    {
      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;
  }

  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]));
      }
    }
    saveLaneArray(dst_multi_file_path, lane_);
  }

  void LaneArrayCallback(const autoware_msgs::LaneArray::ConstPtr& larray)
  {
    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;
      }
    }
  }
};

}  // waypoint_maker

3.构造函数

构造函数 初始化 （参数读取、节点订阅）

  WaypointExtractor() : private_nh_("~")
  {
    init();
  }

  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);
  }

4.析构函数

析构函数 节点waypoint_extractor中止时，执行此函数
std::to_string(&el - &dst_multi_file_path[0])得到的是el在字符串向量dst_multi_file_path中的下标。（转化为字符串）

  // Destructor
  ~WaypointExtractor()
  {
    deinit();
  }

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]));
      }
    }
    saveLaneArray(dst_multi_file_path, lane_);
  }

5.addFileSuffix保存文件命名添加下标（下标添加规则）

std::string::npos为常数，的等于size_type类型可以表示的最大值，用来表示一个不存在的位置
功能：将重复填充在字符串数组中的元素lane_csv_（默认值***/temp/driving_lane.csv***）加上其在数组中的下标，排在数组第二位（下标为1）被更改为***/temp/driving_lane1.csv***

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)	// 判断字符串tmp中是否存在“/”
    {
      tmp.erase(0, idx_slash);			// 存在则删除下标“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;
  }

6.saveLaneArray

将Lane_Array中不同的lane中的轨迹点数据存到对应的.csv后缀文件中，如将lane_array.lanes[1].waypoints中所有轨迹点的"x,y,z,yaw,velocity,change_flag,steering_flag,accel_flag,stop_flag,event_flag"全部保存到/temp/driving_lane1.csv*

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;
      }
    }
  }

---