镭神c16驱动代码流程详解

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

1.主函数流程

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

2.初始化驱动driver

2.初始化驱动driver2.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 driverROS_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 filewhile(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 doubleyuanbool 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 variablesstd::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 variablesros::NodeHandle nh;ros::NodeHandle pnh;ros::Publisher packet_pub;    // Diagnostics updaterdiagnostic_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;
};typedef LslidarC16Driver::LslidarC16DriverPtr LslidarC16DriverPtr;
typedef LslidarC16Driver::LslidarC16DriverConstPtr LslidarC16DriverConstPtr;} // namespace lslidar_driver#endif // _LSLIDAR_C16_DRIVER_H_


#include <string>
#include <cmath>
#include <unistd.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <poll.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/file.h>#include <ros/ros.h>
#include <tf/transform_listener.h>#include <lslidar_c16_driver/lslidar_c16_driver.h>//add by lsy
#include <thread>
#include <stdio.h>
#include <termio.h>namespace lslidar_c16_driver {LslidarC16Driver::LslidarC16Driver(ros::NodeHandle& n, ros::NodeHandle& pn):nh(n),pnh(pn),socket_id(-1){return;}LslidarC16Driver::~LslidarC16Driver() {(void) close(socket_id);return;}bool LslidarC16Driver::loadParameters() {//pnh.param("frame_id", frame_id, std::string("lslidar"));pnh.param("lidar_ip", lidar_ip_string, std::string("192.168.1.200"));pnh.param<int>("device_port", UDP_PORT_NUMBER,2368);pnh.param<bool>("add_multicast", add_multicast, false);//add by lsypnh.param<bool>("is_publish", is_publish, true);pnh.param<bool>("is_shutdown", is_shutdown, false);pnh.param("group_ip", group_ip_string, std::string("224.1.1.2"));inet_aton(lidar_ip_string.c_str(), &lidar_ip);ROS_INFO_STREAM("Opening UDP socket: address " << lidar_ip_string);if(add_multicast) ROS_INFO_STREAM("Opening UDP socket: group_address " << group_ip_string);ROS_INFO_STREAM("Opening UDP socket: port " << UDP_PORT_NUMBER);return true;}bool LslidarC16Driver::createRosIO() {// ROS diagnosticsdiagnostics.setHardwareID("Lslidar_C16");// c16 publishs 20*16 thousands points per second.// Each packet contains 12 blocks. And each block// contains 32 points. Together provides the// packet rate.const double diag_freq = 16*20000.0 / (12*32);diag_max_freq = diag_freq;diag_min_freq = diag_freq;ROS_INFO("expected frequency: %.3f (Hz)", diag_freq);using namespace diagnostic_updater;diag_topic.reset(new TopicDiagnostic("lslidar_packets", diagnostics,FrequencyStatusParam(&diag_min_freq, &diag_max_freq, 0.1, 10),TimeStampStatusParam()));// Outputpacket_pub = nh.advertise<lslidar_c16_msgs::LslidarC16Packet>("lslidar_packet", 100);return true;}bool LslidarC16Driver::openUDPPort() {socket_id = socket(PF_INET, SOCK_DGRAM, 0);if (socket_id == -1) {perror("socket");return false;}sockaddr_in my_addr;                     // my address informationmemset(&my_addr, 0, sizeof(my_addr));    // initialize to zerosmy_addr.sin_family = AF_INET;            // host byte ordermy_addr.sin_port = htons(UDP_PORT_NUMBER);      // short, in network byte orderROS_INFO_STREAM("Opening UDP socket: port " << UDP_PORT_NUMBER);my_addr.sin_addr.s_addr = INADDR_ANY;    // automatically fill in my IPif (bind(socket_id, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1) {perror("bind");                 // TODO: ROS_ERROR errnoreturn false;}//add multicastif(add_multicast){ip_mreq groupcast;groupcast.imr_interface.s_addr=INADDR_ANY;groupcast.imr_multiaddr.s_addr=inet_addr(group_ip_string.c_str());if(setsockopt(socket_id,IPPROTO_IP,IP_ADD_MEMBERSHIP,(char*)&groupcast,sizeof(groupcast))<0) {perror("set multicast error");close(socket_id);return false;}}if (fcntl(socket_id, F_SETFL, O_NONBLOCK|FASYNC) < 0) {perror("non-block");return false;}return true;}bool LslidarC16Driver::initialize() {this->initTimeStamp();if (!loadParameters()) {ROS_ERROR("Cannot load all required ROS parameters...");return false;}if (!createRosIO()) {ROS_ERROR("Cannot create all ROS IO...");return false;}if (!openUDPPort()) {ROS_ERROR("Cannot open UDP port...");return false;}ROS_INFO("Initialised lslidar c16 without error");return true;}int LslidarC16Driver::getPacket(lslidar_c16_msgs::LslidarC16PacketPtr& packet) {double time1 = ros::Time::now().toSec();struct pollfd fds[1];fds[0].fd = socket_id;fds[0].events = POLLIN;static const int POLL_TIMEOUT = 2000; // one second (in msec)sockaddr_in sender_address;socklen_t sender_address_len = sizeof(sender_address);while (true){// Unfortunately, the Linux kernel recvfrom() implementation// uses a non-interruptible sleep() when waiting for data,// which would cause this method to hang if the device is not// providing data.  We poll() the device first to make sure// the recvfrom() will not block.//// Note, however, that there is a known Linux kernel bug:////   Under Linux, select() may report a socket file descriptor//   as "ready for reading", while nevertheless a subsequent//   read blocks.  This could for example happen when data has//   arrived but upon examination has wrong checksum and is//   discarded.  There may be other circumstances in which a//   file descriptor is spuriously reported as ready.  Thus it//   may be safer to use O_NONBLOCK on sockets that should not//   block.// poll() until input availabledo {int retval = poll(fds, 1, POLL_TIMEOUT);if (retval < 0)             // poll() error?{if (errno != EINTR)ROS_ERROR("poll() error: %s", strerror(errno));return 1;}if (retval == 0)            // poll() timeout?{ROS_WARN("lslidar poll() timeout");return 1;}if ((fds[0].revents & POLLERR)|| (fds[0].revents & POLLHUP)|| (fds[0].revents & POLLNVAL)) // device error?{ROS_ERROR("poll() reports lslidar error");return 1;}} while ((fds[0].revents & POLLIN) == 0);// Receive packets that should now be available from the// socket using a blocking read.ssize_t nbytes = recvfrom(socket_id, &packet->data[0], PACKET_SIZE,  0,(sockaddr*) &sender_address, &sender_address_len);// ROS_DEBUG_STREAM("incomplete lslidar packet read: "//                 << nbytes << " bytes");if (nbytes < 0){if (errno != EWOULDBLOCK){perror("recvfail");ROS_INFO("recvfail");return 1;}}else if ((size_t) nbytes == PACKET_SIZE){// read successful,// if packet is not from the lidar scanner we selected by IP,// continue otherwise we are doneif( lidar_ip_string != "" && sender_address.sin_addr.s_addr != lidar_ip.s_addr )continue;elsebreak; //done}}this->getFPGA_GPSTimeStamp(packet);// Average the times at which we begin and end reading.  Use that to// estimate when the scan occurred.double time2 = ros::Time::now().toSec();// packet->stamp = ros::Time((time2 + time1) / 2.0);packet->stamp = this->timeStamp;return 0;}void LslidarC16Driver::status(){while (1){int in;struct termios new_settings;struct termios stored_settings;tcgetattr(0,&stored_settings);new_settings = stored_settings;new_settings.c_lflag &= (~ICANON);new_settings.c_cc[VTIME] = 0;tcgetattr(0,&stored_settings);new_settings.c_cc[VMIN] = 1;tcsetattr(0,TCSANOW,&new_settings);in = getchar();tcsetattr(0,TCSANOW,&stored_settings);std::cout << "abcd" << in << std::endl;if(in == 49)//开始发送包{is_publish = true;}else if(in == 50)//停止发送包{is_publish = false;}else if (in == 51)//关闭雷达{is_shutdown = true;return;}}}bool LslidarC16Driver::polling(){// Allocate a new shared pointer for zero-copy sharing with other nodelets.lslidar_c16_msgs::LslidarC16PacketPtr packet(new lslidar_c16_msgs::LslidarC16Packet());// Since the lslidar delivers data at a very high rate, keep// reading and publishing scans as fast as possible.由于激光雷达以很高的速率传输数据，因此请尽可能快地保持读取和发布扫描。//for (int i = 0; i < config_.npackets; ++i)//  {//    while (true)//      {//        // keep reading until full packet received//        int rc = input_->getPacket(&scan->packets[i]);//        if (rc == 0) break;       // got a full packet?//        if (rc < 0) return false; // end of file reached?//      }//  }while (true){// keep reading until full packet receivedint rc = getPacket(packet);if (rc == 0) break;       // got a full packet?if (rc < 0) return false; // end of file reached?if (rc == 1) return false;}if (is_shutdown){return false;}else{if (is_publish){//是否发布包// publish message using time of last packet readROS_DEBUG("Publishing a full lslidar scan.");packet_pub.publish(*packet);// notify diagnostics that a message has been published, updating// its statusdiag_topic->tick(packet->stamp);diagnostics.update();}return true;}}void LslidarC16Driver::initTimeStamp(void){int i;for(i = 0;i < 10;i ++){this->packetTimeStamp[i] = 0;}this->pointcloudTimeStamp = 0;this->timeStamp = ros::Time(0.0);}void LslidarC16Driver::getFPGA_GPSTimeStamp(lslidar_c16_msgs::LslidarC16PacketPtr &packet){unsigned char head2[] = {packet->data[0],packet->data[1],packet->data[2],packet->data[3]};if(head2[0] == 0xA5 && head2[1] == 0xFF){if(head2[2] == 0x00 && head2[3] == 0x5A){this->packetTimeStamp[4] = packet->data[41];this->packetTimeStamp[5] = packet->data[40];this->packetTimeStamp[6] = packet->data[39];this->packetTimeStamp[7] = packet->data[38];this->packetTimeStamp[8] = packet->data[37];this->packetTimeStamp[9] = packet->data[36];cur_time.tm_sec = this->packetTimeStamp[4];cur_time.tm_min = this->packetTimeStamp[5];cur_time.tm_hour = this->packetTimeStamp[6];cur_time.tm_mday = this->packetTimeStamp[7];cur_time.tm_mon = this->packetTimeStamp[8]-1;cur_time.tm_year = this->packetTimeStamp[9]+2000-1900;this->pointcloudTimeStamp = static_cast<uint64_t>(timegm(&cur_time));if (GPSCountingTS != this->pointcloudTimeStamp){cnt_gps_ts = 0;GPSCountingTS = this->pointcloudTimeStamp;}else if (cnt_gps_ts == 3){GPSStableTS = GPSCountingTS;}else{cnt_gps_ts ++;}// ROS_DEBUG("GPS: y:%d m:%d d:%d h:%d m:%d s:%d",//           cur_time.tm_year,cur_time.tm_mon,cur_time.tm_mday,cur_time.tm_hour,cur_time.tm_min,cur_time.tm_sec);}}else if(head2[0] == 0xFF && head2[1] == 0xEE){uint64_t packet_timestamp;packet_timestamp = (packet->data[1200]  +packet->data[1201] * pow(2, 8) +packet->data[1202] * pow(2, 16) +packet->data[1203] * pow(2, 24)) * 1e3;if ((last_FPGA_ts - packet_timestamp) > 0){GPS_ts = GPSStableTS;// ROS_DEBUG("This is step time, using new GPS ts %lu", GPS_ts);}last_FPGA_ts = packet_timestamp;// timeStamp = ros::Time(this->pointcloudTimeStamp+total_us/10e5);timeStamp = ros::Time(GPS_ts, packet_timestamp);// ROS_DEBUG("ROS TS: %f, GPS: y:%d m:%d d:%d h:%d m:%d s:%d; FPGA: us:%lu",//           timeStamp.toSec(), GPS_ts, packet_timestamp);}}} // namespace lslidar_driver

代码链接

代码链接：https://github.com/DoubleYuanL/ROS-driver.git

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

ROS下配置镭神智能c16雷达信息
ROS下配置镭神智能c16雷达信息用户配置写入协议(UCWP) 介绍 3.0 头部 3.1 电机代码详情 3.2 以太网 3.3 时间用户配置写入协议(UCWP) 介绍用户配置写入协议:Use ...
Ubuntu18.04鲁班猫ROS-melodic适配镭神智能C16多线激光雷达
前言本文记录在Ubuntu18.04-melodic中适配镭神智能C16多线雷达的过程,使用最近比较火的国产ROS开发板鲁班猫(LubanCat )进行适配. 准备硬件一套镭神智能C16多线雷 ...
镭神智能C16激光雷达使用中出现的问题（没啥干货）
项目场景: 提示:ubantu16.04+镭神智能C16激光雷达参考文档:https://www.ncnynl.com/archives/201810/2762.html https://blog. ...
镭神智能C16的ROS驱动的安装方法
前言激光雷达赶上了自动驾驶了浪潮,国产激光雷达也越来越多. 最近团队要购买激光雷达,正好拿镭神智能的产品测试一下,安装驱动是首先要做的,因此在这里记录一下. 产品说明:http://www.leis ...
镭神智能C16调试及其适配Autoware1.14
一.将硬件连通,确定数据通讯成功二.下载好传感器的驱动并编译完成 mkdir -p ~/lslidar_ws/src cd ~/lslidar_ws/src git clone https://gi ...
镭神智能N10（老版本，接口2.0）雷达调试
SunnyG按:最近陆续搞起ROS的外设传感器,由于新手第一次摸这些玩意,难免会走一些弯路,这里记录一下步骤,方便下次能直达. 达成:完成雷达在windows.linux.ROS上的调试 ====== ...
镭神智能32线激光雷达调试
1-连接 1.将电源线以及网线接好 2.还有一个HDMI的接口,可以不接 2-设置IP 1. 在有线设置那选择添加 2.以Ipv4格式添加 3.设置好,选择该IP,重启LiDAR 4.打开终端观察数据 ...
镭神C16雷达+免驱动相机联合标定
雷达和相机的单独调试过程参照我之前发的笔记. 接着往下: 全程需要实物连接首先,下载并启动标定工具箱 mkdir -p ~/CL_calibration_ws/src git clone https ...
镭神智能N10激光雷达测评+ROS_Cartographer应用测试
文章目录前言一.雷达基本性能二.工作原理 TOF原理三.连接测试四.在ROS中启动雷达五.将雷达数据用起来(cartographer建图) 总结前言将N10雷达连接ROS主控(本次 ...

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

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

镭神c16驱动代码流程详解

1.主函数流程

2.初始化驱动driver

3.循环使用UDP获取报文

4.所有成员函数截图

代码详情

代码链接

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

最新文章

热门文章