ROS中發佈GNSS和點雲PCD信息

1.發佈gnss

ros::Publisher pub = nh.advertise<nmea_msgs::Sentence>("/nmea_sentence", 1);
// Publish all topics with the same ROS time stamp.
ros::Time topic_publish_time = ros::Time::now();

// === NMEA Sentence ===
msg.header.stamp = topic_publish_time;
msg.header.frame_id = "/gps";
// 發佈間隔
ros::WallDuration(0.1).sleep();
msg.sentence = `$GPGGA,063201.60,3016.3898531,N,12004.0198533,E,4,19,0.7,6.795,M,7.038,M,1.6,1792*78;
pub.publish(msg);

發佈後的完整信息爲
header:
seq: 156476
stamp:
secs: 1427157704
nsecs: 536139011
frame_id: “/gps”
sentence: “$GNRMC,004129.40,A,3514.0854393,N,13700.3090060,E,5.9921,147.814,240315,7.320,E,D*10”

2.發佈pcd文件

void loadMap()
{
  std::vector<std::string> pcd_paths;
  string filePath = "/home/rtour/Desktop/data/2019-12-31-14-32-00-886/Lidar/lidar0/";
  vector<string> files;
  //vector<string> filesname;

  //獲取該路徑下的所有文件路徑
  get_filelist_from_dir(filePath, files);
  ros::NodeHandle n;
  pcd_pub = n.advertise<sensor_msgs::PointCloud2>("points_raw", 1, true);
  //遍歷所有路徑
  for (int i = 1; i < files.size(); ++i) {
        string dir = filePath;
        std::string path(dir.append(files[i]));
        pcd_paths.push_back(path);
        sensor_msgs::PointCloud2 pcd;
        if (pcl::io::loadPCDFile(path, pcd) == -1) {
          std::cerr << "load failed " << path << std::endl;
        }
        int err = 0;
        // Give time to set up pub/sub
        ros::WallDuration(0.1).sleep();
        publish_pcd(pcd,&err);
   }
}
void publish_pcd(sensor_msgs::PointCloud2 pcd, const int* errp = NULL)
{
  if (pcd.width != 0) {
    cout<<"pub"<<endl;
    pcd.header.frame_id = "/velodyne";
    ros::Time topic_publish_time = ros::Time::now();
    pcd.header.stamp = topic_publish_time;
    pcd_pub.publish(pcd);

    if (errp == NULL || *errp == 0) {
      //stat_msg.data = true;
      //stat_pub.publish(stat_msg);
    }
  }
}

3.平移和旋轉pcd

string filePath = "/home/rtour/.autoware/lz/xxtest.pcd";
        pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
        if (pcl::io::loadPCDFile (filePath, *source_cloud) < 0)  {
          std::cout << "Error loading point cloud " << filePath << std::endl << std::endl;
          return;
        }

      /* Reminder: how transformation matrices work :
               |-------> This column is the translation
        | 1 0 0 x |  \
        | 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left
        | 0 0 1 z |  /
        | 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)

        METHOD #1: Using a Matrix4f
        This is the "manual" method, perfect to understand but error prone !
      */

      // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
      float theta = -M_PI/3; // The angle of rotation in radians
      /*  METHOD #2: Using a Affine3f
        This method is easier and less error prone
      */
      Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
      // Define a translation 
      transform_2.translation() << 31.935742, 720.038504 ,6.741113;
      // The same rotation matrix as before; theta radians arround Z axis
      transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
      // Print the transformation
      printf ("\nMethod #2: using an Affine3f\n");
      std::cout << transform_2.matrix() << std::endl;
      // Executing the transformation
      std::cout << "transform start" << std::endl;
      pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
      // You can either apply transform_1 or transform_2; they are the same
      pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);

     //保存爲一個新的pcd文件 pcl::io::savePCDFileASCII("/home/rtour/.autoware/lz/jh.pcd", *transformed_cloud);
std::cout << "transform end" << std::endl;