ROS下打开镭神智能c16雷达以及驱动的理解

镭神c16驱动代码流程详解

代码太多，用图片简单介绍一下镭神c16激光雷达处理过程，所有函数后面代码详情有。

1.主函数流程

主函数流程
1.创建ROS节点
2.初始化驱动driver
3.循环使用UDP获取雷达报文

2.初始化驱动driver

2.初始化驱动driver
	2.1 加载参数:主要是设置雷达ip，驱动端口，组ip，
	2.2 创建ROS输入输出流:c16每秒发布20*16千点。每个数据包包含12个块。每个块包含32个点。一起提供数据包速率。
	2.3 启动UDP通信：连接UDP，和雷达进行通信。（这里还没有获取数据包）

3.循环使用UDP获取报文

3.循环使用UDP获取报文

雷达数据包报文，两个数据，1.时间。2.uint8 类型的数组，大小为1206,这个会在**雷达数据包解析**文章里单独介绍。

使用recvfrom函数从雷达获取数据保存到&packet->data[0]中，然后packet->stamp = this->timeStamp;保存时间，这样就完成了雷达驱动的主要功能，最后发布雷达包到topic上

4.所有成员函数截图

下面是所有成员函数的截图

代码详情

#include <ros/ros.h>
#include <lslidar_c16_driver/lslidar_c16_driver.h>
#include <thread>

int main(int argc, char** argv){
   
    ros::init(argc, argv, "lslidar_c16_driver_node");
    ros::NodeHandle node;
    ros::NodeHandle private_nh("~");

    // start the driver
    ROS_INFO("namespace is %s", private_nh.getNamespace().c_str());
    lslidar_c16_driver::LslidarC16Driver driver(node, private_nh);

    if (!driver.initialize()) {
   
        ROS_ERROR("Cannot initialize lslidar driver...");
        return 0;
    }
    driver.status();

    // loop until shut down or end of file
    while(ros::ok() && driver.polling()) {
   
        ros::spinOnce();
    }
    return 0;
}

#ifndef LSLIDAR_C16_DRIVER_H
#define LSLIDAR_C16_DRIVER_H

#include <unistd.h>
#include <stdio.h>
#include <netinet/in.h>
#include <string>

#include <boost/shared_ptr.hpp>

#include <ros/ros.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>

#include <lslidar_c16_msgs/LslidarC16Packet.h>
#include <lslidar_c16_msgs/LslidarC16ScanUnified.h>

namespace lslidar_c16_driver {
   

//static uint16_t UDP_PORT_NUMBER = 8080;
static uint16_t PACKET_SIZE = 1206;

class LslidarC16Driver {
   
public:

    LslidarC16Driver(ros::NodeHandle& n, ros::NodeHandle& pn);
    ~LslidarC16Driver();
    // add by doubleyuan
    bool is_publish;
    bool is_shutdown;
    void status();

    bool initialize();
    bool polling();

    void initTimeStamp(void);
    void getFPGA_GPSTimeStamp(lslidar_c16_msgs::LslidarC16PacketPtr &packet);

    typedef boost::shared_ptr<LslidarC16Driver> LslidarC16DriverPtr;
    typedef boost::shared_ptr<const LslidarC16Driver> LslidarC16DriverConstPtr;

private:

    bool loadParameters();
    bool createRosIO();
    bool openUDPPort();
    int getPacket(lslidar_c16_msgs::LslidarC16PacketPtr& msg);

    // Ethernet relate variables
    std::string lidar_ip_string;
    std::string group_ip_string;
    in_addr lidar_ip;
    int UDP_PORT_NUMBER;
    int socket_id;
    int cnt_gps_ts;
    bool use_gps_;
	bool add_multicast;
    // ROS related variables
    ros::NodeHandle nh;
    ros::NodeHandle pnh;
    ros::Publisher packet_pub;    

    // Diagnostics updater
    diagnostic_updater::Updater diagnostics;
    boost::shared_ptr<diagnostic_updater::TopicDiagnostic> diag_topic;
    double diag_min_freq;
    double diag_max_freq;

    uint64_t pointcloudTimeStamp;
    uint64_t GPSStableTS;
    uint64_t GPSCountingTS;
    uint64_t last_FPGA_ts;
    uint64_t GPS_ts;
    unsigned char packetTimeStamp[10];
    struct tm cur_time;
    unsigned short int us;
    unsigned short int ms;
    ros::Time timeStamp;
}