2023年12月13日发(作者：)
rossensor_msgs：：laserscan数据格式及velodyne_lasers。。。
laserscan数据格式如下（摘自wiki):
每个成员根据注
释容易看出表示什么意思，强调一个容易理解错误的地方，ranges[]数组表示雷达旋转时，记录从angle_min到angle_max 角度范围内的
距离数据，数组的大小并不是固定的360个，与激光雷达转速、方向角分辨有关，即以多少角度为间隔采集数据，也就是消息里面的
angle_increment，(angle_max-angle_min) /angle_increment 即是数组的大小，序号从angle_min开始，即angle_min对应的序号为
0，intensities[]与ranges对应，记录每个数据点的强度或者是激光雷达的反射率。
下面通过分析velodyne官方源码中的由pointCloud转为laserscan的文件 velodyne_为例，说明如何发布laserscan消息。
在注释中说明，请依序看完代码。
// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
// All rights reserved.
//
// Software License Agreement (BSD License 2.0)
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following
// disclaimer in the documentation and/or other materials provided
// with the distribution.
// * Neither the name of {copyright_holder} nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#include "velodyne_laserscan/velodyne_laserscan.h"
#include
namespace velodyne_laserscan
{
VelodyneLaserScan::VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv) :
nh_(nh), srv_(nh_priv), ring_count_(0)
{
ros::SubscriberStatusCallback connect_cb = boost::bind(&VelodyneLaserScan::connectCb, this);
//
注册并绑定收到
pointcloud2
消息的回调函数
pub_ = ise
//
声明发布的话题
srv_.setCallback(boost::bind(&VelodyneLaserScan::reconfig, this, _1, _2));
//
动态配置参数的回调函数，当设置的参数改变时回调调用
}
void VelodyneLaserScan::connectCb()
{
boost::lock_guard
if (!pub_.getNumSubscribers())
{
sub_.shutdown();
}
else if (!sub_)
{
sub_ = nh_.subscribe("velodyne_points", 10, &VelodyneLaserScan::recvCallback, this);
}
}
//
收到点云数据消息的回调实现，即转化数据并发布
scan
话题
void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
// Latch ring count
//
一开始
ring_count
为零，有动态参数配置后或者第二次收到数据后，即可正常工作
//
此块逻辑不够清晰
if (!ring_count_)
{
// Check for PointCloud2 field 'ring'
//
检查点去数据中是否有激光
ID
值，如果没有，则此点云数据无效，不处理数据。
bool found = false;
for (size_t i = 0; i < msg->(); i++)
{
if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16)
{
if (msg->fields[i].name == "ring")
{
found = true;
break;
}
}
}
if (!found)
{
ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
return;
}
//
再次检查
ring
的值，感觉没鸟用，废代码
for (sensor_msgs::PointCloud2ConstIterator
{
const uint16_t ring = *it;
if (ring + 1 > ring_count_)
{
ring_count_ = ring + 1;
}
}
if (ring_count_)
{
ROS_INFO("VelodyneLaserScan: Latched ring count of %u", ring_count_);
}
else
{
ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
return;
}
}
// Select ring to use
//
通过配置，获取需要抽取的激光
ID,
即需要取哪一条线（比如
VLP16
中抽第
8
号线）来发布
scan
，
//
并且给
ring_count_
赋值
uint16_t ring;
if ((cfg_.ring < 0) || (cfg_.ring >= ring_count_))
{
// Default to ring closest to being level for each known sensor
if (ring_count_ > 32)
{
ring = 57; // HDL-64E
}
else if (ring_count_ > 16)
{
ring = 23; // HDL-32E
}
else
{
ring = 8; // VLP-16
}
}
else
{
ring = cfg_.ring;
}
//
如果没有配置参数，则按默认的线来运行，
VLP16
的
8
号
laser id
最接近水平
ROS_INFO_ONCE("VelodyneLaserScan: Extracting ring %u", ring);
// Load structure of PointCloud2
//
获取
PointCloud2
点云数据中
x,y,z,
强度
,laserid
的位置偏移，相当于数组索引
//
初始值为
-1,
当获取到后，变为相应位置的正数，判断是否存在
int offset_x = -1;
int offset_y = -1;
int offset_z = -1;
int offset_i = -1;
int offset_r = -1;
for (size_t i = 0; i < msg->(); i++)
{
if (msg->fields[i].datatype == sensor_msgs::PointField::FLOAT32)
{
if (msg->fields[i].name == "x")
{
offset_x = msg->fields[i].offset;
}
else if (msg->fields[i].name == "y")
{
offset_y = msg->fields[i].offset;
}
else if (msg->fields[i].name == "z")
{
offset_z = msg->fields[i].offset;
}
else if (msg->fields[i].name == "intensity")
{
offset_i = msg->fields[i].offset;
}
}
else if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16)
{
if (msg->fields[i].name == "ring")
{
offset_r = msg->fields[i].offset;
}
}
}
// Construct LaserScan message
//
是否存在
x,y,z
值，如果不存在就报错返回，如果存在就初始化
/scan
消息
//
初始化信息就可以看出
/scan
消息的结构组成
if ((offset_x >= 0) && (offset_y >= 0) && (offset_r >= 0))
{
// /scan
数据的精度，即
range[]
数组的大小，
// size
是一圈对应的精度，因为
angle_min,angle_max
在后面已经固定写死了
//
因此
size
是（
pi-(-pi))/resolution
const float RESOLUTION = std::abs(cfg_.resolution);
const size_t SIZE = 2.0 * M_PI / RESOLUTION;
sensor_msgs::LaserScanPtr scan(new sensor_msgs::LaserScan());
scan->header = msg->header;
//
可以看到
angle_increment
的值即为
RESOLUTION
scan->angle_increment = RESOLUTION;
//
从
-pi
到
pi
，程序已经写死了
scan->angle_min = -M_PI;
scan->angle_max = M_PI;
//
截取
0
到
200
米范围的数据，已经超
VLP16
量程了，所有数据都有效
scan->range_min = 0.0;
scan->range_max = 200.0;
scan->time_increment = 0.0;
//
按分辨率推出的，
ranges
的大小在此定义
scan->(SIZE, INFINITY);
//
标准的点云数据中各个数据的序号是有规律的，否则是不规则的数据
//
然而两者处理方式一样的，此处区分意义不大
if ((offset_x == 0) &&
(offset_y == 4) &&
(offset_i % 4 == 0) &&
(offset_r % 4 == 0))
{
scan->(SIZE);
const size_t X = 0;
const size_t Y = 1;
const size_t I = offset_i / 4;
const size_t R = offset_r / 4;
for (sensor_msgs::PointCloud2ConstIterator
{
const uint16_t r = *((const uint16_t*)(&it[R])); // ring
//
对比
laser id
是否与需要取的一致，如果不一致，则不更新
range[]
以及
intensities[]
if (r == ring)
{
const float x = it[X]; // x
const float y = it[Y]; // y
const float i = it[I]; // intensity
//
求某一角度对应的序号，反正切值刚好是从
-pi
到
pi
//
序号从
angle_min
开始升序
,angle_min
为
-pi,
因此实际为：
// bin=(atan2f(y,x)-angle_min)/angle_increment
const int bin = (atan2f(y, x) + static_cast
if ((bin >= 0) && (bin < static_cast
{
//range[]
的值是平面距离
scan->ranges[bin] = sqrtf(x * x + y * y);
scan->intensities[bin] = i;
}
}
}
}
else
{
ROS_WARN_ONCE("VelodyneLaserScan: PointCloud2 fields in unexpected order. Using slower generic method.");
if (offset_i >= 0)
{
scan->(SIZE);
sensor_msgs::PointCloud2ConstIterator
sensor_msgs::PointCloud2ConstIterator
sensor_msgs::PointCloud2ConstIterator
sensor_msgs::PointCloud2ConstIterator
for ( ; iter_r != iter_(); ++iter_x, ++iter_y, ++iter_r, ++iter_i)
{
const uint16_t r = *iter_r; // ring
if (r == ring)
{
const float x = *iter_x; // x
const float y = *iter_y; // y
const float i = *iter_i; // intensity
const int bin = (atan2f(y, x) + static_cast
消息 pub_.publish(scan); } else { ROS_ERROR("VelodyneLaserScan: PointCloud2 missing one or more required fields! (x,y,ring)"); }}//动态参数回调，设置ring
以及revolutionvoid VelodyneLaserScan::reconfig(VelodyneLaserScanConfig& config, uint32_t level){ cfg_ = config;}} // namespace velodyne_laserscan
发布评论