PCL的fromROSMsg()和toROSMsg()不能正确处理xyz之外其他field的数据长度

      使用PCL的PCL的fromROSMsg()函数将ROS的sensor_msgs::PointCloud2类型数据转换为PCL的pcl::PointCloud<T>类型数据时，假如T只是PointXYZ没问题，假如是PointXYZI，intensity这个field的数据类型是float，但是数据长度就是不对的，数据占8个字节而不是float的4个字节！toROSMsg()将PCL的pcl::PointCloud<T>类型数据转换为ROS的sensor_msgs::PointCloud2类型数据时则对数据类型和长度不作检查，完全照搬PCL里的PointXYZI的数据类型和长度，intensity占8个字节这样的有问题数据，rviz可以正常解释和播放展示，自己开发的软件在读取pcl库调用toROSMsg()转换生成的bag时通常根据intensity是float类型而认为数据长度是4个字节而读取到不正常的intensity数据而不能正常播放展示，这时需要自己实现类似fromROSMsg()和toROSMsg()这样的功能函数。

翻看了一下PCL相关源码，里面fromROSMsg()和toROSMsg()函数相关的代码如下：

/opt/ros/melodic/include/pcl_conversions/pcl_conversions.hnamespace pcl {
...
template<typename T>void toROSMsg(const pcl::PointCloud<T> &pcl_cloud, sensor_msgs::PointCloud2 &cloud){pcl::PCLPointCloud2 pcl_pc2;pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);pcl_conversions::moveFromPCL(pcl_pc2, cloud);}template<typename T>void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<T> &pcl_cloud){pcl::PCLPointCloud2 pcl_pc2;pcl_conversions::toPCL(cloud, pcl_pc2);pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);}...
}namespace pcl_conversions {...inlinevoid fromPCL(const pcl::PCLPointField &pcl_pf, sensor_msgs::PointField &pf){pf.name = pcl_pf.name;pf.offset = pcl_pf.offset;pf.datatype = pcl_pf.datatype;pf.count = pcl_pf.count;}inlinevoid fromPCL(const std::vector<pcl::PCLPointField> &pcl_pfs, std::vector<sensor_msgs::PointField> &pfs){pfs.resize(pcl_pfs.size());std::vector<pcl::PCLPointField>::const_iterator it = pcl_pfs.begin();int i = 0;for(; it != pcl_pfs.end(); ++it, ++i) {fromPCL(*(it), pfs[i]);}}inlinevoid copyPCLPointCloud2MetaData(const pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2){fromPCL(pcl_pc2.header, pc2.header);pc2.height = pcl_pc2.height;pc2.width = pcl_pc2.width;fromPCL(pcl_pc2.fields, pc2.fields);//printf("pc2.fields.size() %d, ", pc2.fields.size());//printf("pcl_pc2.fields/pc2.fields: offset %d/%d, datatype %d/%d\n", pcl_pc2.fields[3].offset, pc2.fields[3].offset, pcl_pc2.fields[3].datatype, pc2.fields[3].datatype);pc2.is_bigendian = pcl_pc2.is_bigendian;pc2.point_step = pcl_pc2.point_step;pc2.row_step = pcl_pc2.row_step;pc2.is_dense = pcl_pc2.is_dense;}inlinevoid moveFromPCL(pcl::PCLPointCloud2 &pcl_pc2, sensor_msgs::PointCloud2 &pc2){copyPCLPointCloud2MetaData(pcl_pc2, pc2);pc2.data.swap(pcl_pc2.data);}inlinevoid toPCL(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2){copyPointCloud2MetaData(pc2, pcl_pc2);pcl_pc2.data = pc2.data;}inlinevoid copyPointCloud2MetaData(const sensor_msgs::PointCloud2 &pc2, pcl::PCLPointCloud2 &pcl_pc2){toPCL(pc2.header, pcl_pc2.header);pcl_pc2.height = pc2.height;pcl_pc2.width = pc2.width;toPCL(pc2.fields, pcl_pc2.fields);pcl_pc2.is_bigendian = pc2.is_bigendian;pcl_pc2.point_step = pc2.point_step;pcl_pc2.row_step = pc2.row_step;pcl_pc2.is_dense = pc2.is_dense;}inlinevoid toPCL(const sensor_msgs::PointField &pf, pcl::PCLPointField &pcl_pf){pcl_pf.name = pf.name;pcl_pf.offset = pf.offset;pcl_pf.datatype = pf.datatype;pcl_pf.count = pf.count;}inlinevoid toPCL(const std::vector<sensor_msgs::PointField> &pfs, std::vector<pcl::PCLPointField> &pcl_pfs){pcl_pfs.resize(pfs.size());std::vector<sensor_msgs::PointField>::const_iterator it = pfs.begin();int i = 0;for(; it != pfs.end(); ++it, ++i) {toPCL(*(it), pcl_pfs[i]);}}inlinevoid toPCL(const std_msgs::Header &header, pcl::PCLHeader &pcl_header){toPCL(header.stamp, pcl_header.stamp);pcl_header.seq = header.seq;pcl_header.frame_id = header.frame_id;}inlinevoid toPCL(const ros::Time &stamp, std::uint64_t &pcl_stamp){pcl_stamp = stamp.toNSec() / 1000ull;  // Convert from ns to us}...
}/usr/include/pcl-1.8/pcl/conversions.htemplate<typename PointT> voidtoPCLPointCloud2 (const pcl::PointCloud<PointT>& cloud, pcl::PCLPointCloud2& msg){// Ease the user's burden on specifying width/height for unorganized datasetsif (cloud.width == 0 && cloud.height == 0){msg.width  = static_cast<uint32_t>(cloud.points.size ());msg.height = 1;}else{assert (cloud.points.size () == cloud.width * cloud.height);msg.height = cloud.height;msg.width  = cloud.width;}// Fill point cloud binary data (padding and all)size_t data_size = sizeof (PointT) * cloud.points.size ();msg.data.resize (data_size);if (data_size){memcpy(&msg.data[0], &cloud.points[0], data_size);}// Fill fields metadatamsg.fields.clear ();for_each_type<typename traits::fieldList<PointT>::type> (detail::FieldAdder<PointT>(msg.fields));for(auto f: msg.fields) {printf("f.name %s, offset %d, datatype %d\n", f.name.c_str(), f.offset, f.datatype);}msg.header     = cloud.header;msg.point_step = sizeof (PointT);msg.row_step   = static_cast<uint32_t> (sizeof (PointT) * msg.width);msg.is_dense   = cloud.is_dense;/// @todo msg.is_bigendian = ?;}struct FieldAdder{FieldAdder (std::vector<pcl::PCLPointField>& fields) : fields_ (fields) {};template<typename U> void operator() (){pcl::PCLPointField f;f.name = traits::name<PointT, U>::value;f.offset = traits::offset<PointT, U>::value;f.datatype = traits::datatype<PointT, U>::value;f.count = traits::datatype<PointT, U>::size;fields_.push_back (f);}std::vector<pcl::PCLPointField>& fields_;};/usr/include/pcl-1.8/pcl/point_traits.h
// offsettemplate<class PointT, typename Tag>struct offset : offset<typename POD<PointT>::type, Tag>{// Contents of specialization:// static const size_t value;// Avoid infinite compile-time recursionBOOST_MPL_ASSERT_MSG((!boost::is_same<PointT, typename POD<PointT>::type>::value),POINT_TYPE_NOT_PROPERLY_REGISTERED, (PointT&));};/usr/include/pcl-1.8/pcl/register_point_struct.h#define POINT_CLOUD_REGISTER_FIELD_OFFSET(r, name, elem)                \template<> struct offset<name, pcl::fields::BOOST_PP_TUPLE_ELEM(3, 2, elem)> \{                                                                     \static const size_t value = offsetof(name, BOOST_PP_TUPLE_ELEM(3, 1, elem)); \};#define POINT_CLOUD_REGISTER_POINT_STRUCT(name, fseq)               \POINT_CLOUD_REGISTER_POINT_STRUCT_I(name,                         \BOOST_PP_CAT(POINT_CLOUD_REGISTER_POINT_STRUCT_X fseq, 0))      \/usr/include/pcl-1.8/pcl/point_types.h
POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZI,(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
)
POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZI, pcl::_PointXYZI)/usr/include/pcl-1.8/pcl/PCLPointField.h
namespace pcl
{struct PCLPointField{PCLPointField () : name (), offset (0), datatype (0), count (0){}std::string name;pcl::uint32_t offset;pcl::uint8_t datatype;pcl::uint32_t count;enum PointFieldTypes { INT8 = 1,UINT8 = 2,INT16 = 3,UINT16 = 4,INT32 = 5,UINT32 = 6,FLOAT32 = 7,FLOAT64 = 8 };/usr/include/pcl-1.8/pcl/register_point_struct.h

自己实现类似fromROSMsg()和toROSMsg()函数功能的代码如下：

void ROSMsg2PCL(sensor_msgs::PointCloud2& ros_msgs, cloud::Ptr cloud_ptr) {cloud_ptr->header.seq = ros_msgs.header.seq;cloud_ptr->header.frame_id = ros_msgs.header.frame_id;cloud_ptr->header.stamp.fromNSec(ros_msgs->header.stamp / 1000ull); // ns => uscloud_ptr->height = ros_msgs.height;cloud_ptr->width = ros_msgs.width;cloud_ptr->is_dense = ros_msgs.is_dense;int points_num = ros_msgs.height * ros_msgs.width;cloud_ptr->points.resize(points_num * 4 * 4);  // XYZIfloat * pdata = reinterpret_cast<float*>(&ros_msgs.data[0]);for (auto i = 0; i < points_num; ++i) {cloud_ptr->points[i*4 + 0] = pdata[i*4 + 0];cloud_ptr->points[i*4 + 1] = pdata[i*4 + 1];cloud_ptr->points[i*4 + 2] = pdata[i*4 + 2];if (ros_msgs.point_step == 16) // 4+4+4+4cloud_ptr->points[i*4 + 3] = pdata[i*4 + 3];/*有的雷达输出的rosmsg在fields的排放顺序有特殊安排，需要使用POINT_CLOUD_REGISTER_POINT_STRUCT注册特定的struct来读取XYZI，例如:POINT_CLOUD_REGISTER_POINT_STRUCT(OusterPointXYZIRT,(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity))即便是保证了XYZI的排放顺序,intensity可能占了8个字节，前4个字节为0后4个字节才是intensity值，不同雷达处理可能不一样，所以这里的处理只是示例，不同雷达是情况而定*/else if (ros_msgs.point_step == 20) //4+4+4+8cloud_ptr->points[i*4 + 3] = pdata[i*4 + 4];}
}void initRosMsgFields(std::vector<sensor_msgs::PointField>& fields) {fields.resize(4);fields[0].name = "x";fields[0].offset = 0;fields[0].datatype = 7;fields[0].count = 1;fields[1].name = "y";fields[1].offset = 4;fields[1].datatype = 7;fields[1].count = 1;fields[2].name = "z";fields[2].offset = 8;fields[2].datatype = 7;fields[2].count = 1;fields[3].name = "intensity";fields[3].offset = 12;fields[3].datatype = 7;fields[3].count = 1;
}void PCL2ROSMSg(cloud::Ptr cloud_ptr, sensor_msgs::PointCloud2& ros_msgs) {//set header with PCL Cloud header for common use. ros_msgs.header.stamp.fromNSec(cloud_ptr->header.stamp * 1000ull); // us => nsros_msgs.header.seq = cloud_ptr->header.seq;ros_msgs.header.frame_id = cloud_ptr->header.frame_id;initRosMsgFields(ros_msgs.fields);ros_msgs.width = cloud_ptr->width;   // 1ros_msgs.height = cloud_ptr->height; // point_numros_msgs.point_step = ros_msgs.fields.size() * sizeof(float); // 4 * 4ros_msgs.row_step   = static_cast<uint32_t> (ros_msgs.point_step * ros_msgs.width);ros_msgs.is_dense   = cloud_ptr->is_dense;int data_size = cloud_ptr->size() * ros_msgs.point_step;if (data_size > 0) {ros_msgs.data.resize(data_size);//memcpy(&ros_msgs.data[0], &cloud_ptr->points[0], data_size);float * pdata = reinterpret_cast<float*>(&ros_msgs.data[0]);for (int i = 0; i < cloud_ptr->size(); i++ ) {pcl::PointXYZI pt = cloud_ptr->points[i];pdata[i*4 + 0] = pt.x;pdata[i*4 + 1] = pt.y;pdata[i*4 + 2] = pt.z;pdata[i*4+ 3] = pt.intensity;}}
}

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mzph.cn/news/687168.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

LeetCode 第34天 | 860. 柠檬水找零 406. 根据身高重建队列 452. 用最少数量的箭引爆气球

LeetCode 第34天 | 860. 柠檬水找零 406. 根据身高重建队列 452. 用最少数量的箭引爆气球

860. 柠檬水找零 模拟找零钱的过程。 class Solution { public:bool lemonadeChange(vector<int>& bills) {int _5yuan 0;int _10yuan 0;int _20yuan 0;for (int i 0; i<bills.size(); i) {if (bills[i] 5) {_5yuan 1;}else if (bills[i] 10) {_10yuan 1…
阅读更多...
【机器学习笔记】12 聚类

【机器学习笔记】12 聚类

无监督学习概述 监督学习 在一个典型的监督学习中&#xff0c;训练集有标签&#x1d466; &#xff0c;我们的目标是找到能够区分正样本和负样本的决策边界&#xff0c;需要据此拟合一个假设函数。无监督学习 与此不同的是&#xff0c;在无监督学习中&#xff0c;我们的数据没…
阅读更多...
微服务学习Day4

微服务学习Day4

文章目录 初始MQ同步通讯和异步通讯MQ常见技术介绍 RabbitMQ快速入门入门案例 SpringAMQP介绍例子WorkQueue模型exchange交换机消息转换器 初始MQ 同步通讯和异步通讯 MQ常见技术介绍 RabbitMQ快速入门 入门案例 SpringAMQP 介绍 例子 WorkQueue模型 exchange交换机 消息转换…
阅读更多...
【JavaEE】_HTTP请求报头header

【JavaEE】_HTTP请求报头header

目录 1. Host 2. Content-Length与Content-Type 2.1 Content-Length 2.2 Content-Type 3. User-Agent&#xff08;UA&#xff09; 4. Referer 5. Cookie header的整体格式是“键值对”结构&#xff0c;一行是一个键值对&#xff0c;这些键值对都是HTTP定义好的、有特殊含…
阅读更多...
输入捕获模式测频率PWM输入模式（PWMI）测占空比

输入捕获模式测频率PWM输入模式（PWMI）测占空比

一、概念介绍 输出比较&#xff1a; 比较电路输入的CNT、CCR大小关系 &#xff0c;在通道引脚输出高低电平 二、*频率知识、测量方法补充 * N/fc得到标准频率的时长&#xff0c;也就是待测频率的周期 测频法代码实现&#xff1a;修改对射式红外传感器计次&#xff08;上升沿…
阅读更多...
怎样让MCU/SFU视频会议ovmedia 接入GB28281监控视频参会互动

怎样让MCU/SFU视频会议ovmedia 接入GB28281监控视频参会互动

在国内视频应用对GB监控接入是常规操作&#xff0c;很多系统需要接入监控视频交互处理。我们以ovmedia视频会议为例做一个接入互动。 GB28181协议在流媒体系统较为普及&#xff0c;我们以开源SRS系统对接监控端再接入会议&#xff08;也可以用商用GB流平台&#xff0c;操作基本…
阅读更多...
Open CASCADE学习|分割

Open CASCADE学习|分割

目录 1、添加头文件与源文件 GEOMAlgo_Splitter.h GEOMAlgo_Splitter.cpp 2、测试 2.1平面分割立方体 2.2以边分面 2.3以面分面 1、添加头文件与源文件 GEOMAlgo_Splitter.h // Copyright (C) 2007-2019 CEA/DEN, EDF R&D, OPEN CASCADE//// Copyright (C) 2003-2…
阅读更多...
【大厂AI课学习笔记】【2.1 人工智能项目开发规划与目标】（4）数据准备的流程

【大厂AI课学习笔记】【2.1 人工智能项目开发规划与目标】（4）数据准备的流程

今天学习的是数据准备的流程。 我们已经知道&#xff0c;数据准备占了AI项目超过一半甚至79%的时间。 那么数据准备&#xff0c;都做些什么&#xff0c;有哪些流程。 1.数据采集 观测数据人工收集调查问卷线上数据库 2.数据清洗 有缺失的数据有重复的数据有内容错误的数据…
阅读更多...
51单片机编程应用（C语言）：DS1302实时时钟

51单片机编程应用（C语言）：DS1302实时时钟

单片机计时的缺陷&#xff1a; 1.他的精度不高&#xff0c;没有时钟芯片精度高&#xff0c; 2.会占用单片机CPU的时间&#xff0c; 3.单片机的时钟无法掉电继续运行&#xff0c;&#xff08;最大的缺点&#xff09; DS1302芯片内部有备用电池&#xff0c;可以掉电继续计时…
阅读更多...
fusion360 操作总结（不断更新）

fusion360 操作总结（不断更新）

平移缩放旋转快捷键 画布选择Windows 组合键macOS 组合键平移按住鼠标中键按住鼠标中键缩放滚动鼠标中键滚动鼠标中键动态观察旋转按住 Shift 键并按住鼠标中键按住 Shift 键并按住鼠标中键绕点动态观察按住 Shift 键单击并按住鼠标中键按住 Shift 键单击并按住鼠标中键撤消Ct…
阅读更多...
MCU看门狗

MCU看门狗

目录 一、独立看门狗(IWDG) 1、IWDG 主要作用 2、IWDG 主要特性 3、编程控制 4、注意地方 二、窗口看门狗(WWDG) 1、窗口看门狗作用&#xff1a; 2、窗口看门狗产生复位信号有两个条件&#xff1a; 3、WWDG 框图 4、WWDG 将要复位的时间 5、编程控制 一、独立看门…
阅读更多...
STL:优先级队列的实现

STL:优先级队列的实现

STL中优先级队列本质上就是堆。在上一篇博客中讲到过&#xff1a;堆是一种完全二叉树&#xff0c;逻辑结构上看起来像树&#xff0c;但在物理结构中是存储在线性表中。与普通线性表不同的是&#xff0c;堆中数据大小是规律排列的&#xff1a;小堆中每个节点都大于它的父节点&am…
阅读更多...
SpringBoot实战：打造企业资产管理系统

SpringBoot实战：打造企业资产管理系统

✍✍计算机编程指导师 ⭐⭐个人介绍&#xff1a;自己非常喜欢研究技术问题&#xff01;专业做Java、Python、微信小程序、安卓、大数据、爬虫、Golang、大屏等实战项目。 ⛽⛽实战项目&#xff1a;有源码或者技术上的问题欢迎在评论区一起讨论交流&#xff01; ⚡⚡ Java实战 |…
阅读更多...
001kafka源码项目gradle报错UnsupportedClassVersionError-kafka-报错-大数据学习

001kafka源码项目gradle报错UnsupportedClassVersionError-kafka-报错-大数据学习

1 报错提示 java.lang.UnsupportedClassVersionError: org/eclipse/jgit/lib/AnyObjectId has been compiled by a more recent version of the Java Runtime (class file version 55.0), this version of the Java Runtime only recognizes class file versions up to 52.0 如…
阅读更多...
5 scala的函数式编程简介

5 scala的函数式编程简介

与Java一样&#xff0c;Scala 也是使用 Lambda 表达式实现函数式变成的。 1 遍历 除了使用 for 可以对数组、List、Set 进行遍历外&#xff0c;也可以使用 foreach 函数式编程进行遍历&#xff0c;使代码更为简洁。 foreach 的方法签名为&#xff1a; foreach(f: (A) > …
阅读更多...
C# Avalonia 折线图

C# Avalonia 折线图

线图开发在C# Avalonia框架中可以通过多种方式实现。由于Avalonia旨在成为跨平台的UI框架&#xff0c;您可以利用多种库和方法来绘制折线图。以下是一个简单的例子&#xff0c;展示了如何在Avalonia应用程序中创建一个基本的折线图。 首先&#xff0c;您需要在Avalonia项目中包…
阅读更多...
StarRocks表设计——排序键和数据模型

StarRocks表设计——排序键和数据模型

该篇文章介绍StarRocks-2.5.4版本的数据模型相关内容&#xff0c;有误请指出~ 目录 一、数据模型概述 1.1 四种模型 1.2 排序键 1.2.1 概述 1.2.2 分类 1.2.3 注意事项 二、明细模型 2.1 概述 2.2 适用场景 2.3 建表语句及说明 三、聚合模型 3.1 概述 3.2 适用场…
阅读更多...
RPA基础知识学习清单

RPA基础知识学习清单

序号 主题 内容 1 HTML编程基础 HTML基本概念及文档结构 2 HTML标签及表单
阅读更多...
Linux线程(1)--线程的概念 | 线程控制

Linux线程(1)--线程的概念 | 线程控制

目录 前置知识 线程的概念 Linux中对线程的理解 重新定义进程与线程 重谈地址空间 线程的优缺点 线程的优点 线程的缺点 线程异常 线程的用途 Linux线程 VS 进程 线程控制 创建线程 线程等待 线程终止 线程ID的深入理解 前置知识 我们知道一个进程有属于自己的P…
阅读更多...
docker （六）-进阶篇-数据持久化最佳实践MySQL部署

docker （六）-进阶篇-数据持久化最佳实践MySQL部署

容器的数据挂载通常指的是将宿主机&#xff08;虚拟机或物理机&#xff09;上的目录或文件挂载到容器内部 MySQL单节点安装 详情参考docker官网文档 1 创建对应的数据目录、日志目录、配置文件目录(参考二进制安装&#xff0c;需自己建立数据存储目录) mkdir -p /data/mysq…
阅读更多...
最新文章

Copyright @ 2022~2023