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;
};
typedef LslidarC16Driver::LslidarC16DriverPtr LslidarC16DriverPtr;
typedef LslidarC16Driver::LslidarC16DriverConstPtr LslidarC16DriverConstPtr;
} // namespace lslidar_driver
#endif // _LSLIDAR_C16_DRIVER_H_
#include