Autoware学习笔记——WaypointSaver节点

Autoware学习笔记——WaypointSaver节点

一、要解决问题

• 学会message_filters中subscriber，Synchronizer的使用

• 学会boost::bind 占位符的使用，弄清楚占位符后无参数的原因

• private_nh_以及WaypointSaver::WaypointSaver() : private_nh_("~")的意思

• 看懂launch文件unless if

二、代码分析

• 头文件部分

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
//为了使用message_filters库中的subscriber
#include <message_filters/subscriber.h>
//为了使用message_filters库中的synchronizer
#include <message_filters/synchronizer.h>
//为了使用synchronizers的approximate_time规则
#include <message_filters/sync_policies/approximate_time.h>
#include <std_msgs/Float32.h>
#include <tf/transform_datatypes.h>
#include <fstream>
#include "libwaypoint_follower/libwaypoint_follower.h"

• main函数

int main(int argc, char **argv)
{
   
  ros::init(argc, argv, "waypoint_saver");
  //实例化了WaypointSaver类
  WaypointSaver ws;
  ros::spin();
  return 0;
}

• 初始化部分

//近似时间规则将subscriber队列长度作为其构造函数的参数，这里的50和下面的pose_sub_以及twist_sub_队列长度相对应
static const int SYNC_FRAMES = 50;
//定义近似时间规则TwistPoseSync，将接收TwistStamped和PoseStamped消息类型的两个subscriber进行同步
typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::TwistStamped, geometry_msgs::PoseStamped>
    TwistPoseSync;

• WaypointSaver类

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

private:
  // functions

  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;

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

  // publisher
  ros::Publisher waypoint_saver_pub_;

  // subscriber
  //定义了一个message_filters::Subscriber<geometry_msgs::TwistStamped>类型的指针
  message_filters::Subscriber<geometry_msgs::TwistStamped> *twist_sub_;
  //定义了一个message_filters::Subscriber<geometry_msgs::PoseStamped> 类型的指针
  message_filters::Subscriber<geometry_msgs::PoseStamped> *pose_sub_;
  //定义了一个message_filters::Synchronizer<TwistPoseSync> 类型的指针
  message_filters::Synchronizer<TwistPoseSync> *sync_tp_;