使用ROS订阅节点对VLP16激光雷达数据进行解析

新建ROS工作空间和工具包

新建ROS工作空间

mkdir -p ~/catkin_ws/src 
cd ~/catkin_ws/src
catkin_init_workspace
cd ..
catkin_make

新建ROS工具包

cd ~/catkin_ws/src
catkin_create_pkg velodyne_read std_msgs roscpp rospy
cd ..
catkin_make

在velodyne_read/src新建.cpp文件，在.cpp文件编写代码订阅velodyne_packets节点解析VLP16数据

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <sensor_msgs/LaserScan.h>
#include <velodyne_msgs/VelodyneScan.h>
#include <velodyne_msgs/VelodynePacket.h>
#include <sensor_msgs/point_cloud2_iterator.h>

void chatterCallback(const velodyne_msgs::VelodyneScan::ConstPtr& msg)
{
    //ROS_INFO("I heard: [%X]", msg->packets[0].data[0]);
    //ROS_INFO("I heard: [%X]", msg->packets[0].data[1]);//test
    float angle1=256 * msg->packets[3].data[3] + msg->packets[3].data[2];
    float angle2=angle1 / 100;//calculate angle

    float distance1=256 * msg->packets[3].data[5] + msg->packets[3].data[4];
    float diatance2=distance1 * 2 / 1000;//calculate  angle-15 distance
    
    float distance3=256 * msg->packets[3].data[8] + msg->packets[3].data[7];
    float diatance4=distance3 * 2 / 1000;//calculate  angle1 distance

    float distance5=256 * msg->packets[3].data[11] + msg->packets[3].data[10];
    float diatance6=distance5 * 2 / 1000;//calculate  angle-13 distance

    float distance7=256 * msg->packets[3].data[14] + msg->packets[3].data[13];
    float diatance8=distance7 * 2 / 1000;//calculate  angle-3 distance

    float distance9=256 * msg->packets[3].data[17] + msg->packets[3].data[16];
    float diatance10=distance9 * 2 / 1000;//calculate  angle-11 distance

    float distance11=256 * msg->packets[3].data[20] + msg->packets[3].data[19];
    float diatance12=distance11 * 2 / 1000;//calculate  angle5 distance

    float distance13=256 * msg->packets[3].data[23] + msg->packets[3].data[22];
    float diatance14=distance13 * 2 / 1000;//calculate  angle-9 distance

    float distance15=256 * msg->packets[3].data[26] + msg->packets[3].data[25];
    float diatance16=distance15 * 2 / 1000;//calculate  angle7 distance



    float distance17=256 * msg->packets[3].data[29] + msg->packets[3].data[28];
    float diatance18=distance17 * 2 / 1000;//calculate  angle-7 distance
    
    float distance19=256 * msg->packets[3].data[32] + msg->packets[3].data[31];
    float diatance20=distance19 * 2 / 1000;//calculate  angle9 distance

    float distance21=256 * msg->packets[3].data[35] + msg->packets[3].data[34];
    float diatance22=distance21 * 2 / 1000;//calculate  angle-5 distance

    float distance23=256 * msg->packets[3].data[38] + msg->packets[3].data[37];
    float diatance24=distance23 * 2 / 1000;//calculate  angle11 distance

    float distance25=256 * msg->packets[3].data[41] + msg->packets[3].data[40];
    float diatance26=distance25 * 2 / 1000;//calculate  angle-3 distance

    float distance27=256 * msg->packets[3].data[44] + msg->packets[3].data[43];
    float diatance28=distance27 * 2 / 1000;//calculate  angle13 distance

    float distance29=256 * msg->packets[3].data[47] + msg->packets[3].data[46];
    float diatance30=distance29 * 2 / 1000;//calculate  angle-1 distance

    float distance31=256 * msg->packets[3].data[50] + msg->packets[3].data[49];
    float diatance32=distance31 * 2 / 1000;//calculate  angle15 distance

    ROS_INFO("I heard: [%f %f %f %f %f %f]", angle2, diatance2, diatance4, diatance6, diatance8, diatance10);//output
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "listener");
    ros::NodeHandle n;
    ros::Rate loop_rate(100);
    ros::Subscriber sub = n.subscribe<velodyne_msgs::VelodyneScan>("/velodyne_packets",10,chatterCallback);
    while(1)
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}