/* * livox_driver 大疆雷达的底层驱动 * * 本模块作为单例模式,全局使用. * 负责通过调用livox sdk,与livox的后台进行进行对接. 直接控制大疆的雷达设备. * */ #include "livox_driver.h" Livox_driver::Livox_driver() { m_init_flag = false; } Livox_driver::~Livox_driver() { // 因为退出顺序的原因,析构不再自动调用 Livox_driver_uninit , // 而是在推出前手动调用Livox_driver_uninit Livox_driver_uninit(); } //livox雷达驱动初始化. Error_manager Livox_driver::Livox_driver_init(bool aoto_create_livox_flag) { LOG(INFO) << " ---Livox_driver_init run --- "<< this; if (m_init_flag == false) { //Init是livox_sdk.h里面的初始化函数, 此时会创建livox后台线程 if (!Init()) { Uninit(); LOG(ERROR) << "livox sdk init failed..."; return Error_manager(LIVOX_SKD_INIT_FAILED,MINOR_ERROR,"Livox laser init failed..."); } else { //显示 livox sdk 版本 LivoxSdkVersion _sdkversion; GetLivoxSdkVersion(&_sdkversion); char buf[255] = { 0 }; sprintf(buf, "livox sdk init success. \n Livox SDK version %d.%d.%d .\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch); LOG(INFO) << buf; m_aoto_create_livox_flag = aoto_create_livox_flag; //SetBroadcastCallback 设置广播的回调函数,此时会向局域网发送广播消息. // 收到广播的所有雷达设备都会调用一次livox_device_broadcast_callback, //livox_device_broadcast_callback 函数由livox后台线程来执行. SetBroadcastCallback(livox_device_broadcast_callback); //SetDeviceStateUpdateCallback 设置状态更新的回调函数. //雷达设备建立连接或者断开连接,都会调用 livox_device_change_callback //livox_device_change_callback 函数由livox后台线程来执行. SetDeviceStateUpdateCallback(livox_device_change_callback); //启动设备底层的扫描线程，该线程在单独在后台运行。(该线程由livox_sdk来维护) //只有在start之后.各种回调函数才能生效. if (!Start()) { m_init_flag = false; Uninit(); LOG(ERROR) << "livox sdk start failed..."; return Error_manager(LIVOX_SKD_INIT_FAILED,MINOR_ERROR,"Livox laser Start failed..."); } else { LOG(ERROR) << "livox sdk start success..."; m_init_flag = true; } } } return Error_code::SUCCESS; } //livox雷达驱动反初始化. Error_manager Livox_driver::Livox_driver_uninit() { if ( m_init_flag ) { LOG(INFO) << " ---Livox_driver::Livox_driver_uninit() run --- "<< this; //回收livox_sdk的相关资源.销毁livox_sdk的后台线程 Uninit(); m_init_flag = false; } return Error_code::SUCCESS; } //插入广播码和雷达实例的映射表 Error_manager Livox_driver::Livox_insert_sn_laser(std::string sn, CLivoxLaser* p_livox_laser) { //雷达的sn码一般为15个字符加一个\0 if ( sn.size() != kBroadcastCodeSize-1 ) { return Error_manager(Error_code::LIVOX_DRIVER_SN_ERROR, Error_level::MINOR_ERROR, " Livox_insert_sn_laser error , sn size != 15 "); } //填充雷达设备广播码和雷达实例的映射表.如果重复,直接报错,决不能重复创建雷达. if ( m_sn_laser_map.find(sn) == m_sn_laser_map.end() ) { //没找到就新增一个sn--laser映射 m_sn_laser_map[sn] = p_livox_laser; return Error_code::SUCCESS; } else { return Error_manager(Error_code::LIVOX_DRIVER_SN_REPEAT, Error_level::MINOR_ERROR, " Livox_insert_sn_laser error , sn repeat "); } return Error_code::SUCCESS; } //判断 Livox_driver 模块是否正常, 只有正常之后,才能启动或者停止扫描. bool Livox_driver::is_ready() { return m_init_flag; } //判断是否自动创建雷达,默认为false bool Livox_driver::is_aoto_create_livox() { return m_aoto_create_livox_flag; } //雷达驱动层开始取样. Error_manager Livox_driver::Livox_driver_start_sample(uint8_t handle) { if ( is_ready() ) { LidarStartSampling(handle, livox_start_sample_callback, NULL); return Error_code::SUCCESS; } else { return Error_manager(Error_code::LIVOX_DRIVER_NOT_READY, Error_level::MINOR_ERROR, " Livox_driver is_ready error "); } return Error_code::SUCCESS; } //雷达驱动层停止取样. Error_manager Livox_driver::Livox_driver_stop_sample(uint8_t handle) { if ( is_ready() ) { LidarStopSampling(handle, livox_stop_sample_callback, NULL); return Error_code::SUCCESS; } else { return Error_manager(Error_code::LIVOX_DRIVER_NOT_READY, Error_level::MINOR_ERROR, " Livox_driver is_ready error "); } return Error_code::SUCCESS; } //设备广播的回调函数,在 Livox_driver_init 时使用 SetBroadcastCallback() 进行配置. //局域网内的所有livox雷达在收到广播之后,都会调用本函数. //本函数由livox后台线程来执行. 每个雷达调用一次. void Livox_driver::livox_device_broadcast_callback(const BroadcastDeviceInfo *p_info) { LOG(INFO) << " ---livox_device_broadcast_callback start--- " ; if (p_info == NULL) { return; } //检查设备类型,后续最好确定具体的型号. if ( p_info->dev_type != kDeviceTypeHub ) { //自动创建雷达实例,就是不从外部参数创建,在驱动层链接设备的时候,按照能够连接上的设备自动创建雷达实例.默认为false if ( Livox_driver::get_instance_references().is_aoto_create_livox() ) { //创建雷达啊实例. //为 laser_manager 的 m_laser_vector 分配内存. //以后再写. ; } else//默认从外部导入参数来创建雷达. 之前在CLivoxLaser初始化是,就已经填充了 m_sn_laser_map { //按照输入参数来创建雷达. //校验sn码,将雷达与设备链接起来,注意了,要先校验,再连接 std::string t_sn = p_info->broadcast_code; std::map & t_sn_laser_map = Livox_driver::get_instance_references().m_sn_laser_map; if ( t_sn_laser_map.find(t_sn) != t_sn_laser_map.end() ) { //找到sn了,就连接laser和设备. CLivoxLaser* tp_livox_laser = t_sn_laser_map[t_sn]; uint8_t t_handle = 0; //连接雷达设备,连接成功之后,后台进行会分配一个新的handle,默认0到31 bool result = AddLidarToConnect(p_info->broadcast_code, &t_handle); if (result == kStatusSuccess) { //SetDataCallback 函数会为每个雷达设置数据回调函数, //注意了,livox的后台线程不会立刻调用livox_data_callback, //只有在雷达使用 LidarStartSampling 启动扫描后,当有数据返回时,才会调用livox_data_callback 传回雷达数据. SetDataCallback(t_handle, livox_data_callback, NULL); //此时只是单方面连接,状态仍然是 K_DEVICE_STATE_DISCONNECT //需要在 livox_device_change_callback 雷达设备响应连接之后,并确认连接的结果,才会修改雷达连接状态. tp_livox_laser->set_handle(t_handle); tp_livox_laser->set_device_state( CLivoxLaser::K_DEVICE_STATE_DISCONNECT); //填充雷达句柄和雷达实例的映射表. handle是livox后台线程分配的唯一码.一般不会出错. Livox_driver::get_instance_references().m_handle_laser_map[t_handle] = tp_livox_laser; } else { //连接失败. (一般雷达设备有应答,就会连接成功.) tp_livox_laser->set_handle(-1); tp_livox_laser->set_device_state( CLivoxLaser::K_DEVICE_STATE_FAULT); } } } } } //雷达设备状态改变的回调函数,在 Livox_driver_init 时使用 SetDeviceStateUpdateCallback() 来进行配置 //雷达设备建立连接或者断开连接,都会调用 livox_device_change_callback 来通知我们雷达状态的改变. //本函数由livox后台线程来执行. 每个雷达可能调用多次. void Livox_driver::livox_device_change_callback(const DeviceInfo *p_info, DeviceEvent type) { LOG(INFO) << " ---livox_device_change_callback start--- " ; if (p_info == NULL) { return; } uint8_t t_handle = p_info->handle; //检查handle的有效性 std::map & t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map; if ( t_handle_laser_map.find(t_handle) == t_handle_laser_map.end() ) { return; } else { CLivoxLaser* tp_livox_laser = t_handle_laser_map[t_handle]; //正常情况下,每个雷达连接都会调用两次,分别为 kEventConnect 和 kEventStateChange switch ( type ) { case kEventConnect: //连接成功, 则从断连切换到连接 if ( tp_livox_laser->get_device_state() == CLivoxLaser::K_DEVICE_STATE_DISCONNECT ) { tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_CONNECT); tp_livox_laser->set_device_info(*p_info); //LidarGetExtrinsicParameter 函数会为每个雷达设置 获取欧拉角的回调函数, //获取欧拉角, 并转化为变换矩阵 mp_laser_matrix LidarGetExtrinsicParameter(t_handle, livox_get_extrinsic_parameter_callback,NULL); } break; case kEventDisconnect: //断开连接, 如果不是故障,则切换到断连 if ( tp_livox_laser->get_device_state() != CLivoxLaser::K_DEVICE_STATE_FAULT ) { tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_DISCONNECT); tp_livox_laser->set_device_info(*p_info); } break; case kEventStateChange: //单纯的刷新雷达设备信息 tp_livox_laser->set_device_info(*p_info); break; default: break; } } //注意了:此时并不启动雷达扫描. //只有当接受到扫描任务之后,由 CLivoxLaser::start_scan() 来调用 LidarStartSampling } //获取欧拉角(笛卡尔坐标)的回调函数。在 livox_device_change_callback 连接成功之后使用 LidarGetExtrinsicParameter 来设置回调函数。 //雷达接受到查询指令后,会调用 livox_get_extrinsic_parameter_callback 来返回 LidarGetExtrinsicParameterResponse 的指针. 里面就有 欧拉角(笛卡尔坐标) //本函数由livox后台线程来执行. 每个雷达只需要调用一次. void Livox_driver::livox_get_extrinsic_parameter_callback(uint8_t status, uint8_t handle, LidarGetExtrinsicParameterResponse *response, void *client_data) { LOG(INFO) << " ---livox_get_extrinsic_parameter_callback start--- " ; if ( status == kStatusSuccess ) { // 校验handle的有效性 std::map & t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map; if ( t_handle_laser_map.find(handle) == t_handle_laser_map.end() ) { return; } else { CLivoxLaser *tp_livox_laser = t_handle_laser_map[handle]; double t_pitch = response->pitch; double t_roll = response->roll; double t_yaw = response->yaw; double t_x = response->x; double t_y = response->y; double t_z = response->z; //欧拉角转化为3*4矩阵 t_roll = t_roll * M_PI / 180.0; t_pitch = t_pitch * M_PI / 180.0; t_yaw = t_yaw * M_PI / 180.0; double rotate[12] = { std::cos(t_pitch) * std::cos(t_yaw), std::sin(t_roll) * std::sin(t_pitch) * std::cos(t_yaw) - std::cos(t_roll) * std::sin(t_yaw), std::cos(t_roll) * std::sin(t_pitch) * std::cos(t_yaw) + std::sin(t_roll) * std::sin(t_yaw), t_x, std::cos(t_pitch) * std::sin(t_yaw), std::sin(t_roll) * std::sin(t_pitch) * std::sin(t_yaw) + std::cos(t_roll) * std::cos(t_yaw), std::cos(t_roll) * std::sin(t_pitch) * std::sin(t_yaw) - std::sin(t_roll) * std::cos(t_yaw), t_y, -std::sin(t_pitch), std::sin(t_roll) * std::cos(t_pitch), std::cos(t_roll) * std::cos(t_pitch), t_z }; //设置变换矩阵 tp_livox_laser->set_laser_matrix(rotate, 12); } } } //雷达设备数据返回 的回调函数. 在 livox_device_broadcast_callback 连接成功之后使用 SetDataCallback 来设置回调函数。 //雷达在调用 LidarStartSampling 开始扫描之后,就一直调用 livox_data_callback 来返回数据. //雷达在调用 LidarStopSampling 停止扫描之后,就停止扫描.本函数也会停止调用. //本函数由livox后台线程来执行. 每帧数据都会调用一次. data里面有多个三维点. void Livox_driver::livox_data_callback(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *laser) { //livox_data_callback函数会高频率的调用,为了提高2效率,这里就不验证handle的有效性了,这就需要保证前面的正确操作 std::map &t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map; CLivoxLaser *tp_livox_laser = t_handle_laser_map[handle]; if (data && tp_livox_laser) { //判断是否采集完成 if (tp_livox_laser->is_scan_complete()) { //按照指定的帧数来采集,到点就停止. tp_livox_laser->stop_scan(); //注注注注注意了:stop_scan会调用 LidarStopSampling 通知livox后台线程停止扫描. // 但是这个是有延迟的.会导致数据回调函数 livox_data_callback 仍然会调用3~5次. // 因此这里会反复调用 stop_scan return; } //data实际就是一个数据指针,里面包含data_num个三维点. data_num默认为100个 LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data; //把雷达数据填充到livox数据缓存,此时里面是没有解析的原始数据. data_num默认为100个 Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxRawPoint)); tp_livox_laser->push_livox_data(data_bin); tp_livox_laser->add_livox_frame(1); } } //雷达设备启动扫描的回调函数, 在 CLivoxLaser::start_scan() 需要启动扫描的时候使用 LidarStartSampling 来设置回调函数。 //调用 LidarStartSampling 之后,livox后台进程就直接开始扫描了. 之后就会一直调用 livox_data_callback 来返回数据 void Livox_driver::livox_start_sample_callback(uint8_t status, uint8_t handle, uint8_t response, void *data) { LOG(INFO) << " ---livox_start_sample_callback start--- " ; // 校验handle的有效性 std::map &t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map; if (t_handle_laser_map.find(handle) == t_handle_laser_map.end()) { return; } else { CLivoxLaser *tp_livox_laser = t_handle_laser_map[handle]; if (status == kStatusSuccess) { if (response != 0) { tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_CONNECT); } else { //返回成功并且应答没有报错,这里将雷达设备状态改为扫描中. tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_SAMPLING); } } else if (status == kStatusTimeout) { tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_CONNECT); } else { tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_FAULT); } } } //雷达设备启动扫描的回调函数, 在 CLivoxLaser::stop_scan() 或者其他位置 需要停止的时候使用 LidarStopSampling 来设置回调函数。 //调用 LidarStopSampling 之后,livox后台进程就直接停止扫描了. 也会停止调用 livox_data_callback void Livox_driver::livox_stop_sample_callback(uint8_t status, uint8_t handle, uint8_t response, void *data) { LOG(INFO) << " ---livox_stop_sample_callback start--- " ; // 校验handle的有效性 std::map &t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map; if (t_handle_laser_map.find(handle) == t_handle_laser_map.end()) { return; } else { CLivoxLaser *tp_livox_laser = t_handle_laser_map[handle]; if (status == kStatusSuccess || status == kStatusTimeout) { tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_CONNECT); } else { tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_FAULT); } } }