Autoware planning模块学习笔记（一）：记录轨迹点

Autoware planning模块学习笔记（一）：记录轨迹点

自动驾驶中最简单的规划就是循迹，而要想循迹就得先储存轨迹点。具体的操作过程大家可以移步autoware入门教程-使用rosbag数据生成路径点，里面介绍了储存路径点的操作，但是后面的代码运行过程，博主在此为大家解读一下。

点击Motion Planing->waypoint_maker->waypoint_saver后面的app设置保存路径

根据这一操作，查找src/autoware/utilities/runtime_manager/scripts/computing.yaml可知勾选waypoint_saver后运行ROS包waypoint_maker内的waypoint_saver.launch

waypoint_saver.launch如下：

<!-- -->
<launch>

	<arg name="input_type" default="LaneArrayTopic" />
	<arg name="save_finename" default="/tmp/saved_waypoints.csv" />
  <arg name="interval" default="1" />
  <arg name="pose_topic" default="current_pose" />
  <arg name="velocity_topic" default="current_velocity" />
  <arg name="save_velocity" default="true" />
	<arg name="lane_topic" default="/lane_waypoints_array" />

	<node pkg="waypoint_maker" type="waypoint_saver" name="waypoint_saver" output="screen" unless="$(arg input_type)">
		<param name="save_filename" value="$(arg save_finename)" />
		<param name="interval" value="$(arg interval)" />
		<param name="velocity_topic" value="$(arg velocity_topic)" />
		<param name="pose_topic" value="$(arg pose_topic)" />
    <param name="save_velocity" value="$(arg save_velocity)" />
	</node>
	<node pkg="waypoint_maker" type="waypoint_extractor" name="waypoint_extractor" output="screen" if="$(arg input_type)">
		<param name="lane_csv" value="$(arg save_finename)" />
		<param name="lane_topic" value="$(arg lane_topic)" />
	</node>

</launch>

也就是启动waypoint_saver和waypoint_extractor两个节点

由waypoint_maker的CMakeLists.txt可知节点waypoint_saver的源文件是nodes/waypoint_saver/waypoint_saver.cpp，节点waypoint_extractor的源文件是nodes/waypoint_extractor/waypoint_extractor.cpp

节点waypoint_saver

我们先看waypoint_saver.cpp，首先是其main函数

int main(int argc, char **argv)
{
  ros::init(argc, argv, "waypoint_saver");
  WaypointSaver ws;
  ros::spin();
  return 0;
}

节点waypoint_saver启动时新建WaypointSaver对象，查看WaypointSaver唯一的构造函数WaypointSaver()。

根据构造函数可知：新建WaypointSaver对象时首先根据waypoint_saver.launch文件对WaypointSaver中的成员变量进行设置。节点waypoint_saver采用类成员的形式（
message_filters::Subscriber<geometry_msgs::TwistStamped> *twist_sub_;
message_filters::Subscriber<geometry_msgs::PoseStamped> *pose_sub_;
message_filters::Synchronizer *sync_tp_;）时间同步车辆位置和速度，若只记录车辆位置，则只新建pose_sub_并调用回调函数poseCallback;（防盗标记：zhengkunxian）若还需要记录车辆速度，则另外新建twist_sub_订阅速度消息及sync_tp_用于时间同步回调TwistPoseCallback。更多ros消息时间同步与回调的细节参考https://www.cnblogs.com/yebo92/p/5600249.html。最后发布者waypoint_saver_pub_发布标记消息用以在rviz中可视化车辆位置和速度信息。

WaypointSaver::WaypointSaver() : private_nh_("~")
{
  // 根据launch文件设置参数
  private_nh_.param<std::string>("save_filename", filename_, std::string("data.txt"));
  private_nh_.param<std::string>("pose_topic", pose_topic_, std::string("current_pose"));
  private_nh_.param<std::string>("velocity_topic", velocity_topic_, std::string("current_velocity"));
  private_nh_.param<double>("interval", interval_, 1.0);
  private_nh_.param<bool>("save_velocity", save_velocity_, false);

  // 订阅车辆位姿消息;之所以采用message_filters::Subscriber是为了后面使用
  //message_filters::Synchronizer实现ros消息时间同步与回调
  pose_sub_ = new message_filters::Subscriber<geometry_msgs::PoseStamped>(nh_, pose_topic_, 50);

  if (save_velocity_)//如果要保存速度
  {
  //订阅车辆速度消息;
    twist_sub_ = new message_filters::Subscriber<geometry_msgs::TwistStamped>(nh_, velocity_topic_, 50);
    sync_tp_ = new message_filters::Synchronizer<TwistPoseSync>(TwistPoseSync(SYNC_FRAMES), *twist_sub_, *pose_sub_);
    sync_tp_->registerCallback(boost::bind(&WaypointSaver::TwistPoseCallback, this, _1, _2));//ros消息时间同步与回调
  }
  else
  {
    pose_sub_->registerCallback(boost::bind(&WaypointSaver::poseCallback, this, _1));
  }

  // publisher
  waypoint_saver_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("waypoint_saver_marker", 10, true);
}

poseCallback与TwistPoseCallback如下，都调用了outputProcessing，区别只是poseCallback中将速度置为0，而TwistPoseCallback输入了订阅的车辆速度：

void WaypointSaver::poseCallback(const geometry_msgs::PoseStampedConstPtr &pose_msg) const
{
  outputProcessing(pose_msg->pose, 0);
}

void WaypointSaver::TwistPoseCallback(const geometry_msgs::TwistStampedConstPtr &twist_msg,
                                      const geometry_msgs::PoseStampedConstPtr &pose_msg) const
{
  outputProcessing(pose_msg->pose, mps2kmph(twist_msg->twist.linear.x));
}

outputProcessing的作用是车辆每行驶一定距离就向文件filename_内写入车辆位置x，y，z，航向角和车速，同时调用displayMarker，在函数displayMarker中waypoint_saver_pub_发布visualization_msgs::MarkerArray消息用以在rviz中显示车辆位置和速度信息。关于可视化标记的更多信息可以参考Markers: Sending Basic Shapes (C++)

void WaypointSaver::outputProcessing(geometry_msgs::Pose current_pose, double velocity) const
{
  std::ofstream ofs(filename_.c_str(), std::ios::app);//ios::app   在每次写之前找到文件尾
  static geometry_msgs::Pose previous_pose;
  static bool receive_once = false;
  // first subscribe
  if (!receive_once)
  {
    ofs << "x,y,z,yaw,velocity,change_flag" << std::endl;
    ofs << std::fixed << std::setprecision(4) << current_pose.position.x << "," << current_pose.position.y << ","
        << current_pose.position.z << "," << tf::getYaw(current_pose.orientation) << ",0,0" << std::endl;
    receive_once = true;
    displayMarker(current_pose, 0);
    previous_pose = current_pose;
  }
  else
  {
    double distance = sqrt(pow((current_pose.position.x - previous_pose.position.x), 2) +
                           pow((current_pose.position.y - previous_pose.position.y), 2));

    // if car moves [interval] meter
    if (distance > interval_)
    {
      ofs << std::fixed << std::setprecision(4) << current_pose.position.x << "," << current_pose.position.y << ","
          << current_pose.position.z << "," << tf::getYaw(current_pose.orientation) << "," << velocity << ",0" << std::endl;

      displayMarker(current_pose, velocity);
      previous_pose = current_pose;
    }
  }
}

节点waypoint_extractor

分析完成waypoint_saver.cpp后，再分析节点waypoint_extractor的源文件nodes/waypoint_extractor/waypoint_extractor.cpp。同样的，首先是main函数：

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

与节点waypoint_saver类似，节点waypoint_extractor在main函数中建立WaypointExtractor对象。查看其构造函数可知其调用了init函数。

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

查看init函数，首先设置变量，注意lane_csv_默认值为/tmp/driving_lane.csv，后面会用到。接着订阅lane_topic_，继续查看回调函数LaneArrayCallback。

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

函数LaneArrayCallback中将larray赋值给成员变量lane_

  void LaneArrayCallback(const autoware_msgs::LaneArray::ConstPtr& larray)
  {
    if (larray->lanes.empty())
    {
      return;
    }
    lane_ = *larray;
  }

节点waypoint_extractor运行停止时，调用WaypointExtractor的析构函数~WaypointExtractor()，析构函数中调用deinit函数。deinit函数中新建字符串向量dst_multi_file_path，长度为lane_.lanes.size()，以lane_csv_进行填充，接着对其遍历（防盗标记：zhengkunxian），对向量中的每个字符串调用函数addFileSuffix。

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

上面for (auto& el : dst_multi_file_path)循环中的std::to_string(&el - &dst_multi_file_path[0])其实得到的是el在字符串向量dst_multi_file_path中的下标。至于修饰el的auto关键字的作用是可以在声明变量的时候根据变量初始值的类型自动为此变量选择匹配的类型。通过下图可以验证，可知&el得到el的地址，to_string(&el - &dst_multi_file_path[0])就是得到el在字符串向量dst_multi_file_path中的下标（转换为字符串）。

通过上面的分析可知，循环调用函数addFileSuffix时往其中传入字符串向量dst_multi_file_path中的字符串和其对应的下标（转换为字符串形式），而dst_multi_file_path中的字符串都是lane_csv_的值。addFileSuffix代码如下：

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。通过下面的测试可以验证：

以上对字符串数组dst_multi_file_path中的元素重命名的循环操作执行结束后，接着执行saveLaneArray(dst_multi_file_path, lane_)，其中lane_是在回调函数LaneArrayCallback中根据订阅的lane_topic_传来的autoware_msgs::LaneArray格式数据进行赋值更新的（防盗标记：zhengkunxian）。saveLaneArray函数的作用正如其函数名，即将lane_array中不同的lane中的waypoint存到对应的.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中。

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

博主在源码中查找了一下topic“/lane_waypoints_array”，发现是这个topic是在src/autoware/core_planning/lane_planner/nodes/lane_navi/lane_navi.cpp中发布的，也就是车辆自己的决策模块规划出来的路径。

总结一下，整个轨迹点记录的逻辑其实很简单，启动了两个节点：waypoint_saver和waypoint_extractor。其中waypoint_saver节点的功能就是订阅位置，速度的topic，值得注意的是借助了message_filters::Synchronizer对这两种消息进行了时间同步，避免自己后续匹配相同时间速度，位置的麻烦，接着车辆每行驶一定距离就向文件filename_内写入车辆位置x，y，z，航向角和车速，同时发布visualization_msgs::MarkerArray消息用以在rviz中标记车辆位置和显示速度信息；waypoint_extractor的作用是在其被关闭时将lane_navi节点规划出来的路径集合中的每一条路径内的轨迹点的相关信息按照路径分别存储在对应的.csv文件内。