ros学习之topic实例3

最新推荐文章于 2022-07-27 08:41:14 发布

FourierTransformer

最新推荐文章于 2022-07-27 08:41:14 发布

阅读量128

点赞数

分类专栏：点云文章标签：自动驾驶 c++

本文链接：https://blog.csdn.net/qfpkzheng/article/details/120474004

版权

点云专栏收录该内容

9 篇文章 0 订阅

订阅专栏

这次直接上代码了，仍然是发布点云的，但是这次用的是点云的指针,熟悉了一下其用法

>>#include <iostream>                                                                                                                                                                                                                                                                                                                                                      
  #include <boost/thread/thread.hpp>
  #include <pcl/point_types.h>
  #include <pcl_conversions/pcl_conversions.h>
  #include <pcl/point_cloud.h>
  #include <ros/ros.h>
  #include <sensor_msgs/PointCloud2.h>
  #include <pcl/visualization/pcl_visualizer.h>
  #include <pcl/common/common_headers.h>
  #include <pcl/features/normal_3d.h>
  #include <pcl/io/pcd_io.h>
  #include <pcl/console/parse.h>
  #include "opencv2/highgui.hpp"
  using namespace cv;
  using namespace std;
          
          
  int main(int argc, char** argv) {
          
  ros::init(argc, argv, "color");  //初始化了一个节点，名字为color
  ros::NodeHandle nh;
  ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("/color_cloud_ptr", 1000);  //建立了一个发布器，主题是color_cloud_ptr
    //这里的1000代表这个topic发布的队列的长度。
          
  ros::Rate rate(2);  //点云更新频率2Hz
    unsigned int num_points = 1000;  //点云的数据量1000
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
 sensor_msgs::PointCloud2 output_msg;  //建立一个可以直接发布的点云
          
    int j=0;
 while (ros::ok()) {
      //点云发 时的时间chuo
    output_msg.header.stamp = ros::Time::now();
      for (int i = 0; i < num_points; i++) {
        //点云中每个点位于一个10*10*10的方块内随机分布，颜色也随机
        pcl::PointXYZRGB p;
        p.x = 10 * rand () / (RAND_MAX + 1.0f); // rand () / (RAND_MAX + 1.0f)为[0.1)   rand () / (RAND_MAX )为[0.1]              
        p.y = 10 * rand () / (RAND_MAX + 1.0f);
        p.z = 10 * rand () / (RAND_MAX + 1.0f);
        p.r =  (rand() % (255+1)); //(rand() % (b-a+1))+ a
        p.g =  (rand() % (255+1));
        p.b =  (rand() % (255+1));
        cloud_ptr->push_back(p);
      }   
      pcl::toROSMsg(*cloud_ptr, output_msg);  //将点云转化为消息才能发布
      output_msg.header.frame_id = "map";  //frame_id为map，rviz中就不用改了
      pub.publish(output_msg); //发布出去
      rate.sleep();
      j++;
      std::cout <<"------:  "<<j<<endl;
      std:: string filename = "test_pcd_" + std::to_string(j) +".pcd";
      cout<< "file output: " << filename << endl;
      pcl::io::savePCDFileASCII (filename, *cloud_ptr);
          
    }     
    return 0;
  }