ros sensor_msgs::laserscan 数据格式及velodyne_laserscan.cpp文件解析

2024-01-18 00:20
文章标签 解析 数据格式 ros sensor cpp velodyne msgs laserscan

本文主要是介绍ros sensor_msgs::laserscan 数据格式及velodyne_laserscan.cpp文件解析，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

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.cpp为例，说明如何发布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,
// 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 <sensor_msgs/point_cloud2_iterator.h>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_ = nh.advertise<sensor_msgs::LaserScan>("scan", 10, connect_cb, connect_cb);//声明发布的话题srv_.setCallback(boost::bind(&VelodyneLaserScan::reconfig, this, _1, _2));//动态配置参数的回调函数，当设置的参数改变时回调调用}void VelodyneLaserScan::connectCb()
{boost::lock_guard<boost::mutex> lock(connect_mutex_);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->fields.size(); 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<uint16_t> it(*msg, "ring"); it != it.end(); ++it){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 sensorif (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->fields.size(); 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))/resolutionconst 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->ranges.resize(SIZE, INFINITY);// 标准的点云数据中各个数据的序号是有规律的，否则是不规则的数据// 然而两者处理方式一样的，此处区分意义不大	if ((offset_x == 0) &&(offset_y == 4) &&(offset_i % 4 == 0) &&(offset_r % 4 == 0)){scan->intensities.resize(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<float> it(*msg, "x"); it != it.end(); ++it){const uint16_t r = *((const uint16_t*)(&it[R]));  // ring// 对比laser id 是否与需要取的一致，如果不一致，则不更新range[]以及intensities[]if (r == ring){const float x = it[X];  // xconst float y = it[Y];  // yconst 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<float>(M_PI)) / RESOLUTION;if ((bin >= 0) && (bin < static_cast<int>(SIZE))){//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->intensities.resize(SIZE);sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_r(*msg, "ring");sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");sensor_msgs::PointCloud2ConstIterator<float> iter_i(*msg, "intensity");for ( ; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r, ++iter_i){const uint16_t r = *iter_r;  // ringif (r == ring){const float x = *iter_x;  // xconst float y = *iter_y;  // yconst float i = *iter_i;  // intensity            const int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;if ((bin >= 0) && (bin < static_cast<int>(SIZE))){scan->ranges[bin] = sqrtf(x * x + y * y);scan->intensities[bin] = i;}}}}else{sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_r(*msg, "ring");sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");for (; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r){const uint16_t r = *iter_r;  // ringif (r == ring){const float x = *iter_x;  // xconst float y = *iter_y;  // yconst int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;if ((bin >= 0) && (bin < static_cast<int>(SIZE))){scan->ranges[bin] = sqrtf(x * x + y * y);}}}}}//发布 消息pub_.publish(scan);}else{ROS_ERROR("VelodyneLaserScan: PointCloud2 missing one or more required fields! (x,y,ring)");}
}//动态参数回调，设置ring 以及revolution
void VelodyneLaserScan::reconfig(VelodyneLaserScanConfig& config, uint32_t level)
{cfg_ = config;
}}  // namespace velodyne_laserscan

这篇关于ros sensor_msgs::laserscan 数据格式及velodyne_laserscan.cpp文件解析的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/617618。

相关文章

网页解析 lxml 库--实战

网页解析 lxml 库--实战

lxml库使用流程 lxml 是 Python 的第三方解析库，完全使用 Python 语言编写，它对 XPath表达式提供了良好的支 持，因此能够了高效地解析 HTML/XML 文档。本节讲解如何通过 lxml 库解析 HTML 文档。 pip install lxml lxm| 库提供了一个 etree 模块，该模块专门用来解析 HTML/XML 文档，下面来介绍一下 lxml 库
阅读更多...
【C++】_list常用方法解析及模拟实现

【C++】_list常用方法解析及模拟实现

相信自己的力量，只要对自己始终保持信心，尽自己最大努力去完成任何事，就算事情最终结果是失败了，努力了也不留遗憾。💓💓💓 目录   ✨说在前面 🍋知识点一：什么是list？ •🌰1.list的定义 •🌰2.list的基本特性 •🌰3.常用接口介绍 🍋知识点二：list常用接口 •🌰1.默认成员函数 🔥构造函数（⭐） 🔥析构函数 •🌰2.list对象
阅读更多...
OWASP十大安全漏洞解析

OWASP十大安全漏洞解析

OWASP（开放式Web应用程序安全项目）发布的“十大安全漏洞”列表是Web应用程序安全领域的权威指南，它总结了Web应用程序中最常见、最危险的安全隐患。以下是对OWASP十大安全漏洞的详细解析： 1. 注入漏洞（Injection） 描述：攻击者通过在应用程序的输入数据中插入恶意代码，从而控制应用程序的行为。常见的注入类型包括SQL注入、OS命令注入、LDAP注入等。 影响：可能导致数据泄
阅读更多...
从状态管理到性能优化：全面解析 Android Compose

从状态管理到性能优化：全面解析 Android Compose

文章目录 引言一、Android Compose基本概念1.1 什么是Android Compose?1.2 Compose的优势1.3 如何在项目中使用Compose 二、Compose中的状态管理2.1 状态管理的重要性2.2 Compose中的状态和数据流2.3 使用State和MutableState处理状态2.4 通过ViewModel进行状态管理 三、Compose中的列表和滚动
阅读更多...
Spring 源码解读：自定义实现Bean定义的注册与解析

Spring 源码解读：自定义实现Bean定义的注册与解析

引言 在Spring框架中，Bean的注册与解析是整个依赖注入流程的核心步骤。通过Bean定义，Spring容器知道如何创建、配置和管理每个Bean实例。本篇文章将通过实现一个简化版的Bean定义注册与解析机制，帮助你理解Spring框架背后的设计逻辑。我们还将对比Spring中的BeanDefinition和BeanDefinitionRegistry，以全面掌握Bean注册和解析的核心原理。
阅读更多...
CSP 2023 提高级第一轮 CSP-S 2023初试题 完善程序第二题解析 未完

CSP 2023 提高级第一轮 CSP-S 2023初试题 完善程序第二题解析 未完

一、题目阅读 （最大值之和）给定整数序列 a0,⋯,an−1，求该序列所有非空连续子序列的最大值之和。上述参数满足 1≤n≤105 和 1≤ai≤108。 一个序列的非空连续子序列可以用两个下标 ll 和 rr（其中0≤l≤r<n0≤l≤r<n）表示，对应的序列为 al,al+1,⋯,ar​。两个非空连续子序列不同，当且仅当下标不同。 例如，当原序列为 [1,2,1,2] 时，要计算子序列 [
阅读更多...
多线程解析报表

多线程解析报表

假如有这样一个需求，当我们需要解析一个Excel里多个sheet的数据时，可以考虑使用多线程，每个线程解析一个sheet里的数据，等到所有的sheet都解析完之后，程序需要提示解析完成。 Way1 join import java.time.LocalTime;public class Main {public static void main(String[] args) thro
阅读更多...
ZooKeeper 中的 Curator 框架解析

ZooKeeper 中的 Curator 框架解析

Apache ZooKeeper 是一个为分布式应用提供一致性服务的软件。它提供了诸如配置管理、分布式同步、组服务等功能。在使用 ZooKeeper 时，Curator 是一个非常流行的客户端库，它简化了 ZooKeeper 的使用，提供了高级的抽象和丰富的工具。本文将详细介绍 Curator 框架，包括它的设计哲学、核心组件以及如何使用 Curator 来简化 ZooKeeper 的操作。 1
阅读更多...
Unity3D自带Mouse Look鼠标视角代码解析。

Unity3D自带Mouse Look鼠标视角代码解析。

Unity3D自带Mouse Look鼠标视角代码解析。 代码块 代码块语法遵循标准markdown代码，例如： using UnityEngine;using System.Collections;/// MouseLook rotates the transform based on the mouse delta./// Minimum and Maximum values can
阅读更多...
图解TCP三次握手｜深度解析｜为什么是三次

图解TCP三次握手｜深度解析｜为什么是三次

写在前面 这篇文章我们来讲解析 TCP三次握手。 TCP 报文段 传输控制块TCB：存储了每一个连接中的一些重要信息。比如TCP连接表，指向发送和接收缓冲的指针，指向重传队列的指针，当前的发送和接收序列等等。 我们再来看一下TCP报文段的组成结构 TCP 三次握手 过程 假设有一台客户端，B有一台服务器。最初两端的TCP进程都是处于CLOSED关闭状态，客户端A打开链接，服务器端
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2