怎么看一个网站是谁做的,单位网站建设意见建议,网站制作安全防范方式,广州网站建设网络推广使用PCL的PCL的fromROSMsg()函数将ROS的sensor_msgs::PointCloud2类型数据转换为PCL的pcl::PointCloudT类型数据时#xff0c;假如T只是PointXYZ没问题#xff0c;假如是PointXYZI#xff0c;intensity这个field的数据类型是float#xff0c;但是数据长度就是不对的…      使用PCL的PCL的fromROSMsg()函数将ROS的sensor_msgs::PointCloud2类型数据转换为PCL的pcl::PointCloudT类型数据时假如T只是PointXYZ没问题假如是PointXYZIintensity这个field的数据类型是float但是数据长度就是不对的数据占8个字节而不是float的4个字节toROSMsg()将PCL的pcl::PointCloudT类型数据转换为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 { ... templatetypename Tvoid toROSMsg(const pcl::PointCloudT pcl_cloud, sensor_msgs::PointCloud2 cloud){pcl::PCLPointCloud2 pcl_pc2;pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);pcl_conversions::moveFromPCL(pcl_pc2, cloud);}templatetypename Tvoid fromROSMsg(const sensor_msgs::PointCloud2 cloud, pcl::PointCloudT 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::vectorpcl::PCLPointField pcl_pfs, std::vectorsensor_msgs::PointField pfs){pfs.resize(pcl_pfs.size());std::vectorpcl::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::vectorsensor_msgs::PointField pfs, std::vectorpcl::PCLPointField pcl_pfs){pcl_pfs.resize(pfs.size());std::vectorsensor_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.htemplatetypename PointT voidtoPCLPointCloud2 (const pcl::PointCloudPointT cloud, pcl::PCLPointCloud2 msg){// Ease the users burden on specifying width/height for unorganized datasetsif (cloud.width 0 cloud.height 0){msg.width static_castuint32_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_typetypename traits::fieldListPointT::type (detail::FieldAdderPointT(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_castuint32_t (sizeof (PointT) * msg.width);msg.is_dense cloud.is_dense;/// todo msg.is_bigendian ?;}struct FieldAdder{FieldAdder (std::vectorpcl::PCLPointField fields) : fields_ (fields) {};templatetypename U void operator() (){pcl::PCLPointField f;f.name traits::namePointT, U::value;f.offset traits::offsetPointT, U::value;f.datatype traits::datatypePointT, U::value;f.count traits::datatypePointT, U::size;fields_.push_back (f);}std::vectorpcl::PCLPointField fields_;};/usr/include/pcl-1.8/pcl/point_traits.h // offsettemplateclass PointT, typename Tagstruct offset : offsettypename PODPointT::type, Tag{// Contents of specialization:// static const size_t value;// Avoid infinite compile-time recursionBOOST_MPL_ASSERT_MSG((!boost::is_samePointT, typename PODPointT::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 offsetname, 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_castfloat*(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) // 4444cloud_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) //4448cloud_ptr-points[i*4 3] pdata[i*4 4];} }void initRosMsgFields(std::vectorsensor_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_castuint32_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_castfloat*(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;}} }