Live2D

pcl点云与ros之间的数据类型转换

一、基础数据类型

pcl::PointCloud<pcl::PointXYZ> pcl的点云

pcl::PCLPointCloud2 pcl的第二种点云

sensor_msgs::PointCloud2 ROS中的点云

sensor_msgs::PointCloud ROS中的点云

 二、转换函数

1. sensor_msgs::PCLPointCloud2 <=> pcl::PCLPointCloud2

void pcl_conversions::toPCL(const sensor_msgs::PointCloud2 &, pcl::PCLPointCloud2 &)

void pcl_conversions::moveFromPCL(const pcl::PCLPointCloud2 &, const sensor_msgs::PointCloud2 &);

2. pcl::PCLPointCloud2 <=> pcl::PointCloud<pcl::PointXYZ>

void pcl::fromPCLPointCloud2(const pcl::PCLPointCloud2 &, pcl::PointCloud<pointT> &);

void pcl::toPCLPointCloud2(const pcl::PointCloud<pointT> &, pcl::PCLPointCloud2 &);

3. sensor_msgs::PCLPointCloud2 <=> pcl::PointCloud<pcl::PointXYZ>

void pcl::fromROSMsg(const sensor_msgs::PointCloud2 &, pcl::PointCloud<T> &);

void pcl::toROSMsg(const pcl::PointCloud<T> &, sensor_msgs::PointCloud2 &);

4. sensor_msgs::PointCloud2 <=> sensor_msgs::PointCloud

static bool sensor_msgs::convertPointCloudToPointCloud2(const sensor_msgs::PointCloud & input,sensor_msgs::PointCloud2 & output);

static bool sensor_msgs::convertPointCloud2ToPointCloud(const sensor_msgs::PointCloud2 & input,sensor_msgs::PointCloud & output);

三、实例

        pcl::PointCloud<PointT> mls_points;
        sensor_msgs::PointCloud2 cloud_final;
        pcl::toROSMsg(mls_points,cloud_final);


        std::vector<double> angles_after,ranges_after;
        // Iterate through pointcloud
        for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud_final, "x"),
          iter_y(cloud_final, "y"), iter_z(cloud_final, "z");
          iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
        {

          double range = hypot(*iter_x, *iter_y);
          double angle = atan2(*iter_y, *iter_x);
          angles_after.push_back(angle);
          ranges_after.push_back(range);
        }
        sensor_msgs::LaserScan msg;
        msg.header = scan_msg->header;
        msg.angle_min       = scan_msg->angle_min;
        msg.angle_max       = scan_msg->angle_max;
        msg.angle_increment = scan_msg->angle_increment;
        msg.time_increment  = scan_msg->time_increment; // 0.00002469;
        msg.range_min       = scan_msg->range_min;
        msg.range_max       = scan_msg->range_max;
        msg.ranges.resize(ranges.size());
        for(int i=0;i<msg.ranges.size();i++)
        {
            msg.ranges[i]=ranges[i];
        }

 

posted @ 2020-10-15 11:38  檀木  阅读(1777)  评论(0编辑  收藏  举报
//一下两个链接最好自己保存下来,再上传到自己的博客园的“文件”选项中