ROS中发布GNSS和点云PCD信息

本文主要是介绍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/subros::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 Matrix4fThis 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 Affine3fThis 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 axistransform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));// Print the transformationprintf ("\nMethod #2: using an Affine3f\n");std::cout << transform_2.matrix() << std::endl;// Executing the transformationstd::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 samepcl::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;

这篇关于ROS中发布GNSS和点云PCD信息的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---