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

最新推荐文章于 2025-03-09 01:04:02 发布

dzjoke

最新推荐文章于 2025-03-09 01:04:02 发布

阅读量2.4k

点赞数 2

分类专栏： ROS

本文链接：https://blog.youkuaiyun.com/dzjoke/article/details/119576269

版权

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 (</

最低0.47元/天解锁文章