（一）学习笔记autoware源码core_planning（waypoint_saver)

#include 
#include 
#include                 /*rviz中使用MarkerArray绘制地图线*/
#include 
#include                 /*使用message_filters进行多传感器消息同步*/
#include 
#include 
#include 
#include                     /*tf坐标变换*/

#include                                      /*文件指针,fstream类同时支持>>和<<操作符，即能输出输入文件。
ofstream是从内存到硬盘，ifstream是从硬盘到内存；Ifstream类支持>>操作符，ofstream类支持<<操作符；一个输入文件，一个输出文件*/

#include "libwaypoint_follower/libwaypoint_follower.h"

static const int SYNC_FRAMES = 50;

/*
使用关键字 typedef 可以为类型起一个新的别名。typedef 的用法一般为：

typedef  oldName  newName;

oldName 是类型原来的名字，newName 是类型新的名字。例如：

typedef int INTEGER;
INTEGER a, b;
a = 1;
b = 2;

INTEGER a, b;等效于int a, b;。
*/
typedef message_filters::sync_policies::ApproximateTime
    TwistPoseSync;

/*   C++中使用关键字 class 来定义类, 其基本形式如下:
class 类名
{
                                                       public 与 private 为属性/方法限制的关键字
    public:
    //公共的行为或属性            public 表示公开的属性和方法, 外界可以直接访问或者调用
 
    private:
    //公共的行为或属性            private 表示该部分内容是私密的, 不能被外部所访问或调用, 只能被本类内部访问
};
*/

class WaypointSaver   
{
public:
  WaypointSaver();
  ~WaypointSaver();

private:
  // functions

/*
一般情况下，返回类型是void的函数使用return语句是为了引起函数的强制结束，这种return的用法类似于循环结构中的break语句的作用。
示例一：交换两个整型变量数值的函数

#include 

using namespace std; 

void swap(int& a, int&b)
{
    if(a == b)
    {
        return;//若两值相等，无需比较，即让函数停止运行
    }
    int temp;
    temp = a;
    a = b;
    b = temp;
}

int main() 
{
    int a=3, b=4;
    cout<<"交换前a=3, b=4"<交换前a=3, b=4
  交换后a=4, b=3
这个函数首先检查两个值是否相等，如果相等则退出函数；如果不相等，则交换这两个值，隐式的return发生在最后一个赋值语句后。
*/
  void TwistPoseCallback(const geometry_msgs::TwistStampedConstPtr &twist_msg,
                         const geometry_msgs::PoseStampedConstPtr &pose_msg) const;
  void poseCallback(const geometry_msgs::PoseStampedConstPtr &pose_msg) const;
  void displayMarker(geometry_msgs::Pose pose, double velocity) const;
  void outputProcessing(geometry_msgs::Pose current_pose, double velocity) const;

/*
geometry_msgs意思是几何学数据类型。
*/

  // handle
  ros::NodeHandle nh_;
  ros::NodeHandle private_nh_;

  // publisher      发布消息
  ros::Publisher waypoint_saver_pub_;

  // subscriber     订阅消息
  /* 时间同步车辆位置和速度，若只记录车辆位置，则只新建pose_sub_并调用回调函数poseCallback;
  若还需要记录车辆速度，则另外新建twist_sub_订阅速度消息及sync_tp_用于时间同步回调TwistPoseCallback。  */
  message_filters::Subscriber *twist_sub_;
  message_filters::Subscriber *pose_sub_;
  message_filters::Synchronizer *sync_tp_;

  // variables
  bool save_velocity_;      /* 布尔类型      是否保存当前速度   */
  double interval_;         /* double类型   间隔时间   */
  std::string filename_, pose_topic_, velocity_topic_; /* 1.获取文件名。2.位姿话题。3.速度话题。   */
};

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

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

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

  // publisher
  waypoint_saver_pub_ = nh_.advertise("waypoint_saver_marker", 10, true);
}

WaypointSaver::~WaypointSaver()
{
  delete twist_sub_;
  delete pose_sub_;
  delete sync_tp_;
}
/*
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中显示车辆位置和速度信息。
*/
void WaypointSaver::outputProcessing(geometry_msgs::Pose current_pose, double velocity) const
{
  std::ofstream ofs(filename_.c_str(), std::ios::app);/*将订阅的数据写入ofs文件的末尾处*/
  /*
  常见的打开模式:
  ios::in–打开一个可读取文件
  ios::out–打开一个可写入文件
  ios:binary –以二进制的形式打开一个文件。
  ios::app –写入的所有数据将被追加到文件的末尾
  ios::trunk –删除文件原来已存在的内容
  ios::nocreate –如果要打开的文件并不存在，那么以此叁数调用open函数将无法进行。
  ios:noreplece –如果要打开的文件已存在，试图用open函数打开时将返回一个错误。
  */
  static geometry_msgs::Pose previous_pose;
  static bool receive_once = false;
  // first subscribe
  if (!receive_once)/*第一次写入位姿，可以理解汽车起步位置，没有速度，一次运动一般只调用一次。
  是否将当前位姿写入消息数据，因为static bool receive_once = false，所以!receive_once=ture。一般会执行下面语句*/
  {
    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;/*写入x,y,z,yaw*/
    receive_once = true;/*此时改了bool值*/
    displayMarker(current_pose, 0);
    previous_pose = current_pose;
  }
  else/*当汽车运动后，每隔interval距离写入一次数据*/
  {
    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;/*写入x,y,z,yaw,速度*/

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

void WaypointSaver::displayMarker(geometry_msgs::Pose pose, double velocity) const/*下面代码就是rviz可视化的内容*/
{
  static visualization_msgs::MarkerArray marray;
  static int id = 0;

  // initialize marker
  visualization_msgs::Marker marker;
  marker.id = id;
  marker.header.frame_id = "map";
  marker.header.stamp = ros::Time();
  marker.frame_locked = true;

  // create saved waypoint marker
  marker.scale.x = 0.5;/*标记坐标*/
  marker.scale.y = 0.1;
  marker.scale.z = 0.1;
  marker.color.a = 1.0;/*标记颜色rgb（范围0~1）可自己修改喜欢的颜色*/
  marker.color.r = 0.0;
  marker.color.g = 1.0;
  marker.color.b = 0.0;
  marker.ns = "saved_waypoint_arrow";
  marker.type = visualization_msgs::Marker::ARROW;
  marker.action = visualization_msgs::Marker::ADD;
  marker.pose = pose;
  marray.markers.push_back(marker);

  // create saved waypoint velocity text
  marker.scale.z = 0.4;
  marker.color.a = 1.0;
  marker.color.r = 1.0;
  marker.ns = "saved_waypoint_velocity";
  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
  marker.action = visualization_msgs::Marker::ADD;
  std::ostringstream oss;
  oss << std::fixed << std::setprecision(2) << velocity << " km/h";
  marker.text = oss.str();
  marray.markers.push_back(marker);

  waypoint_saver_pub_.publish(marray);
  id++;
}

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