Initial commit: Livox SDK2 source (excluding build artifacts)
This commit is contained in:
gardentech
@@ -0,0 +1,431 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#include "build_request.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#include <functional>
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
|
||||
#include "base/logging.h"
|
||||
#include "comm/define.h"
|
||||
|
||||
namespace livox {
|
||||
namespace lidar {
|
||||
|
||||
bool BuildRequest::BuildUpdateViewLidarCfgRequest(const ViewLidarIpInfo& view_lidar_info, uint8_t* req_buf, uint16_t& req_len) {
|
||||
uint16_t key_num = 0;
|
||||
if(view_lidar_info.dev_type == kLivoxLidarTypePA) {
|
||||
key_num = 1;
|
||||
} else {
|
||||
key_num = 2;
|
||||
}
|
||||
|
||||
req_len = 0;
|
||||
memcpy(&req_buf[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * point_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
|
||||
point_kv->key = static_cast<uint16_t>(kKeyLidarPointDataHostIpCfg);
|
||||
point_kv->length = sizeof(uint8_t) * 8;
|
||||
HostIpInfoValue* host_point_ip_info_val = (HostIpInfoValue*)&point_kv->value;
|
||||
if (!InitHostIpAddr(view_lidar_info.host_ip, host_point_ip_info_val)) {
|
||||
LOG_ERROR("Build update view lidar cfg request failed, init host ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
|
||||
memcpy(&(host_point_ip_info_val->host_port), &view_lidar_info.host_point_port, sizeof(view_lidar_info.host_point_port));
|
||||
memcpy(&(host_point_ip_info_val->lidar_port), &view_lidar_info.lidar_point_port, sizeof(view_lidar_info.lidar_point_port));
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue);
|
||||
|
||||
if (view_lidar_info.dev_type == kLivoxLidarTypePA) {
|
||||
return true;
|
||||
}
|
||||
|
||||
LivoxLidarKeyValueParam * imu_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
|
||||
imu_kv->key = static_cast<uint16_t>(kKeyLidarImuHostIpCfg);
|
||||
imu_kv->length = sizeof(uint8_t) * 8;
|
||||
HostIpInfoValue* host_imu_ip_info_val = (HostIpInfoValue*)&imu_kv->value;
|
||||
if (!InitHostIpAddr(view_lidar_info.host_ip, host_imu_ip_info_val)) {
|
||||
LOG_ERROR("Build update view lidar cfg request failed, init imu host ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
|
||||
memcpy(&(host_imu_ip_info_val->host_port), &view_lidar_info.host_imu_data_port, sizeof(view_lidar_info.host_imu_data_port));
|
||||
memcpy(&(host_imu_ip_info_val->lidar_port), &view_lidar_info.lidar_imu_data_port, sizeof(view_lidar_info.lidar_imu_data_port));
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool BuildRequest::BuildUpdateMid360LidarCfgRequest(const LivoxLidarCfg& lidar_cfg,
|
||||
uint8_t* req_buf, uint16_t& req_len) {
|
||||
|
||||
uint16_t key_num = 3;
|
||||
memcpy(&req_buf[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * state_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
|
||||
state_kv->key = static_cast<uint16_t>(kKeyStateInfoHostIpCfg);
|
||||
state_kv->length = sizeof(uint8_t) * 8;
|
||||
HostIpInfoValue* host_state_ip_info_val = (HostIpInfoValue*)&state_kv->value;
|
||||
if (!InitHostIpAddr(lidar_cfg.host_net_info.host_ip, host_state_ip_info_val)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed, init host ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (lidar_cfg.host_net_info.multicast_ip.empty()) {
|
||||
if (!InitHostIpAddr(lidar_cfg.host_net_info.host_ip, host_state_ip_info_val)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed, init host ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
if (!InitMulticastHostIpAddr(lidar_cfg.host_net_info.multicast_ip, host_state_ip_info_val)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed, init pointcloud multicast ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t lidar_state_port = kMid360LidarPushMsgPort;
|
||||
memcpy(&(host_state_ip_info_val->host_port), &lidar_cfg.host_net_info.push_msg_port, sizeof(lidar_cfg.host_net_info.push_msg_port));
|
||||
memcpy(&(host_state_ip_info_val->lidar_port), &lidar_state_port, sizeof(lidar_state_port));
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue);
|
||||
|
||||
LivoxLidarKeyValueParam * point_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
|
||||
point_kv->key = static_cast<uint16_t>(kKeyLidarPointDataHostIpCfg);
|
||||
point_kv->length = sizeof(uint8_t) * 8;
|
||||
HostIpInfoValue* host_point_ip_info_val = (HostIpInfoValue*)&point_kv->value;
|
||||
if (lidar_cfg.host_net_info.multicast_ip.empty()) {
|
||||
if (!InitHostIpAddr(lidar_cfg.host_net_info.host_ip, host_point_ip_info_val)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed, init pointcloud host ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
if (!InitMulticastHostIpAddr(lidar_cfg.host_net_info.multicast_ip, host_point_ip_info_val)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed, init pointcloud multicast ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t lidar_point_port = kMid360LidarPointCloudPort;
|
||||
memcpy(&(host_point_ip_info_val->host_port), &lidar_cfg.host_net_info.point_data_port, sizeof(lidar_cfg.host_net_info.point_data_port));
|
||||
memcpy(&(host_point_ip_info_val->lidar_port), &lidar_point_port, sizeof(lidar_point_port));
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue);
|
||||
|
||||
LivoxLidarKeyValueParam * imu_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
|
||||
imu_kv->key = static_cast<uint16_t>(kKeyLidarImuHostIpCfg);
|
||||
imu_kv->length = sizeof(uint8_t) * 8;
|
||||
HostIpInfoValue* host_imu_ip_info_val = (HostIpInfoValue*)&imu_kv->value;
|
||||
|
||||
if (lidar_cfg.host_net_info.multicast_ip.empty()) {
|
||||
if (!InitHostIpAddr(lidar_cfg.host_net_info.host_ip, host_imu_ip_info_val)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed, init imu host ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
if (!InitMulticastHostIpAddr(lidar_cfg.host_net_info.multicast_ip, host_imu_ip_info_val)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed, init imu multicast ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t lidar_imu_port = kMid360LidarImuDataPort;
|
||||
memcpy(&(host_imu_ip_info_val->host_port), &lidar_cfg.host_net_info.imu_data_port, sizeof(lidar_cfg.host_net_info.imu_data_port));
|
||||
memcpy(&(host_imu_ip_info_val->lidar_port), &lidar_imu_port, sizeof(lidar_imu_port));
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue);
|
||||
// LOG_ERROR("Build imu host ip:{}, host_port:{}, lidar_port:{}", lidar_cfg.host_net_info.imu_data_ip.c_str(),
|
||||
// lidar_cfg.host_net_info.imu_data_port, lidar_imu_port);
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
bool BuildRequest::BuildUpdateLidarCfgRequest(const LivoxLidarCfg& lidar_cfg,
|
||||
uint8_t* req_buf, uint16_t& req_len) {
|
||||
|
||||
uint16_t key_num = 0;
|
||||
if(lidar_cfg.device_type == kLivoxLidarTypePA) {
|
||||
key_num = 1;
|
||||
} else {
|
||||
key_num = 2;
|
||||
}
|
||||
|
||||
memcpy(&req_buf[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * point_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
|
||||
point_kv->key = static_cast<uint16_t>(kKeyLidarPointDataHostIpCfg);
|
||||
point_kv->length = sizeof(uint8_t) * 8;
|
||||
HostIpInfoValue* host_point_ip_info_val = (HostIpInfoValue*)&point_kv->value;
|
||||
if (lidar_cfg.host_net_info.multicast_ip.empty()) {
|
||||
if (!InitHostIpAddr(lidar_cfg.host_net_info.host_ip, host_point_ip_info_val)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed, init pointcloud host ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
if (!InitMulticastHostIpAddr(lidar_cfg.host_net_info.multicast_ip, host_point_ip_info_val)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed, init pointcloud multicast ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t lidar_point_port = 0;
|
||||
if (lidar_cfg.device_type == kLivoxLidarTypeIndustrialHAP) {
|
||||
lidar_point_port = kHAPPointDataPort;
|
||||
} else if (lidar_cfg.device_type == kLivoxLidarTypeMid360) {
|
||||
lidar_point_port = kMid360LidarPointCloudPort;
|
||||
} else if (lidar_cfg.device_type == kLivoxLidarTypePA) {
|
||||
lidar_point_port = kPaLidarPointCloudPort;
|
||||
} else if (lidar_cfg.device_type == kLivoxLidarTypeMid360s) {
|
||||
lidar_point_port = kMid360sLidarPointCloudPort;
|
||||
}
|
||||
else {
|
||||
LOG_ERROR("Build update lidar cfg request failed, unknown the dev_type:{}", lidar_cfg.device_type);
|
||||
return false;
|
||||
}
|
||||
memcpy(&(host_point_ip_info_val->host_port), &lidar_cfg.host_net_info.point_data_port, sizeof(lidar_cfg.host_net_info.point_data_port));
|
||||
memcpy(&(host_point_ip_info_val->lidar_port), &lidar_point_port, sizeof(lidar_point_port));
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue);
|
||||
|
||||
if (lidar_cfg.device_type == kLivoxLidarTypePA) {
|
||||
return true;
|
||||
}
|
||||
|
||||
LivoxLidarKeyValueParam * imu_kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
|
||||
imu_kv->key = static_cast<uint16_t>(kKeyLidarImuHostIpCfg);
|
||||
imu_kv->length = sizeof(uint8_t) * 8;
|
||||
HostIpInfoValue* host_imu_ip_info_val = (HostIpInfoValue*)&imu_kv->value;
|
||||
|
||||
if (lidar_cfg.host_net_info.multicast_ip.empty()) {
|
||||
if (!InitHostIpAddr(lidar_cfg.host_net_info.host_ip, host_imu_ip_info_val)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed, init imu host ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
if (!InitMulticastHostIpAddr(lidar_cfg.host_net_info.multicast_ip, host_imu_ip_info_val)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed, init imu multicast ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t lidar_imu_port = 0;
|
||||
if (lidar_cfg.device_type == kLivoxLidarTypeIndustrialHAP) {
|
||||
lidar_imu_port = kHAPIMUPort;
|
||||
} else if (lidar_cfg.device_type == kLivoxLidarTypeMid360) {
|
||||
lidar_imu_port = kMid360LidarImuDataPort;
|
||||
} else if (lidar_cfg.device_type == kLivoxLidarTypeMid360s) {
|
||||
lidar_imu_port = kMid360sLidarImuDataPort;
|
||||
}
|
||||
else {
|
||||
LOG_ERROR("Build update lidar cfg request failed, unknown the dev_type:{}", lidar_cfg.device_type);
|
||||
return false;
|
||||
}
|
||||
|
||||
memcpy(&(host_imu_ip_info_val->host_port), &lidar_cfg.host_net_info.imu_data_port, sizeof(lidar_cfg.host_net_info.imu_data_port));
|
||||
memcpy(&(host_imu_ip_info_val->lidar_port), &lidar_imu_port, sizeof(lidar_imu_port));
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue);
|
||||
// LOG_ERROR("Build imu host ip:{}, host_port:{}, lidar_port:{}", lidar_cfg.host_net_info.imu_data_ip.c_str(),
|
||||
// lidar_cfg.host_net_info.imu_data_port, lidar_imu_port);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool BuildRequest::BuildSetLidarIPInfoRequest(const LivoxLidarIpInfo& lidar_ip_config, uint8_t* req_buf, uint16_t& req_len) {
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buf[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyLidarIpCfg);
|
||||
kv->length = sizeof(uint8_t) * 12;
|
||||
LivoxLidarIpInfoValue* lidar_ip_val = (LivoxLidarIpInfoValue*)&kv->value;
|
||||
if (!InitLidarIpinfoVal(lidar_ip_config, lidar_ip_val)) {
|
||||
LOG_ERROR("Build set lidar ip info request failed, init lidar ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(LivoxLidarIpInfoValue);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool BuildRequest::BuildSetHostStateInfoIPCfgRequest(const HostStateInfoIpInfo& host_state_info_ipcfg,
|
||||
uint8_t* req_buf, uint16_t& req_len) {
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buf[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyStateInfoHostIpCfg);
|
||||
kv->length = sizeof(uint8_t) * 8;
|
||||
HostIpInfoValue* host_ip_info_val = (HostIpInfoValue*)&kv->value;
|
||||
|
||||
if (!InitHostIpAddr(host_state_info_ipcfg.host_ip_addr, host_ip_info_val)) {
|
||||
LOG_ERROR("Build set host point data ip info request failed, init host ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
|
||||
memcpy(&(host_ip_info_val->host_port), &host_state_info_ipcfg.host_state_info_port, sizeof(host_state_info_ipcfg.host_state_info_port));
|
||||
memcpy(&(host_ip_info_val->lidar_port), &host_state_info_ipcfg.lidar_state_info_port, sizeof(host_state_info_ipcfg.lidar_state_info_port));
|
||||
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool BuildRequest::BuildSetHostPointDataIPInfoRequest(const HostPointIPInfo& host_point_ip_cfg, uint8_t* req_buf, uint16_t& req_len) {
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buf[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyLidarPointDataHostIpCfg);
|
||||
kv->length = sizeof(uint8_t) * 8;
|
||||
HostIpInfoValue* host_ip_info_val = (HostIpInfoValue*)&kv->value;
|
||||
|
||||
if (!InitHostIpAddr(host_point_ip_cfg.host_ip_addr, host_ip_info_val)) {
|
||||
LOG_ERROR("Build set host point data ip info request failed, init host ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
|
||||
memcpy(&(host_ip_info_val->host_port), &host_point_ip_cfg.host_point_data_port, sizeof(host_point_ip_cfg.host_point_data_port));
|
||||
memcpy(&(host_ip_info_val->lidar_port), &host_point_ip_cfg.lidar_point_data_port, sizeof(host_point_ip_cfg.lidar_point_data_port));
|
||||
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool BuildRequest::BuildSetHostImuDataIPInfoRequest(const HostImuDataIPInfo& host_imu_ipcfg, uint8_t* req_buf, uint16_t& req_len) {
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buf[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buf[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyLidarImuHostIpCfg);
|
||||
kv->length = sizeof(uint8_t) * 8;
|
||||
HostIpInfoValue* host_ip_info_val = (HostIpInfoValue*)&kv->value;
|
||||
|
||||
if (!InitHostIpAddr(host_imu_ipcfg.host_ip_addr, host_ip_info_val)) {
|
||||
LOG_ERROR("Build set host imu data ip info request failed, init host ip addr failed.");
|
||||
return false;
|
||||
}
|
||||
|
||||
memcpy(&(host_ip_info_val->host_port), &host_imu_ipcfg.host_imu_data_port, sizeof(host_imu_ipcfg.host_imu_data_port));
|
||||
memcpy(&(host_ip_info_val->lidar_port), &host_imu_ipcfg.lidar_imu_data_port, sizeof(host_imu_ipcfg.lidar_imu_data_port));
|
||||
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(HostIpInfoValue);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool BuildRequest::InitLidarIpinfoVal(const LivoxLidarIpInfo& lidar_ip_config, LivoxLidarIpInfoValue* lidar_ipinfo_val_ptr) {
|
||||
if (lidar_ipinfo_val_ptr == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Set lidar ip info
|
||||
std::vector<uint8_t> vec_lidar_ip;
|
||||
if (!IpToU8(lidar_ip_config.ip_addr, ".", vec_lidar_ip)) {
|
||||
return false;
|
||||
}
|
||||
memcpy(lidar_ipinfo_val_ptr->lidar_ipaddr, vec_lidar_ip.data(), sizeof(uint8_t) * 4);
|
||||
|
||||
// Set lidar subnet mask
|
||||
std::vector<uint8_t> vec_lidar_subnet_mask;
|
||||
if (!IpToU8(lidar_ip_config.net_mask, ".", vec_lidar_subnet_mask)) {
|
||||
return false;
|
||||
}
|
||||
memcpy(lidar_ipinfo_val_ptr->lidar_subnet_mask, vec_lidar_subnet_mask.data(), sizeof(uint8_t) * 4);
|
||||
|
||||
// Set lidar gateway
|
||||
std::vector<uint8_t> vec_lidar_gateway;
|
||||
if (!IpToU8(lidar_ip_config.gw_addr, ".", vec_lidar_gateway)) {
|
||||
return false;
|
||||
}
|
||||
memcpy(lidar_ipinfo_val_ptr->lidar_gateway, vec_lidar_gateway.data(), sizeof(uint8_t) * 4);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool BuildRequest::InitHostIpAddr(const std::string& host_ip, HostIpInfoValue* host_ipinfo_val_ptr) {
|
||||
if (host_ipinfo_val_ptr == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Set lidar ip info
|
||||
std::vector<uint8_t> vec_host_ip;
|
||||
if (!IpToU8(host_ip, ".", vec_host_ip)) {
|
||||
return false;
|
||||
}
|
||||
memcpy(host_ipinfo_val_ptr->host_ip, vec_host_ip.data(), sizeof(uint8_t) * 4);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool BuildRequest::InitMulticastHostIpAddr(const std::string& multicast_ip, HostIpInfoValue* host_ipinfo_val_ptr) {
|
||||
if (host_ipinfo_val_ptr == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Set lidar multicast ip info
|
||||
std::vector<uint8_t> vec_host_ip;
|
||||
if (!IpToU8(multicast_ip, ".", vec_host_ip)) {
|
||||
return false;
|
||||
}
|
||||
memcpy(host_ipinfo_val_ptr->host_ip, vec_host_ip.data(), sizeof(uint8_t) * 4);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool BuildRequest::IpToU8(const std::string& src, const std::string& seq, std::vector<uint8_t>& result) {
|
||||
std::string::size_type pos1, pos2;
|
||||
pos2 = src.find(seq);
|
||||
pos1 = 0;
|
||||
while (std::string::npos != pos2) {
|
||||
int32_t val = std::stoi(src.substr(pos1, pos2-pos1));
|
||||
if (val < 0 || val > 256) {
|
||||
LOG_ERROR("Build broadcast request failed, ip to u8 failed, the ip:{}, the fault val:{}",
|
||||
src, val);
|
||||
return false;
|
||||
}
|
||||
|
||||
result.push_back(val);
|
||||
pos1 = pos2 + seq.size();
|
||||
pos2 = src.find(seq, pos1);
|
||||
}
|
||||
|
||||
if (pos1 != src.length()) {
|
||||
int32_t val = std::stoi(src.substr(pos1, pos2-pos1));
|
||||
if (val < 0 || val > 256) {
|
||||
LOG_ERROR("Build broadcast request failed, ip to u8 failed, the ip:{}, the fault val:{}",
|
||||
src, val);
|
||||
return false;
|
||||
}
|
||||
result.push_back(val);
|
||||
}
|
||||
|
||||
if (result.size() != 4) {
|
||||
LOG_ERROR("Build broadcast request failed, ip to u8 failed, the ip:{}, the val size:{}",
|
||||
src.c_str(), result.size());
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace lidar
|
||||
} // namespace livox
|
||||
@@ -0,0 +1,66 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
#ifndef LIVOX_BUILD_REQUEST_H_
|
||||
#define LIVOX_BUILD_REQUEST_H_
|
||||
|
||||
#include <memory>
|
||||
#include <map>
|
||||
#include <list>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include "base/io_loop.h"
|
||||
#include "comm/comm_port.h"
|
||||
|
||||
#include "livox_lidar_def.h"
|
||||
#include "comm/define.h"
|
||||
|
||||
#include <memory>
|
||||
|
||||
namespace livox {
|
||||
namespace lidar {
|
||||
|
||||
|
||||
class BuildRequest {
|
||||
public:
|
||||
static bool BuildUpdateViewLidarCfgRequest(const ViewLidarIpInfo& view_lidar_info, uint8_t* req_buf, uint16_t& req_len);
|
||||
static bool BuildUpdateLidarCfgRequest(const LivoxLidarCfg& lidar_cfg, uint8_t* req_buf, uint16_t& req_len);
|
||||
static bool BuildUpdateMid360LidarCfgRequest(const LivoxLidarCfg& lidar_cfg, uint8_t* req_buf, uint16_t& req_len);
|
||||
static bool BuildSetLidarIPInfoRequest(const LivoxLidarIpInfo& ip_config, uint8_t* req_buf, uint16_t& req_len);
|
||||
static bool BuildSetHostStateInfoIPCfgRequest(const HostStateInfoIpInfo& host_state_info_ipcfg, uint8_t* req_buf, uint16_t& req_len);
|
||||
static bool BuildSetHostPointDataIPInfoRequest(const HostPointIPInfo& lidar_ip_config, uint8_t* req_buf, uint16_t& req_len);
|
||||
static bool BuildSetHostImuDataIPInfoRequest(const HostImuDataIPInfo& host_imu_ipcfg, uint8_t* req_buf, uint16_t& req_len);
|
||||
static bool IpToU8(const std::string& src, const std::string& seq, std::vector<uint8_t>& result);
|
||||
private:
|
||||
static bool InitLidarIpinfoVal(const LivoxLidarIpInfo& lidar_ip_config, LivoxLidarIpInfoValue* lidar_ipinfo_val_ptr);
|
||||
static bool InitMulticastHostIpAddr(const std::string& multicast_ip, HostIpInfoValue* host_ipinfo_val_ptr);
|
||||
static bool InitHostIpAddr(const std::string& host_ip, HostIpInfoValue* host_ipinfo_val_ptr);
|
||||
};
|
||||
|
||||
} // namespace direct
|
||||
} // namespace livox
|
||||
|
||||
# endif // LIVOX_BUILD_REQUEST_H_
|
||||
@@ -0,0 +1,64 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#ifndef COMMAND_HANDLER_H_
|
||||
#define COMMAND_HANDLER_H_
|
||||
|
||||
#include <memory>
|
||||
#include <map>
|
||||
#include <mutex>
|
||||
|
||||
#include "base/command_callback.h"
|
||||
#include "base/io_thread.h"
|
||||
|
||||
#include "comm/protocol.h"
|
||||
#include "comm/define.h"
|
||||
|
||||
#include "livox_lidar_def.h"
|
||||
#include "device_manager.h"
|
||||
|
||||
namespace livox {
|
||||
namespace lidar {
|
||||
|
||||
class CommandHandler {
|
||||
public:
|
||||
CommandHandler(DeviceManager* device_manager) : device_manager_(device_manager) {}
|
||||
~CommandHandler() {}
|
||||
|
||||
virtual bool Init(bool is_view) = 0;
|
||||
virtual bool Init(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map) = 0;
|
||||
|
||||
virtual void Handle(const uint32_t handle, uint16_t lidar_port, const Command& command) = 0;
|
||||
virtual void UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info) = 0;
|
||||
virtual void UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port) = 0;
|
||||
virtual livox_status SendCommand(const Command& command) = 0;
|
||||
virtual livox_status SendLoggerCommand(const Command &command) = 0;
|
||||
protected:
|
||||
DeviceManager* device_manager_;
|
||||
};
|
||||
|
||||
} // namespace livox
|
||||
} // namespace lidar
|
||||
|
||||
#endif // COMMAND_HANDLER_H_
|
||||
@@ -0,0 +1,886 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#include "command_impl.h"
|
||||
|
||||
#include "livox_lidar_def.h"
|
||||
#include "general_command_handler.h"
|
||||
#include "debug_point_cloud_handler/debug_point_cloud_manager.h"
|
||||
#include "spdlog/fmt/fmt.h"
|
||||
|
||||
#include "base/logging.h"
|
||||
#include "comm/protocol.h"
|
||||
#include "comm/generate_seq.h"
|
||||
|
||||
#include "build_request.h"
|
||||
|
||||
#include <sstream>
|
||||
#include <inttypes.h>
|
||||
#include <string>
|
||||
#include <iomanip>
|
||||
#include <chrono>
|
||||
#include <vector>
|
||||
|
||||
namespace livox {
|
||||
namespace lidar {
|
||||
|
||||
std::int64_t StringToTimestamp(std::string const& fmt, std::string const& date) {
|
||||
std::tm timestamp = {};
|
||||
std::stringstream date_ss(date);
|
||||
date_ss >> std::get_time(×tamp, fmt.c_str());
|
||||
auto time_point = std::chrono::system_clock::from_time_t(std::mktime(×tamp));
|
||||
|
||||
return std::chrono::duration_cast<std::chrono::seconds>(time_point.time_since_epoch()).count();
|
||||
}
|
||||
|
||||
std::vector<std::string> Split(std::string const& str, char const pattern) {
|
||||
std::vector<std::string> res;
|
||||
std::stringstream input(str);
|
||||
std::string part;
|
||||
while (getline(input, part, pattern)) {
|
||||
res.push_back(part);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
std::uint64_t ParseGPRMC(std::string const& gprmc) {
|
||||
std::vector<std::string> gprmc_vec = Split(gprmc, ',');
|
||||
if (gprmc_vec.size() < 9 || gprmc_vec[1].length() < 6 || gprmc_vec[9].length() < 6) {
|
||||
LOG_ERROR("gprmc check failed. gprmc is : {}", gprmc);
|
||||
return 0;
|
||||
}
|
||||
auto year = gprmc_vec[9].substr(4);
|
||||
auto month = gprmc_vec[9].substr(2, 2);
|
||||
auto day = gprmc_vec[9].substr(0, 2);
|
||||
auto hour = gprmc_vec[1].substr(0, 2);
|
||||
auto minute = gprmc_vec[1].substr(2, 2);
|
||||
auto second = gprmc_vec[1].substr(4, 2);
|
||||
|
||||
std::string time = fmt::format("{}-{}-{} {}:{}:{}", "20" + year, month, day, hour, minute, second);
|
||||
std::uint64_t time_ms = StringToTimestamp("%Y-%m-%d %H:%M:%S", time) * 1000;
|
||||
return time_ms * 1000 * 1000;
|
||||
}
|
||||
|
||||
livox_status CommandImpl::QueryLivoxLidarInternalInfo(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
std::set<ParamKeyName> key_sets;
|
||||
|
||||
if (!GeneralCommandHandler::GetInstance().GetQueryLidarInternalInfoKeys(handle, key_sets)) {
|
||||
LOG_ERROR("Query livox lidar internal info failed.");
|
||||
return kLivoxLidarStatusInvalidHandle;
|
||||
}
|
||||
|
||||
uint16_t key_num = key_sets.size();
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
for (const auto &key : key_sets) {
|
||||
LivoxLidarKeyValueParam* kList = (LivoxLidarKeyValueParam*)&req_buff[req_len];
|
||||
kList->key = static_cast<uint16_t>(key);
|
||||
req_len += sizeof(uint16_t);
|
||||
}
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarGetInternalInfo,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarDiagInternalInfoResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::QueryLivoxLidarFwType(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
|
||||
LivoxLidarKeyValueParam* kList = (LivoxLidarKeyValueParam*)&req_buff[req_len];
|
||||
kList->key = static_cast<uint16_t>(kKeyFwType);
|
||||
req_len += sizeof(uint16_t);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarGetInternalInfo,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarDiagInternalInfoResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::QueryLivoxLidarFirmwareVer(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
|
||||
LivoxLidarKeyValueParam* kList = (LivoxLidarKeyValueParam*)&req_buff[req_len];
|
||||
kList->key = static_cast<uint16_t>(kKeyVersionApp);
|
||||
req_len += sizeof(uint16_t);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarGetInternalInfo,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarDiagInternalInfoResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarPclDataType(uint32_t handle, LivoxLidarPointDataType data_type, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyPclDataType);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = static_cast<uint8_t>(data_type);
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarScanPattern(uint32_t handle, LivoxLidarScanPattern scan_pattern, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyPatternMode);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = static_cast<uint8_t>(scan_pattern);
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarDualEmit(uint32_t handle, bool enable, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyDualEmitEn);
|
||||
kv->length = sizeof(uint8_t);
|
||||
if (enable) {
|
||||
kv->value[0] = 0x01;
|
||||
} else {
|
||||
kv->value[0] = 0x00;
|
||||
}
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::EnableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyPointSendEn);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = 0x00;
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::DisableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyPointSendEn);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = 0x01;
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarIp(uint32_t handle, const LivoxLidarIpInfo* ip_config,
|
||||
LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
if (!BuildRequest::BuildSetLidarIPInfoRequest(*ip_config, req_buff, req_len)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarStateInfoHostIPCfg(uint32_t handle, const HostStateInfoIpInfo& host_state_info_ipcfg,
|
||||
LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
if (!BuildRequest::BuildSetHostStateInfoIPCfgRequest(host_state_info_ipcfg, req_buff, req_len)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarPointDataHostIPCfg(uint32_t handle, const HostPointIPInfo& host_point_ipcfg,
|
||||
LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
if (!BuildRequest::BuildSetHostPointDataIPInfoRequest(host_point_ipcfg, req_buff, req_len)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarImuDataHostIPCfg(uint32_t handle, const HostImuDataIPInfo& host_imu_ipcfg,
|
||||
LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
if (!BuildRequest::BuildSetHostImuDataIPInfoRequest(host_imu_ipcfg, req_buff, req_len)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarInstallAttitude(uint32_t handle, const LivoxLidarInstallAttitude& install_attitude,
|
||||
LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyInstallAttitude);
|
||||
kv->length = sizeof(LivoxLidarInstallAttitude);
|
||||
|
||||
LivoxLidarInstallAttitude* install_attitude_val = (LivoxLidarInstallAttitude*)&kv->value;
|
||||
memcpy(install_attitude_val, &install_attitude, sizeof(LivoxLidarInstallAttitude));
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(LivoxLidarInstallAttitude);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarFovCfg0(uint32_t handle, const FovCfg& fov_cfg0, LivoxLidarAsyncControlCallback cb,
|
||||
void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyFovCfg0);
|
||||
kv->length = sizeof(FovCfg);
|
||||
|
||||
FovCfg* fov_cfg = (FovCfg*)&kv->value;
|
||||
memcpy(fov_cfg, &fov_cfg0, sizeof(FovCfg));
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(FovCfg);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarFovCfg1(uint32_t handle, const FovCfg& fov_cfg1, LivoxLidarAsyncControlCallback cb,
|
||||
void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyFovCfg1);
|
||||
kv->length = sizeof(FovCfg);
|
||||
|
||||
FovCfg* fov_cfg = (FovCfg*)&kv->value;
|
||||
memcpy(fov_cfg, &fov_cfg1, sizeof(FovCfg));
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(FovCfg);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::EnableLivoxLidarFov(uint32_t handle, uint8_t fov_en, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyFovCfgEn);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = fov_en;
|
||||
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::DisableLivoxLidarFov(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyFovCfgEn);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = 0x00;
|
||||
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarDetectMode(uint32_t handle, LivoxLidarDetectMode mode,
|
||||
LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyDetectMode);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = static_cast<uint8_t>(mode);
|
||||
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarFuncIOCfg(uint32_t handle, const FuncIOCfg& func_io_cfg,
|
||||
LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyFuncIoCfg);
|
||||
kv->length = sizeof(FuncIOCfg);
|
||||
|
||||
FuncIOCfg* func_io_cfg_val = (FuncIOCfg*)&kv->value;
|
||||
memcpy(func_io_cfg_val, &func_io_cfg, sizeof(FuncIOCfg));
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(FuncIOCfg);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarBlindSpot(uint32_t handle, uint32_t blind_spot, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
//blind spot
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyBlindSpotSet);
|
||||
kv->length = sizeof(uint32_t);
|
||||
uint32_t* blind_spot_set = reinterpret_cast<uint32_t*>(&kv->value[0]);
|
||||
*blind_spot_set = blind_spot;
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(uint32_t);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarWorkMode(uint32_t handle, LivoxLidarWorkMode work_mode, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyWorkMode);
|
||||
kv->length = sizeof(uint8_t);
|
||||
uint8_t* val_work_mode = reinterpret_cast<uint8_t*>(&kv->value[0]);
|
||||
*val_work_mode = work_mode;
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(uint8_t);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
|
||||
livox_status CommandImpl::EnableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
//glass heat
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyGlassHeat);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = 0x01; //enable glass heat
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::DisableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
//glass heat
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyGlassHeat);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = 0x00; //disable glass heat
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarGlassHeat(uint32_t handle, LivoxLidarGlassHeat glass_heat, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
//glass heat
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyGlassHeat);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = static_cast<uint8_t>(glass_heat);
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::EnableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyImuDataEn);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = 0x01;
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::DisableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyImuDataEn);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = 0x00;
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::EnableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
//glass heat
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyFusaEn);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = 0x01; //enable glass heat
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::DisableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
//glass heat
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyFusaEn);
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = 0x00; //disable glass heat
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::StartForcedHeating(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
return SendSingleControlCommand(handle, cb, client_data, kKeyForceHeatEn, 0x01/*enable forced heating*/);
|
||||
}
|
||||
|
||||
livox_status CommandImpl::StopForcedHeating(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
return SendSingleControlCommand(handle, cb, client_data, kKeyForceHeatEn, 0x00/*disable forced heating*/);
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarEscMode(uint32_t handle, LivoxLidarEscMode esc_mode, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeySetEscMode);
|
||||
kv->length = sizeof(uint8_t);
|
||||
uint8_t* val_esc_mode = reinterpret_cast<uint8_t*>(&kv->value[0]);
|
||||
*val_esc_mode = esc_mode;
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(uint8_t);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarLogParam(uint32_t handle, const LivoxLidarLogParam& log_param, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyLogParamSet);
|
||||
kv->length = sizeof(LivoxLidarLogParam);
|
||||
LivoxLidarLogParam* log_param_val = (LivoxLidarLogParam*)&kv->value;
|
||||
memcpy(log_param_val, &log_param, sizeof(LivoxLidarLogParam));
|
||||
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - sizeof(uint8_t) + sizeof(LivoxLidarLogParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data) {
|
||||
return GeneralCommandHandler::GetInstance().LivoxLidarRequestReset(handle, cb, client_data);
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarDebugPointCloud(uint32_t handle, bool enable,
|
||||
LivoxLidarLoggerCallback cb, void* client_data) {
|
||||
DebugPointCloudManager::GetInstance().Enable(enable);
|
||||
|
||||
LivoxLidarDebugPointCloudRequest req_buff {};
|
||||
req_buff.enable = enable ? 1 : 0;
|
||||
req_buff.host_port = kHostDebugPointCloudPort; // 44332
|
||||
req_buff.bandwidth = 0; // units Mbps
|
||||
sscanf(GeneralCommandHandler::GetInstance().GetLidarCfg(handle).host_net_info.host_ip.c_str(),
|
||||
"%" SCNu8 ".%" SCNu8 ".%" SCNu8 ".%" SCNu8, &req_buff.host_ip_addr[0]
|
||||
, &req_buff.host_ip_addr[1]
|
||||
, &req_buff.host_ip_addr[2]
|
||||
, &req_buff.host_ip_addr[3]);
|
||||
return GeneralCommandHandler::GetInstance().SendLoggerCommand(handle,
|
||||
kCommandIDLidarDebugPointCloudControl,
|
||||
reinterpret_cast<uint8_t*>(&req_buff),
|
||||
uint16_t(sizeof(LivoxLidarDebugPointCloudRequest)),
|
||||
MakeCommandCallback<LivoxLidarLoggerResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarRmcSyncTime(uint32_t handle, const char* rmc, uint16_t rmc_length,
|
||||
LivoxLidarRmcSyncTimeCallBack cb, void* client_data) {
|
||||
LivoxLidarRmcSyncTimeRequest req_buff {};
|
||||
req_buff.type = LivoxLidarRmcSyncTimeRequest::SyncTimeType::kRmcSyncTime;
|
||||
req_buff.ns = ParseGPRMC(std::string(rmc, rmc_length));
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarSetPPSSync,
|
||||
reinterpret_cast<uint8_t*>(&req_buff),
|
||||
uint16_t(sizeof(LivoxLidarRmcSyncTimeRequest)),
|
||||
MakeCommandCallback<LivoxLidarRmcSyncTimeResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SetLivoxLidarWorkModeAfterBoot(uint32_t handle, LivoxLidarWorkModeAfterBoot work_mode, LivoxLidarAsyncControlCallback cb, void* client_data) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
uint16_t key_num = 1;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = static_cast<uint16_t>(kKeyWorkModeAfterBoot);
|
||||
kv->length = sizeof(uint8_t);
|
||||
uint8_t* val_work_mode = reinterpret_cast<uint8_t*>(&kv->value[0]);
|
||||
*val_work_mode = work_mode;
|
||||
req_len += sizeof(LivoxLidarKeyValueParam) - 1 + sizeof(uint8_t);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
// Upgrade
|
||||
livox_status CommandImpl::LivoxLidarStartUpgrade(uint32_t handle, uint8_t *data, uint16_t length,
|
||||
LivoxLidarStartUpgradeCallback cb, void* client_data) {
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDGeneralRequestUpgrade,
|
||||
data,
|
||||
length,
|
||||
MakeCommandCallback<LivoxLidarStartUpgradeResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::LivoxLidarXferFirmware(uint32_t handle, uint8_t *data, uint16_t length,
|
||||
LivoxLidarXferFirmwareCallback cb, void* client_data) {
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDGeneralXferFirmware,
|
||||
data,
|
||||
length,
|
||||
MakeCommandCallback<LivoxLidarXferFirmwareResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::LivoxLidarCompleteXferFirmware(uint32_t handle, uint8_t *data, uint16_t length,
|
||||
LivoxLidarCompleteXferFirmwareCallback cb, void* client_data) {
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDGeneralCompleteXferFirmware,
|
||||
data,
|
||||
length,
|
||||
MakeCommandCallback<LivoxLidarCompleteXferFirmwareResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::LivoxLidarGetUpgradeProgress(uint32_t handle, uint8_t *data,
|
||||
uint16_t length, LivoxLidarGetUpgradeProgressCallback cb, void* client_data) {
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDGeneralRequestUpgradeProgress,
|
||||
data,
|
||||
length,
|
||||
MakeCommandCallback<LivoxLidarGetUpgradeProgressResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::LivoxLidarRequestFirmwareInfo(uint32_t handle,
|
||||
LivoxLidarRequestFirmwareInfoCallback cb, void* client_data) {
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDGeneralRequestFirmwareInfo,
|
||||
nullptr,
|
||||
0,
|
||||
MakeCommandCallback<LivoxLidarRequestFirmwareInfoResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::LivoxLidarRequestReboot(uint32_t handle, LivoxLidarRebootCallback cb,
|
||||
void* client_data) {
|
||||
LivoxLidarRebootRequest reboot_request;
|
||||
reboot_request.timeout = 100;
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarRebootDevice, (uint8_t *)&reboot_request,
|
||||
sizeof(reboot_request), MakeCommandCallback<LivoxLidarRebootResponse>(cb,
|
||||
client_data));
|
||||
}
|
||||
|
||||
livox_status CommandImpl::SendSingleControlCommand(uint32_t handle,
|
||||
LivoxLidarAsyncControlCallback cb,
|
||||
void* client_data,
|
||||
uint16_t command_key,
|
||||
uint8_t value) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t key_num = 1;
|
||||
uint16_t req_len = 0;
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
LivoxLidarKeyValueParam * kv = (LivoxLidarKeyValueParam *)&req_buff[req_len];
|
||||
kv->key = command_key;
|
||||
kv->length = sizeof(uint8_t);
|
||||
kv->value[0] = value;
|
||||
req_len += sizeof(LivoxLidarKeyValueParam);
|
||||
|
||||
return GeneralCommandHandler::GetInstance().SendCommand(handle,
|
||||
kCommandIDLidarWorkModeControl,
|
||||
req_buff,
|
||||
req_len,
|
||||
MakeCommandCallback<LivoxLidarAsyncControlResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
} // namespace livox
|
||||
} // namespace lidar
|
||||
|
||||
@@ -0,0 +1,149 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#ifndef COMMAND_IMPL_H_
|
||||
#define COMMAND_IMPL_H_
|
||||
|
||||
#include <memory>
|
||||
#include <map>
|
||||
#include <mutex>
|
||||
|
||||
#include "base/command_callback.h"
|
||||
#include "base/io_thread.h"
|
||||
|
||||
#include "comm/protocol.h"
|
||||
#include "comm/define.h"
|
||||
|
||||
#include "livox_lidar_api.h"
|
||||
|
||||
#include "livox_lidar_def.h"
|
||||
#include "device_manager.h"
|
||||
#include "command_handler.h"
|
||||
|
||||
namespace livox {
|
||||
namespace lidar {
|
||||
|
||||
class CommandImpl {
|
||||
public:
|
||||
static livox_status QueryLivoxLidarInternalInfo(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data);
|
||||
|
||||
static livox_status QueryLivoxLidarFwType(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data);
|
||||
|
||||
static livox_status QueryLivoxLidarFirmwareVer(uint32_t handle, QueryLivoxLidarInternalInfoCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarPclDataType(uint32_t handle, LivoxLidarPointDataType data_type, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarScanPattern(uint32_t handle, LivoxLidarScanPattern scan_pattern, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarDualEmit(uint32_t handle, bool enable, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status EnableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
static livox_status DisableLivoxLidarPointSend(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarIp(uint32_t handle, const LivoxLidarIpInfo* ip_config, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
static livox_status SetLivoxLidarStateInfoHostIPCfg(uint32_t handle, const HostStateInfoIpInfo& host_state_info_ipcfg,
|
||||
LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
static livox_status SetLivoxLidarPointDataHostIPCfg(uint32_t handle, const HostPointIPInfo& host_point_ipcfg,
|
||||
LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarImuDataHostIPCfg(uint32_t handle, const HostImuDataIPInfo& host_imu_ipcfg,
|
||||
LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarInstallAttitude(uint32_t handle, const LivoxLidarInstallAttitude& install_attitude,
|
||||
LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarFovCfg0(uint32_t handle, const FovCfg& fov_cfg0, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
static livox_status SetLivoxLidarFovCfg1(uint32_t handle, const FovCfg& fov_cfg1, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
static livox_status EnableLivoxLidarFov(uint32_t handle, uint8_t fov_en, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
static livox_status DisableLivoxLidarFov(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarDetectMode(uint32_t handle, LivoxLidarDetectMode mode,
|
||||
LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarFuncIOCfg(uint32_t handle, const FuncIOCfg& func_io_cfg,
|
||||
LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarBlindSpot(uint32_t handle, uint32_t blind_spot, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarWorkMode(uint32_t handle, LivoxLidarWorkMode work_mode, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status EnableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
static livox_status DisableLivoxLidarGlassHeat(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarGlassHeat(uint32_t handle, LivoxLidarGlassHeat glass_heat, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status EnableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
static livox_status DisableLivoxLidarImuData(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status EnableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
static livox_status DisableLivoxLidarFusaFunciont(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status StartForcedHeating(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
static livox_status StopForcedHeating(uint32_t handle, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarEscMode(uint32_t handle, LivoxLidarEscMode esc_mode, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarLogParam(uint32_t handle, const LivoxLidarLogParam& log_param, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
static livox_status LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarDebugPointCloud(uint32_t handle, bool enable, LivoxLidarLoggerCallback cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarRmcSyncTime(uint32_t handle, const char* rmc, uint16_t rmc_length, LivoxLidarRmcSyncTimeCallBack cb, void* client_data);
|
||||
|
||||
static livox_status SetLivoxLidarWorkModeAfterBoot(uint32_t handle, LivoxLidarWorkModeAfterBoot work_mode, LivoxLidarAsyncControlCallback cb, void* client_data);
|
||||
|
||||
/*******Upgrade Module***********/
|
||||
static livox_status LivoxLidarRequestReboot(uint32_t handle, LivoxLidarRebootCallback cb, void* client_data);
|
||||
|
||||
/**
|
||||
* Upgrade related command
|
||||
*/
|
||||
static livox_status LivoxLidarStartUpgrade(uint32_t handle, uint8_t *data, uint16_t length,
|
||||
LivoxLidarStartUpgradeCallback cb, void* client_data);
|
||||
|
||||
static livox_status LivoxLidarXferFirmware(uint32_t handle, uint8_t *data, uint16_t length,
|
||||
LivoxLidarXferFirmwareCallback cb, void* client_data);
|
||||
|
||||
static livox_status LivoxLidarCompleteXferFirmware(uint32_t handle, uint8_t *data,
|
||||
uint16_t length, LivoxLidarCompleteXferFirmwareCallback cb, void* client_data);
|
||||
|
||||
static livox_status LivoxLidarGetUpgradeProgress(uint32_t handle, uint8_t *data,
|
||||
uint16_t length, LivoxLidarGetUpgradeProgressCallback cb, void* client_data);
|
||||
|
||||
|
||||
static livox_status LivoxLidarRequestFirmwareInfo(uint32_t handle,
|
||||
LivoxLidarRequestFirmwareInfoCallback cb, void* client_data);
|
||||
private:
|
||||
static livox_status SendSingleControlCommand(uint32_t handle,
|
||||
LivoxLidarAsyncControlCallback cb,
|
||||
void* client_data,
|
||||
uint16_t command_key,
|
||||
uint8_t value);
|
||||
};
|
||||
|
||||
} // namespace livox
|
||||
} // namespace lidar
|
||||
|
||||
#endif // COMMAND_IMPL_H_
|
||||
@@ -0,0 +1,813 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#include "general_command_handler.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include "livox_lidar_def.h"
|
||||
#include "command_handler/command_handler.h"
|
||||
#include "command_handler/hap_command_handler.h"
|
||||
#include "command_handler/mid360_command_handler.h"
|
||||
#include "command_handler/mid360s_command_handler.h"
|
||||
#include "logger_handler/logger_manager.h"
|
||||
#include "debug_point_cloud_handler/debug_point_cloud_manager.h"
|
||||
#include "base/logging.h"
|
||||
#include "comm/protocol.h"
|
||||
#include "comm/generate_seq.h"
|
||||
|
||||
#include "build_request.h"
|
||||
|
||||
namespace livox {
|
||||
namespace lidar {
|
||||
|
||||
GeneralCommandHandler::GeneralCommandHandler()
|
||||
: device_manager_(nullptr),
|
||||
comm_port_(nullptr),
|
||||
livox_lidar_info_change_cb_(nullptr),
|
||||
livox_lidar_info_change_client_data_(nullptr),
|
||||
livox_lidar_info_cb_(nullptr),
|
||||
livox_lidar_info_client_data_(nullptr),
|
||||
detection_host_ip_(""),
|
||||
is_view_(false) {
|
||||
}
|
||||
|
||||
GeneralCommandHandler& GeneralCommandHandler::GetInstance() {
|
||||
static GeneralCommandHandler general_command_handler;
|
||||
return general_command_handler;
|
||||
}
|
||||
|
||||
bool GeneralCommandHandler::Init(const std::string& host_ip, const bool is_view, DeviceManager* device_manager) {
|
||||
is_view_ = is_view;
|
||||
detection_host_ip_ = host_ip;
|
||||
device_manager_ = device_manager;
|
||||
comm_port_.reset(new CommPort());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GeneralCommandHandler::Init(std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr, DeviceManager* device_manager) {
|
||||
if (comm_port_ == nullptr) {
|
||||
is_view_ = false;
|
||||
device_manager_ = device_manager;
|
||||
comm_port_.reset(new CommPort());
|
||||
}
|
||||
if (lidars_command_handler_.find(kLivoxLidarTypeIndustrialHAP) == lidars_command_handler_.end()) {
|
||||
lidars_command_handler_[kLivoxLidarTypeIndustrialHAP].reset(new HapCommandHandler(device_manager_));
|
||||
}
|
||||
if (lidars_command_handler_.find(kLivoxLidarTypeMid360) == lidars_command_handler_.end()) {
|
||||
lidars_command_handler_[kLivoxLidarTypeMid360].reset(new Mid360CommandHandler(device_manager_));
|
||||
}
|
||||
if (lidars_command_handler_.find(kLivoxLidarTypeMid360s) == lidars_command_handler_.end()) {
|
||||
lidars_command_handler_[kLivoxLidarTypeMid360s].reset(new Mid360sCommandHandler(device_manager_));
|
||||
}
|
||||
AddDetectedLidar(custom_lidars_cfg_ptr);
|
||||
return true;
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::AddDetectedLidar(const std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr) {
|
||||
for (auto it = custom_lidars_cfg_ptr->begin(); it != custom_lidars_cfg_ptr->end(); ++it) {
|
||||
const LivoxLidarCfg& lidar_cfg = *it;
|
||||
uint32_t lidar_ip = inet_addr(lidar_cfg.lidar_net_info.lidar_ipaddr.c_str());
|
||||
if (custom_lidars_cfg_map_.find(lidar_ip) == custom_lidars_cfg_map_.end()) {
|
||||
custom_lidars_cfg_map_[lidar_ip] = lidar_cfg;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::Destory() {
|
||||
device_manager_ = nullptr;
|
||||
comm_port_.reset(nullptr);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(dev_type_mutex_);
|
||||
device_dev_type_.clear();
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(devices_mutex_);
|
||||
devices_.clear();
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(command_handle_mutex_);
|
||||
lidars_command_handler_.clear();
|
||||
}
|
||||
|
||||
{
|
||||
std::mutex commands_mutex_;
|
||||
std::map<uint32_t, std::pair<Command, TimePoint> > commands_;
|
||||
}
|
||||
|
||||
livox_lidar_info_change_cb_ = nullptr;
|
||||
livox_lidar_info_change_client_data_ = nullptr;
|
||||
|
||||
detection_host_ip_ = "";
|
||||
is_view_ = false;
|
||||
}
|
||||
|
||||
GeneralCommandHandler::~GeneralCommandHandler() {
|
||||
Destory();
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::Handler(uint32_t handle, uint16_t lidar_port, uint8_t *buf, uint32_t buf_size) {
|
||||
if (buf == nullptr || buf_size == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
CommPacket packet;
|
||||
memset(&packet, 0, sizeof(packet));
|
||||
|
||||
if (!(comm_port_->ParseCommStream((uint8_t*)buf, buf_size, &packet))) {
|
||||
LOG_INFO("Parse GeneralCommandHandler Command Stream failed.");
|
||||
return;
|
||||
}
|
||||
|
||||
if (lidar_port == kDetectionPort && packet.cmd_id == kCommandIDLidarSearch) {
|
||||
if (packet.cmd_type == kCommandTypeCmd) {
|
||||
return;
|
||||
}
|
||||
HandleDetectionData(handle, lidar_port, packet);
|
||||
return;
|
||||
}
|
||||
|
||||
if (packet.cmd_type == kCommandTypeAck) {
|
||||
uint32_t seq = packet.seq_num;
|
||||
Command command;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(commands_mutex_);
|
||||
if (commands_.find(seq) == commands_.end()) {
|
||||
LOG_ERROR("Handle cmd ack failed, can not find command");
|
||||
return;
|
||||
}
|
||||
|
||||
command = commands_[seq].first;
|
||||
command.packet = packet;
|
||||
commands_.erase(seq);
|
||||
}
|
||||
|
||||
if (command.cb) {
|
||||
(*command.cb)(kLivoxLidarStatusSuccess, handle, command.packet.data);
|
||||
}
|
||||
return;
|
||||
}
|
||||
if (packet.cmd_id == kCommandIDLidarPushMsg) {
|
||||
std::shared_ptr<CommandHandler> cmd_handler = GetLidarCommandHandler(handle);
|
||||
if (cmd_handler == nullptr) {
|
||||
LOG_ERROR("Handler general command failed, get push msg command handler faield.");
|
||||
return;
|
||||
}
|
||||
Command command;
|
||||
command.packet = packet;
|
||||
cmd_handler->Handle(handle, lidar_port, command);
|
||||
}
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::Handler(const uint8_t dev_type, const uint32_t handle, const uint16_t lidar_port,
|
||||
uint8_t *buf, uint32_t buf_size) {
|
||||
if (buf == nullptr || buf_size == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (cmd_observer_cb_) {
|
||||
cmd_observer_cb_(handle, reinterpret_cast<LivoxLidarCmdPacket*>(buf), cmd_observer_client_data_);
|
||||
}
|
||||
|
||||
if (dev_type == kLivoxLidarTypePA && lidar_port == kPaLidarFaultPort) {
|
||||
std::shared_ptr<CommandHandler> cmd_handler = GetLidarCommandHandler(dev_type);
|
||||
if (cmd_handler == nullptr) {
|
||||
LOG_ERROR("GeneralCommandHandler::Handler get cmd handler failed");
|
||||
return;
|
||||
}
|
||||
Command command;
|
||||
command.packet.data = buf;
|
||||
command.packet.data_len = buf_size;
|
||||
cmd_handler->Handle(handle, lidar_port, command);
|
||||
return;
|
||||
}
|
||||
|
||||
CommPacket packet;
|
||||
memset(&packet, 0, sizeof(packet));
|
||||
if (!(comm_port_->ParseCommStream((uint8_t*)buf, buf_size, &packet))) {
|
||||
LOG_INFO("Parse Command Stream failed.");
|
||||
return;
|
||||
}
|
||||
|
||||
if (lidar_port == kDetectionPort && packet.cmd_id == kCommandIDLidarSearch) {
|
||||
if (packet.cmd_type == kCommandTypeCmd) {
|
||||
return;
|
||||
}
|
||||
HandleDetectionData(handle, lidar_port, packet);
|
||||
return;
|
||||
}
|
||||
|
||||
std::shared_ptr<CommandHandler> cmd_handler = GetLidarCommandHandler(dev_type);
|
||||
if (cmd_handler == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
Command command;
|
||||
if (packet.cmd_type == kCommandTypeAck) {
|
||||
uint16_t seq = packet.seq_num;
|
||||
std::lock_guard<std::mutex> lock(commands_mutex_);
|
||||
if (commands_.find(seq) != commands_.end()) {
|
||||
command = commands_[seq].first;
|
||||
command.packet = packet;
|
||||
commands_.erase(seq);
|
||||
}
|
||||
} else if (packet.cmd_type == kCommandTypeCmd) {
|
||||
command.packet = packet;
|
||||
command.handle = handle;
|
||||
}
|
||||
cmd_handler->Handle(handle, lidar_port, command);
|
||||
}
|
||||
|
||||
bool GeneralCommandHandler::VerifyNetSegment(const DetectionData* detection_data) {
|
||||
if (is_view_) {
|
||||
if (detection_host_ip_.empty()) {
|
||||
LOG_ERROR("Verify net segment faield, the host ip is empty.");
|
||||
return false;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> host_ip_vec;
|
||||
if (!BuildRequest::IpToU8(detection_host_ip_, ".", host_ip_vec)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (host_ip_vec[0] == detection_data->lidar_ip[0] &&
|
||||
host_ip_vec[1] == detection_data->lidar_ip[1] &&
|
||||
host_ip_vec[2] == detection_data->lidar_ip[2]) {
|
||||
LOG_INFO("Host ip:{}, lidar ip:{}.{}.{}.{}", detection_host_ip_.c_str(), detection_data->lidar_ip[0],
|
||||
detection_data->lidar_ip[1], detection_data->lidar_ip[2], detection_data->lidar_ip[3]);
|
||||
return true;
|
||||
}
|
||||
std::string lidar_ip = std::to_string(detection_data->lidar_ip[0]) + "." +
|
||||
std::to_string(detection_data->lidar_ip[1]) + "." +
|
||||
std::to_string(detection_data->lidar_ip[2]) + "." +
|
||||
std::to_string(detection_data->lidar_ip[3]);
|
||||
LOG_ERROR("The host address and lidar address are on different network segments, the host_ip:{}, lidar_ip:{}",
|
||||
detection_host_ip_.c_str(), lidar_ip.c_str());
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::CreateCommandHandler(const uint8_t dev_type) {
|
||||
if (!is_view_) {
|
||||
std::lock_guard<std::mutex> lock(command_handle_mutex_);
|
||||
if (dev_type == kLivoxLidarTypeIndustrialHAP) {
|
||||
if (!(lidars_command_handler_[dev_type]->Init(custom_lidars_cfg_map_))) {
|
||||
LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type);
|
||||
}
|
||||
} else if (dev_type == kLivoxLidarTypeMid360) {
|
||||
if (!(lidars_command_handler_[dev_type]->Init(custom_lidars_cfg_map_))) {
|
||||
LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type);
|
||||
}
|
||||
} else if (dev_type == kLivoxLidarTypePA) {
|
||||
if (!(lidars_command_handler_[dev_type]->Init(custom_lidars_cfg_map_))) {
|
||||
LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type);
|
||||
}
|
||||
} else if (dev_type == kLivoxLidarTypeMid360s) {
|
||||
if (!(lidars_command_handler_[dev_type]->Init(custom_lidars_cfg_map_))) {
|
||||
LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type);
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
std::lock_guard<std::mutex> lock(command_handle_mutex_);
|
||||
if (lidars_command_handler_.find(dev_type) == lidars_command_handler_.end()) {
|
||||
if (dev_type == kLivoxLidarTypeIndustrialHAP) {
|
||||
std::shared_ptr<HapCommandHandler> hap_command_handler_ptr(new HapCommandHandler(device_manager_));
|
||||
lidars_command_handler_[dev_type] = hap_command_handler_ptr;
|
||||
if (!(lidars_command_handler_[dev_type]->Init(is_view_))) {
|
||||
LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type);
|
||||
}
|
||||
} else if (dev_type == kLivoxLidarTypeMid360) {
|
||||
std::shared_ptr<Mid360CommandHandler> mid360_command_handler_ptr(new Mid360CommandHandler(device_manager_));
|
||||
lidars_command_handler_[dev_type] = mid360_command_handler_ptr;
|
||||
if (!(lidars_command_handler_[dev_type]->Init(is_view_))) {
|
||||
LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type);
|
||||
}
|
||||
} else if (dev_type == kLivoxLidarTypeMid360s) {
|
||||
std::shared_ptr<Mid360sCommandHandler> mid360s_command_handler_ptr(new Mid360sCommandHandler(device_manager_));
|
||||
lidars_command_handler_[dev_type] = mid360s_command_handler_ptr;
|
||||
if (!(lidars_command_handler_[dev_type]->Init(is_view_))) {
|
||||
LOG_ERROR("General command handler init failed, the lidar of type:{} command init failed.", dev_type);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::HandleDetectionData(uint32_t handle, uint16_t lidar_port, const CommPacket& packet) {
|
||||
if (packet.data == nullptr || packet.data_len == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
DetectionData* detection_data = (DetectionData*)(packet.data);
|
||||
if (detection_data->ret_code != 0) {
|
||||
LOG_ERROR("Detection lidar faield, the handle:{}, lidar_port:{}, ret_code:{}",
|
||||
handle, lidar_port, detection_data->ret_code);
|
||||
return;
|
||||
}
|
||||
|
||||
LOG_INFO("Handle detection data, handle:{}, dev_type:{}, sn:{}, cmd_port:{}",
|
||||
handle, detection_data->dev_type, detection_data->sn, detection_data->cmd_port);
|
||||
|
||||
LoggerManager::GetInstance().AddDevice(handle, detection_data);
|
||||
DebugPointCloudManager::GetInstance().AddDevice(handle, detection_data);
|
||||
|
||||
if (!VerifyNetSegment(detection_data)) {
|
||||
return;
|
||||
}
|
||||
|
||||
CreateCommandHandler(detection_data->dev_type);
|
||||
|
||||
std::string lidar_ip = std::to_string(detection_data->lidar_ip[0]) + "." +
|
||||
std::to_string(detection_data->lidar_ip[1]) + "." +
|
||||
std::to_string(detection_data->lidar_ip[2]) + "." +
|
||||
std::to_string(detection_data->lidar_ip[3]);
|
||||
|
||||
if (devices_.find(handle) != devices_.end()) {
|
||||
DeviceInfo& device_info = devices_[handle];
|
||||
if (!(device_info.is_update_cfg.load()) && (device_info.is_get_loader_mode.load()) && !(device_info.is_loader_mode.load())) {
|
||||
if (!is_view_) {
|
||||
UpdateLidarCfg(detection_data->dev_type, handle, detection_data->cmd_port);
|
||||
}
|
||||
}
|
||||
|
||||
if (strcmp(device_info.sn.c_str(), detection_data->sn) != 0) {
|
||||
LOG_ERROR("Lidar ip conflic, the lidar ip:{}, the sn1:{}, the sn2:{}", lidar_ip.c_str(),
|
||||
device_info.sn.c_str(), detection_data->sn);
|
||||
}
|
||||
|
||||
if (device_manager_) {
|
||||
device_manager_->HandleDetectionData(handle, detection_data, device_info.is_get_loader_mode.load(),
|
||||
device_info.is_loader_mode.load());
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(dev_type_mutex_);
|
||||
if (device_dev_type_.find(handle) != device_dev_type_.end()) {
|
||||
if (device_dev_type_[handle] != detection_data->dev_type) {
|
||||
LOG_ERROR("Lidar dev type conflic, the lidar ip:{}, the dev_type1:{}, the dev_type2:{}",
|
||||
lidar_ip.c_str(), device_dev_type_[handle], detection_data->dev_type);
|
||||
}
|
||||
} else {
|
||||
device_dev_type_[handle] = detection_data->dev_type;
|
||||
}
|
||||
}
|
||||
|
||||
DeviceInfo& device_info = devices_[handle];
|
||||
device_info.sn = detection_data->sn;
|
||||
device_info.lidar_ip = lidar_ip;
|
||||
device_info.dev_type = detection_data->dev_type;
|
||||
device_info.is_get_loader_mode.store(false);
|
||||
device_info.is_update_cfg.store(false);
|
||||
device_info.is_callback.store(false);
|
||||
GetFirmwareType(handle, device_info);
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::GetFirmwareType(const uint32_t handle, DeviceInfo& device_info) {
|
||||
if (!device_manager_->sdk_framework_cfg_ptr_->master_sdk) {
|
||||
return;
|
||||
}
|
||||
if (device_info.is_get_loader_mode.load()) {
|
||||
return;
|
||||
}
|
||||
|
||||
QueryFwType(handle);
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::QueryFwTypeCallback(livox_status status, uint32_t handle,
|
||||
LivoxLidarDiagInternalInfoResponse* response, void* client_data) {
|
||||
static int8_t count = 0;
|
||||
if (count > 10) {
|
||||
LOG_ERROR("Query livox lidar failed, the retry time more than 10.");
|
||||
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
|
||||
self->UpdateFwType(handle, 1);
|
||||
count = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
if (client_data == nullptr) {
|
||||
LOG_ERROR("Query livox lidar Fw type failed, client data is nullptr.");
|
||||
count += 1;
|
||||
return;
|
||||
}
|
||||
|
||||
if (status != kLivoxLidarStatusSuccess) {
|
||||
LOG_ERROR("Query livox lidar Fw type failed, the status:{}", status);
|
||||
count += 1;
|
||||
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
|
||||
self->QueryFwType(handle);
|
||||
return;
|
||||
}
|
||||
|
||||
if (response == nullptr) {
|
||||
LOG_ERROR("Query livox lidar Fw type failed, the response is nullptr.");
|
||||
count += 1;
|
||||
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
|
||||
self->QueryFwType(handle);
|
||||
return;
|
||||
}
|
||||
|
||||
if (response->ret_code != 0) {
|
||||
LOG_ERROR("Query livox lidar Fw type failed, the ret_code:{}", response->ret_code);
|
||||
count += 1;
|
||||
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
|
||||
self->QueryFwType(handle);
|
||||
return;
|
||||
}
|
||||
|
||||
// if (response->param_num != 1) {
|
||||
// LOG_ERROR("Query livox lidar Fw type failed, the key_num:{}", response->param_num);
|
||||
// count += 1;
|
||||
// GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
|
||||
// self->QueryFwType(handle);
|
||||
// return;
|
||||
// }
|
||||
|
||||
uint16_t off = 0;
|
||||
LivoxLidarKeyValueParam* kv = (LivoxLidarKeyValueParam*)&response->data[off];
|
||||
if (kv->key != kKeyFwType) {
|
||||
LOG_ERROR("Query Fw type filed, the key had fault, the key:{}", kv->key);
|
||||
count += 1;
|
||||
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
|
||||
self->QueryFwType(handle);
|
||||
return;
|
||||
}
|
||||
off += sizeof(uint16_t) * 2;
|
||||
if (kv->length != sizeof(uint8_t)) {
|
||||
LOG_ERROR("Query Fw type failed, the val lenth is error, the len:", kv->length);
|
||||
count += 1;
|
||||
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
|
||||
self->QueryFwType(handle);
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t fw_type = 0;
|
||||
memcpy(&fw_type, &(response->data[off]), kv->length);
|
||||
LOG_INFO("Query Fw type succ, the fw_type:{}", fw_type);
|
||||
|
||||
count = 0;
|
||||
|
||||
GeneralCommandHandler* self = (GeneralCommandHandler*)(client_data);
|
||||
self->UpdateFwType(handle, fw_type);
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::UpdateFwType(const uint32_t handle, const uint8_t fw_type) {
|
||||
if (devices_.find(handle) != devices_.end()) {
|
||||
DeviceInfo& device_info = devices_[handle];
|
||||
device_info.is_get_loader_mode.store(true);
|
||||
if (fw_type) {
|
||||
device_info.is_loader_mode.store(false);
|
||||
} else {
|
||||
device_info.is_loader_mode.store(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
livox_status GeneralCommandHandler::QueryFwType(const uint32_t handle) {
|
||||
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
|
||||
std::set<ParamKeyName> key_sets;
|
||||
key_sets.insert(kKeyFwType);
|
||||
|
||||
uint16_t key_num = key_sets.size();
|
||||
memcpy(&req_buff[req_len], &key_num, sizeof(key_num));
|
||||
|
||||
req_len = sizeof(key_num) + sizeof(uint16_t);
|
||||
|
||||
for (const auto &key : key_sets) {
|
||||
LivoxLidarKeyValueParam* kList = (LivoxLidarKeyValueParam*)&req_buff[req_len];
|
||||
kList->key = static_cast<uint16_t>(key);
|
||||
req_len += sizeof(uint16_t);
|
||||
}
|
||||
return SendCommand(handle, kCommandIDLidarGetInternalInfo, req_buff, req_len,
|
||||
MakeCommandCallback<LivoxLidarDiagInternalInfoResponse>(GeneralCommandHandler::QueryFwTypeCallback, this));
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info) {
|
||||
std::shared_ptr<CommandHandler> cmd_handler = GetLidarCommandHandler(view_lidar_info.dev_type);
|
||||
if (cmd_handler != nullptr && device_manager_->sdk_framework_cfg_ptr_->master_sdk) {
|
||||
cmd_handler->UpdateLidarCfg(view_lidar_info);
|
||||
}
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::UpdateLidarCfg(const uint8_t dev_type, const uint32_t handle, const uint16_t lidar_cmd_port) {
|
||||
std::shared_ptr<CommandHandler> cmd_handler = GetLidarCommandHandler(dev_type);
|
||||
if (cmd_handler != nullptr && device_manager_->sdk_framework_cfg_ptr_->master_sdk) {
|
||||
cmd_handler->UpdateLidarCfg(handle, lidar_cmd_port);
|
||||
}
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::LivoxLidarInfoChange(const uint32_t handle) {
|
||||
LivoxLidarInfo lidar_info;
|
||||
bool is_loader_mode = false;
|
||||
bool is_callback = false;
|
||||
{
|
||||
devices_[handle].is_update_cfg.store(true);
|
||||
if (devices_.find(handle) == devices_.end()) {
|
||||
LOG_ERROR("Lidar info change failed, can not found device, the handle:{}", handle);
|
||||
return;
|
||||
}
|
||||
|
||||
DeviceInfo& device_info = devices_[handle];
|
||||
is_loader_mode = device_info.is_loader_mode.load();
|
||||
is_callback = device_info.is_callback.load();
|
||||
device_info.is_callback.store(true);
|
||||
strcpy(lidar_info.sn, device_info.sn.c_str());
|
||||
strcpy(lidar_info.lidar_ip, device_info.lidar_ip.c_str());
|
||||
lidar_info.dev_type = device_info.dev_type;
|
||||
|
||||
}
|
||||
|
||||
if (device_manager_ && is_view_ && !is_loader_mode) {
|
||||
device_manager_->UpdateViewLidarCfgCallback(handle);
|
||||
}
|
||||
|
||||
if (!is_callback) {
|
||||
if (livox_lidar_info_change_cb_) {
|
||||
livox_lidar_info_change_cb_(handle, &lidar_info, livox_lidar_info_change_client_data_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::PushLivoxLidarInfo(const uint32_t handle, const std::string& info) {
|
||||
std::lock_guard<std::mutex> lock(dev_type_mutex_);
|
||||
if (device_dev_type_.find(handle) != device_dev_type_.end()) {
|
||||
uint8_t dev_type = device_dev_type_[handle];
|
||||
|
||||
if (livox_lidar_info_cb_) {
|
||||
livox_lidar_info_cb_(handle, dev_type, info.c_str(), livox_lidar_info_client_data_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::shared_ptr<CommandHandler> GeneralCommandHandler::GetLidarCommandHandler(const uint32_t handle) {
|
||||
std::lock_guard<std::mutex> lock(dev_type_mutex_);
|
||||
if (device_dev_type_.find(handle) != device_dev_type_.end()) {
|
||||
uint8_t dev_type = device_dev_type_[handle];
|
||||
return GetLidarCommandHandler(dev_type);
|
||||
}
|
||||
LOG_ERROR("Get command handler failed, get dev type failed, the handle:{}", handle);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
std::shared_ptr<CommandHandler> GeneralCommandHandler::GetLidarCommandHandler(const uint8_t dev_type) {
|
||||
std::lock_guard<std::mutex> lock(command_handle_mutex_);
|
||||
if (lidars_command_handler_.find(dev_type) != lidars_command_handler_.end()) {
|
||||
return lidars_command_handler_[dev_type];
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
bool GeneralCommandHandler::GetQueryLidarInternalInfoKeys(const uint32_t handle, std::set<ParamKeyName>& key_sets) {
|
||||
std::lock_guard<std::mutex> lock(dev_type_mutex_);
|
||||
if (device_dev_type_.find(handle) != device_dev_type_.end()) {
|
||||
uint8_t dev_type = device_dev_type_[handle];
|
||||
if (dev_type == kLivoxLidarTypeIndustrialHAP) {
|
||||
std::set<ParamKeyName> tmp_key_sets {
|
||||
kKeyPclDataType,
|
||||
kKeyPatternMode,
|
||||
kKeyDualEmitEn,
|
||||
kKeyPointSendEn,
|
||||
kKeyLidarIpCfg,
|
||||
kKeyLidarPointDataHostIpCfg,
|
||||
kKeyLidarImuHostIpCfg,
|
||||
kKeyLogHostIpCfg,
|
||||
kKeyInstallAttitude,
|
||||
kKeyBlindSpotSet,
|
||||
kKeyWorkMode,
|
||||
kKeyGlassHeat,
|
||||
kKeyImuDataEn,
|
||||
kKeyFusaEn,
|
||||
kKeyForceHeatEn,
|
||||
kKeySn,
|
||||
kKeyProductInfo,
|
||||
kKeyVersionApp,
|
||||
kKeyVersionLoader,
|
||||
kKeyVersionHardware,
|
||||
kKeyMac,
|
||||
kKeyCurWorkState,
|
||||
kKeyStatusCode,
|
||||
kKeyLidarDiagStatus,
|
||||
kKeyLidarFlashStatus,
|
||||
kKeyFwType,
|
||||
kKeyCurGlassHeatState
|
||||
};
|
||||
key_sets.swap(tmp_key_sets);
|
||||
return true;
|
||||
} else if (dev_type == kLivoxLidarTypeMid360) {
|
||||
std::set<ParamKeyName> tmp_key_sets {
|
||||
kKeyPclDataType,
|
||||
kKeyPatternMode,
|
||||
kKeyLidarIpCfg,
|
||||
kKeyStateInfoHostIpCfg,
|
||||
kKeyLidarPointDataHostIpCfg,
|
||||
kKeyLidarImuHostIpCfg,
|
||||
kKeyInstallAttitude,
|
||||
kKeyFovCfg0,
|
||||
kKeyFovCfg1,
|
||||
kKeyFovCfgEn,
|
||||
kKeyDetectMode,
|
||||
kKeyFuncIoCfg,
|
||||
kKeyWorkMode,
|
||||
kKeyImuDataEn,
|
||||
kKeySn,
|
||||
kKeyProductInfo,
|
||||
kKeyVersionApp,
|
||||
kKeyVersionLoader,
|
||||
kKeyVersionHardware,
|
||||
kKeyMac,
|
||||
kKeyCurWorkState,
|
||||
kKeyCoreTemp,
|
||||
kKeyPowerUpCnt,
|
||||
kKeyLocalTimeNow,
|
||||
kKeyLastSyncTime,
|
||||
kKeyTimeOffset,
|
||||
kKeyTimeSyncType,
|
||||
kKeyLidarDiagStatus,
|
||||
kKeyFwType,
|
||||
kKeyHmsCode
|
||||
};
|
||||
key_sets.swap(tmp_key_sets);
|
||||
return true;
|
||||
} else if (dev_type == kLivoxLidarTypeMid360s){
|
||||
std::set<ParamKeyName> tmp_key_sets {
|
||||
kKeyPclDataType,
|
||||
kKeyPatternMode,
|
||||
kKeyLidarIpCfg,
|
||||
kKeyStateInfoHostIpCfg,
|
||||
kKeyLidarPointDataHostIpCfg,
|
||||
kKeyLidarImuHostIpCfg,
|
||||
kKeyInstallAttitude,
|
||||
kKeyFovCfg0,
|
||||
kKeyFovCfg1,
|
||||
kKeyFovCfgEn,
|
||||
kKeyDetectMode,
|
||||
kKeyFuncIoCfg,
|
||||
kKeyWorkMode,
|
||||
kKeyImuDataEn,
|
||||
kKeySetEscMode,
|
||||
kKeySn,
|
||||
kKeyProductInfo,
|
||||
kKeyVersionApp,
|
||||
kKeyVersionLoader,
|
||||
kKeyVersionHardware,
|
||||
kKeyMac,
|
||||
kKeyCurWorkState,
|
||||
kKeyCoreTemp,
|
||||
kKeyPowerUpCnt,
|
||||
kKeyLocalTimeNow,
|
||||
kKeyLastSyncTime,
|
||||
kKeyTimeOffset,
|
||||
kKeyTimeSyncType,
|
||||
kKeyLidarDiagStatus,
|
||||
kKeyFwType,
|
||||
kKeyHmsCode
|
||||
};
|
||||
key_sets.swap(tmp_key_sets);
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
const LivoxLidarCfg& GeneralCommandHandler::GetLidarCfg(const uint32_t handle) {
|
||||
return custom_lidars_cfg_map_[handle];
|
||||
}
|
||||
|
||||
livox_status GeneralCommandHandler::LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data) {
|
||||
LivoxLidarResetRequest reset_request;
|
||||
std::string sn;
|
||||
if (devices_.find(handle) != devices_.end()) {
|
||||
sn = devices_[handle].sn;
|
||||
} else {
|
||||
return kLivoxLidarStatusChannelNotExist;
|
||||
}
|
||||
|
||||
if (sn.size() > 16) {
|
||||
LOG_ERROR("Request reset failed, the sn size too long, the sn:", sn.c_str());
|
||||
return kLivoxLidarStatusChannelNotExist;
|
||||
}
|
||||
|
||||
memcpy(reset_request.data, sn.c_str(), sn.size());
|
||||
return SendCommand(handle, kCommandIDLidarResetDevice, (uint8_t*)&reset_request, sizeof(LivoxLidarResetRequest),
|
||||
MakeCommandCallback<LivoxLidarResetResponse>(cb, client_data));
|
||||
}
|
||||
|
||||
livox_status GeneralCommandHandler::SendCommand(uint32_t handle,
|
||||
uint16_t command_id,
|
||||
uint8_t *data,
|
||||
uint16_t length,
|
||||
const std::shared_ptr<CommandCallback> &cb) {
|
||||
struct in_addr addr;
|
||||
addr.s_addr = handle;
|
||||
std::string lidar_ip = inet_ntoa(addr);
|
||||
uint16_t seq = GenerateSeq::GetSeq();
|
||||
Command command(seq, command_id, kCommandTypeCmd, kHostSend, data, length, handle, lidar_ip, cb);
|
||||
|
||||
std::shared_ptr<CommandHandler> cmd_handler = GetLidarCommandHandler(handle);
|
||||
if (cmd_handler == nullptr) {
|
||||
LOG_ERROR("Send command failed, get cmd handler failed, the handle:{}, command_id:{}.", handle, command_id);
|
||||
return kLivoxLidarStatusSendFailed;
|
||||
}
|
||||
|
||||
cmd_handler->SendCommand(command);
|
||||
AddCommand(command);
|
||||
return kLivoxLidarStatusSuccess;
|
||||
}
|
||||
|
||||
livox_status GeneralCommandHandler::SendLoggerCommand(uint32_t handle,
|
||||
uint16_t command_id,
|
||||
uint8_t *data,
|
||||
uint16_t length,
|
||||
const std::shared_ptr<CommandCallback> &cb) {
|
||||
struct in_addr addr;
|
||||
addr.s_addr = handle;
|
||||
std::string lidar_ip = inet_ntoa(addr);
|
||||
uint16_t seq = GenerateSeq::GetSeq();
|
||||
Command command(seq, command_id, kCommandTypeCmd, kHostSend, data, length, handle, lidar_ip, cb);
|
||||
|
||||
std::shared_ptr<CommandHandler> cmd_handler = GetLidarCommandHandler(handle);
|
||||
if (cmd_handler == nullptr) {
|
||||
LOG_ERROR("Send command failed, get cmd handler failed, the handle:{}, command_id:{}.", handle, command_id);
|
||||
return kLivoxLidarStatusSendFailed;
|
||||
}
|
||||
|
||||
cmd_handler->SendLoggerCommand(command);
|
||||
AddCommand(command);
|
||||
return kLivoxLidarStatusSuccess;
|
||||
}
|
||||
|
||||
|
||||
void GeneralCommandHandler::AddCommand(const Command& command) {
|
||||
if (command.packet.cmd_type == kCommandTypeAck) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(commands_mutex_);
|
||||
commands_[command.packet.seq_num] = std::make_pair(command, std::chrono::steady_clock::now() + std::chrono::milliseconds(command.time_out));
|
||||
Command &cmd = commands_[command.packet.seq_num].first;
|
||||
if (cmd.packet.data != NULL) {
|
||||
cmd.packet.data = NULL;
|
||||
cmd.packet.data_len = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void GeneralCommandHandler::CommandsHandle(TimePoint now) {
|
||||
std::list<Command> timeout_commands;
|
||||
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(commands_mutex_);
|
||||
std::map<uint32_t, std::pair<Command, TimePoint> >::iterator ite = commands_.begin();
|
||||
while (ite != commands_.end()) {
|
||||
std::pair<Command, TimePoint> &command_pair = ite->second;
|
||||
if (now > command_pair.second) {
|
||||
timeout_commands.push_back(command_pair.first);
|
||||
uint32_t seq = ite->first;
|
||||
++ite;
|
||||
commands_.erase(seq);
|
||||
} else {
|
||||
++ite;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (auto& timeout_command : timeout_commands) {
|
||||
if (timeout_command.cb) {
|
||||
(*timeout_command.cb)(kLivoxLidarStatusTimeout, timeout_command.handle, timeout_command.packet.data);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace livox
|
||||
} // namespace lidar
|
||||
|
||||
@@ -0,0 +1,163 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#ifndef GENERAL_COMMAND_HANDLER_H_
|
||||
#define GENERAL_COMMAND_HANDLER_H_
|
||||
|
||||
#include <memory>
|
||||
#include <map>
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
|
||||
#include "base/command_callback.h"
|
||||
#include "base/io_thread.h"
|
||||
|
||||
#include "comm/protocol.h"
|
||||
#include "comm/define.h"
|
||||
|
||||
#include "livox_lidar_api.h"
|
||||
|
||||
#include "livox_lidar_def.h"
|
||||
#include "device_manager.h"
|
||||
#include "command_handler.h"
|
||||
|
||||
namespace livox {
|
||||
namespace lidar {
|
||||
|
||||
typedef struct {
|
||||
std::string sn;
|
||||
std::string lidar_ip;
|
||||
uint8_t dev_type;
|
||||
std::atomic<bool> is_update_cfg;
|
||||
std::atomic<bool> is_get_loader_mode;
|
||||
std::atomic<bool> is_loader_mode;
|
||||
std::atomic<bool> is_callback;
|
||||
} DeviceInfo;
|
||||
|
||||
class HapCommandHandle;
|
||||
|
||||
class GeneralCommandHandler : public noncopyable {
|
||||
private:
|
||||
GeneralCommandHandler();
|
||||
GeneralCommandHandler(const GeneralCommandHandler& other) = delete;
|
||||
GeneralCommandHandler& operator=(const GeneralCommandHandler& other) = delete;
|
||||
public:
|
||||
~GeneralCommandHandler();
|
||||
void Destory();
|
||||
static GeneralCommandHandler& GetInstance();
|
||||
|
||||
bool Init(const std::string& host_ip, const bool is_view, DeviceManager* device_manager);
|
||||
|
||||
bool Init(std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr, DeviceManager* device_manager);
|
||||
|
||||
// void SetDeviceManager(DeviceManager* device_manager);
|
||||
|
||||
void Handler(uint32_t handle, uint16_t lidar_port, uint8_t *buf, uint32_t buf_size);
|
||||
|
||||
void Handler(const uint8_t dev_type, const uint32_t handle, const uint16_t lidar_port,
|
||||
uint8_t *buf, uint32_t buf_size);
|
||||
|
||||
void CreateCommandHandler(const uint8_t dev_type);
|
||||
|
||||
livox_status SendCommand(uint32_t handle, uint16_t command_id, uint8_t *data,
|
||||
uint16_t length, const std::shared_ptr<CommandCallback> &cb);
|
||||
|
||||
livox_status SendLoggerCommand(uint32_t handle, uint16_t command_id, uint8_t *data,
|
||||
uint16_t length, const std::shared_ptr<CommandCallback> &cb);
|
||||
|
||||
void CommandsHandle(TimePoint now);
|
||||
void AddCommand(const Command& command);
|
||||
void AddDetectedLidar(const std::shared_ptr<std::vector<LivoxLidarCfg>>& custom_lidars_cfg_ptr);
|
||||
|
||||
void SetLivoxLidarInfoChangeCallback(LivoxLidarInfoChangeCallback cb, void* client_data) {
|
||||
livox_lidar_info_change_cb_ = cb;
|
||||
livox_lidar_info_change_client_data_ = client_data;
|
||||
}
|
||||
|
||||
void SetLivoxLidarInfoCallback(LivoxLidarInfoCallback cb, void* client_data) {
|
||||
livox_lidar_info_cb_ = cb;
|
||||
livox_lidar_info_client_data_ = client_data;
|
||||
}
|
||||
|
||||
void LivoxLidarAddCmdObserver(LivoxLidarCmdObserverCallBack cb, void* client_data) {
|
||||
cmd_observer_cb_ = cb;
|
||||
cmd_observer_client_data_ = client_data;
|
||||
}
|
||||
|
||||
void LivoxLidarRemoveCmdObserver() {
|
||||
cmd_observer_cb_ = nullptr;
|
||||
cmd_observer_client_data_ = nullptr;
|
||||
}
|
||||
|
||||
void UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info);
|
||||
void UpdateLidarCfg(const uint8_t dev_type, const uint32_t handle, const uint16_t lidar_cmd_port);
|
||||
void LivoxLidarInfoChange(const uint32_t handle);
|
||||
void PushLivoxLidarInfo(const uint32_t handle, const std::string& info);
|
||||
bool GetQueryLidarInternalInfoKeys(const uint32_t handle, std::set<ParamKeyName>& key_sets);
|
||||
const LivoxLidarCfg& GetLidarCfg(const uint32_t handle);
|
||||
livox_status LivoxLidarRequestReset(uint32_t handle, LivoxLidarResetCallback cb, void* client_data);
|
||||
static void QueryFwTypeCallback(livox_status status, uint32_t handle, LivoxLidarDiagInternalInfoResponse* response, void* client_data);
|
||||
private:
|
||||
bool VerifyNetSegment(const DetectionData* detection_data);
|
||||
std::shared_ptr<CommandHandler> GetLidarCommandHandler(const uint8_t dev_type);
|
||||
std::shared_ptr<CommandHandler> GetLidarCommandHandler(const uint32_t handle);
|
||||
void HandleDetectionData(uint32_t handle, uint16_t lidar_port, const CommPacket& packet);
|
||||
void GetFirmwareType(const uint32_t handle, DeviceInfo& device_info);
|
||||
livox_status QueryFwType(const uint32_t handle);
|
||||
void UpdateFwType(const uint32_t handle, const uint8_t fw_type);
|
||||
private:
|
||||
DeviceManager* device_manager_;
|
||||
std::unique_ptr<CommPort> comm_port_;
|
||||
|
||||
std::map<uint32_t, LivoxLidarCfg> custom_lidars_cfg_map_;
|
||||
|
||||
std::mutex dev_type_mutex_;
|
||||
std::map<uint32_t, uint8_t> device_dev_type_;
|
||||
|
||||
std::mutex devices_mutex_;
|
||||
std::map<uint32_t, DeviceInfo> devices_;
|
||||
|
||||
std::mutex command_handle_mutex_;
|
||||
std::map<uint8_t, std::shared_ptr<CommandHandler>> lidars_command_handler_;
|
||||
|
||||
std::mutex commands_mutex_;
|
||||
std::map<uint32_t, std::pair<Command, TimePoint> > commands_;
|
||||
|
||||
LivoxLidarInfoChangeCallback livox_lidar_info_change_cb_;
|
||||
void* livox_lidar_info_change_client_data_;
|
||||
|
||||
LivoxLidarInfoCallback livox_lidar_info_cb_;
|
||||
void* livox_lidar_info_client_data_;
|
||||
|
||||
LivoxLidarCmdObserverCallBack cmd_observer_cb_{nullptr};
|
||||
void* cmd_observer_client_data_{nullptr};
|
||||
|
||||
std::string detection_host_ip_;
|
||||
bool is_view_;
|
||||
};
|
||||
|
||||
} // namespace livox
|
||||
} // namespace lidar
|
||||
|
||||
#endif // GENERAL_COMMAND_HANDLER_H_
|
||||
@@ -0,0 +1,292 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#include "hap_command_handler.h"
|
||||
#include "livox_lidar_def.h"
|
||||
|
||||
#include "base/command_callback.h"
|
||||
|
||||
#include "base/logging.h"
|
||||
#include "comm/protocol.h"
|
||||
#include "comm/generate_seq.h"
|
||||
|
||||
#include "build_request.h"
|
||||
#include "general_command_handler.h"
|
||||
|
||||
#include "parse_lidar_state_info.h"
|
||||
|
||||
namespace livox {
|
||||
namespace lidar {
|
||||
|
||||
HapCommandHandler::HapCommandHandler(DeviceManager* device_manager)
|
||||
: CommandHandler(device_manager),
|
||||
comm_port_(new CommPort),
|
||||
is_view_(false) {
|
||||
}
|
||||
|
||||
bool HapCommandHandler::Init(bool is_view) {
|
||||
is_view_ = is_view;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool HapCommandHandler::Init(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map) {
|
||||
for (const auto& it : custom_lidars_cfg_map) {
|
||||
if (it.second.device_type == kLivoxLidarTypeIndustrialHAP && custom_lidars_.find(it.first) == custom_lidars_.end()) {
|
||||
custom_lidars_[it.first] = it.second;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void HapCommandHandler::Handle(const uint32_t handle, uint16_t lidar_port, const Command& command) {
|
||||
if (command.packet.cmd_type == kCommandTypeAck) {
|
||||
LOG_INFO(" Receive Ack: Id {} Seq {}", command.packet.cmd_id, command.packet.seq_num);
|
||||
OnCommandAck(handle, command);
|
||||
} else if (command.packet.cmd_type == kCommandTypeCmd) {
|
||||
LOG_INFO(" Receive Command: Id {} Seq {}", command.packet.cmd_id, command.packet.seq_num);
|
||||
OnCommandCmd(handle, lidar_port, command);
|
||||
}
|
||||
}
|
||||
|
||||
void HapCommandHandler::OnCommandAck(uint32_t handle, const Command &command) {
|
||||
if (command.cb == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (command.packet.data == nullptr) {
|
||||
(*command.cb)(kLivoxLidarStatusTimeout, handle, command.packet.data);
|
||||
return;
|
||||
}
|
||||
|
||||
(*command.cb)(kLivoxLidarStatusSuccess, handle, command.packet.data);
|
||||
}
|
||||
|
||||
void HapCommandHandler::OnCommandCmd(const uint32_t handle, uint16_t lidar_port, const Command& command) {
|
||||
if (command.packet.cmd_id == kCommandIDLidarPushMsg && lidar_port == kHAPPushMsgPort) {
|
||||
std::string info;
|
||||
ParseLidarStateInfo::Parse(command.packet, info);
|
||||
GeneralCommandHandler::GetInstance().PushLivoxLidarInfo(handle, info);
|
||||
}
|
||||
}
|
||||
|
||||
void HapCommandHandler::UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(device_mutex_);
|
||||
if (devices_.find(view_lidar_info.handle) != devices_.end()) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
SetViewLidar(view_lidar_info);
|
||||
}
|
||||
|
||||
void HapCommandHandler::UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(device_mutex_);
|
||||
if (devices_.find(handle) != devices_.end()) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (custom_lidars_.find(handle) != custom_lidars_.end()) {
|
||||
const LivoxLidarCfg& lidar_cfg = custom_lidars_[handle];
|
||||
SetCustomLidar(handle, lidar_cmd_port, lidar_cfg);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void HapCommandHandler::SetViewLidar(const ViewLidarIpInfo& view_lidar_info) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
if (!BuildRequest::BuildUpdateViewLidarCfgRequest(view_lidar_info, req_buff, req_len)) {
|
||||
LOG_ERROR("Build update view lidar cfg request failed.");
|
||||
return;
|
||||
}
|
||||
|
||||
struct in_addr addr;
|
||||
addr.s_addr = view_lidar_info.handle;
|
||||
std::string lidar_ip = inet_ntoa(addr);
|
||||
uint16_t seq = GenerateSeq::GetSeq();
|
||||
Command command(seq, kCommandIDLidarWorkModeControl, kCommandTypeCmd, kHostSend, req_buff, req_len, view_lidar_info.handle,
|
||||
lidar_ip, MakeCommandCallback<LivoxLidarAsyncControlResponse>(HapCommandHandler::UpdateLidarCallback, this));
|
||||
SendCommand(command, view_lidar_info.lidar_cmd_port);
|
||||
}
|
||||
|
||||
void HapCommandHandler::SetCustomLidar(const uint32_t handle, const uint16_t lidar_cmd_port, const LivoxLidarCfg& lidar_cfg) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
if (!BuildRequest::BuildUpdateLidarCfgRequest(lidar_cfg, req_buff, req_len)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed.");
|
||||
return;
|
||||
}
|
||||
|
||||
struct in_addr addr;
|
||||
addr.s_addr = handle;
|
||||
std::string lidar_ip = inet_ntoa(addr);
|
||||
uint16_t seq = GenerateSeq::GetSeq();
|
||||
Command command(seq, kCommandIDLidarWorkModeControl, kCommandTypeCmd, kHostSend, req_buff, req_len, handle,
|
||||
lidar_ip, MakeCommandCallback<LivoxLidarAsyncControlResponse>(HapCommandHandler::UpdateLidarCallback, this));
|
||||
SendCommand(command, lidar_cmd_port);
|
||||
}
|
||||
|
||||
void HapCommandHandler::UpdateLidarCallback(livox_status status, uint32_t handle,
|
||||
LivoxLidarAsyncControlResponse *response, void *client_data) {
|
||||
if (status != kLivoxLidarStatusSuccess) {
|
||||
LOG_INFO("Update lidar failed, the status:{}", status);
|
||||
return;
|
||||
}
|
||||
|
||||
if (response == nullptr) {
|
||||
LOG_ERROR("Update lidar failed, the handle:{}, status:{}, response is nullptr.", handle, status);
|
||||
return;
|
||||
}
|
||||
|
||||
if (response->ret_code == 0 && response->error_key == 0) {
|
||||
if (client_data != nullptr) {
|
||||
HapCommandHandler* self = (HapCommandHandler*)client_data;
|
||||
self->AddDevice(handle);
|
||||
}
|
||||
LOG_INFO("Update lidar:{} succ.", handle);
|
||||
GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
|
||||
} else {
|
||||
GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
|
||||
LOG_ERROR("Update lidar failed, the ret_code:{}, error_key:{}", response->ret_code, response->error_key);
|
||||
}
|
||||
}
|
||||
|
||||
void HapCommandHandler::AddDevice(const uint32_t handle) {
|
||||
std::lock_guard<std::mutex> lock(device_mutex_);
|
||||
devices_.insert(handle);
|
||||
}
|
||||
|
||||
bool HapCommandHandler::IsStatusException(const Command &command) {
|
||||
if (!command.packet.data) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (command.packet.cmd_id != kCommandIDLidarWorkModeControl) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LivoxLidarAsyncControlResponse* data = (LivoxLidarAsyncControlResponse*)(command.packet.data);
|
||||
if (data->ret_code != 0) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
livox_status HapCommandHandler::SendCommand(const Command &command, const uint16_t lidar_cmd_port) {
|
||||
if (command.packet.cmd_type == kCommandTypeAck) {
|
||||
return kLivoxLidarStatusFailure;
|
||||
}
|
||||
|
||||
GeneralCommandHandler::GetInstance().AddCommand(command);
|
||||
|
||||
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
|
||||
int size = 0;
|
||||
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
|
||||
|
||||
|
||||
struct sockaddr_in servaddr;
|
||||
servaddr.sin_family = AF_INET;
|
||||
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
|
||||
servaddr.sin_port = htons(lidar_cmd_port);
|
||||
|
||||
//LOG_INFO("HapCommandHandler::SendCommand seq:{}, lidar_ip:{}", command.packet.seq_num, command.lidar_ip.c_str());
|
||||
int byte_send = device_manager_->SendCommand(kLivoxLidarTypeIndustrialHAP, command.handle, buf, size, (const struct sockaddr *) &servaddr, sizeof(servaddr));
|
||||
if (byte_send < 0) {
|
||||
LOG_ERROR("Sent cmd to lidar failed, the send_byte:{}, cmd_id:{}, seq:{}, lidar_ip:{}",
|
||||
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
|
||||
if (command.cb) {
|
||||
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
|
||||
}
|
||||
return kLivoxLidarStatusSendFailed;
|
||||
}
|
||||
return kLivoxLidarStatusSuccess;
|
||||
}
|
||||
|
||||
livox_status HapCommandHandler::SendCommand(const Command &command) {
|
||||
if (command.packet.cmd_type == kCommandTypeAck) {
|
||||
return kLivoxLidarStatusFailure;
|
||||
}
|
||||
|
||||
GeneralCommandHandler::GetInstance().AddCommand(command);
|
||||
|
||||
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
|
||||
int size = 0;
|
||||
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
|
||||
|
||||
struct sockaddr_in servaddr;
|
||||
servaddr.sin_family = AF_INET;
|
||||
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
|
||||
servaddr.sin_port = htons(kHAPCmdPort);
|
||||
|
||||
//LOG_INFO("HapCommandHandler::SendCommand seq:{}, lidar_ip:{}", command.packet.seq_num, command.lidar_ip.c_str());
|
||||
int byte_send = device_manager_->SendCommand(kLivoxLidarTypeIndustrialHAP, command.handle, buf, size, (const struct sockaddr *) &servaddr, sizeof(servaddr));
|
||||
if (byte_send < 0) {
|
||||
LOG_ERROR("Sent cmd to lidar failed, the send_byte:{}, cmd_id:{}, seq:{}, lidar_ip:{}",
|
||||
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
|
||||
if (command.cb) {
|
||||
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
|
||||
}
|
||||
return kLivoxLidarStatusSendFailed;
|
||||
}
|
||||
return kLivoxLidarStatusSuccess;
|
||||
}
|
||||
|
||||
livox_status HapCommandHandler::SendLoggerCommand(const Command &command) {
|
||||
if (command.packet.cmd_type == kCommandTypeAck) {
|
||||
return kLivoxLidarStatusFailure;
|
||||
}
|
||||
|
||||
GeneralCommandHandler::GetInstance().AddCommand(command);
|
||||
|
||||
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
|
||||
int size = 0;
|
||||
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
|
||||
|
||||
struct sockaddr_in servaddr;
|
||||
servaddr.sin_family = AF_INET;
|
||||
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
|
||||
servaddr.sin_port = htons(kHAPLogPort);
|
||||
|
||||
//LOG_INFO("HapCommandHandler::SendCommand seq:{}, lidar_ip:{}", command.packet.seq_num, command.lidar_ip.c_str());
|
||||
int byte_send = device_manager_->SendLoggerCommand(kLivoxLidarTypeIndustrialHAP, command.handle, buf, size, (const struct sockaddr *) &servaddr, sizeof(servaddr));
|
||||
if (byte_send < 0) {
|
||||
LOG_ERROR("Sent cmd to lidar failed, the send_byte:{}, cmd_id:{}, seq:{}, lidar_ip:{}",
|
||||
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
|
||||
if (command.cb) {
|
||||
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
|
||||
}
|
||||
return kLivoxLidarStatusSendFailed;
|
||||
}
|
||||
return kLivoxLidarStatusSuccess;
|
||||
}
|
||||
|
||||
bool HapCommandHandler::GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port) {
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace livox
|
||||
} // namespace lidar
|
||||
|
||||
@@ -0,0 +1,91 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#ifndef HAP_COMMAND_HANDLER_H_
|
||||
#define HAP_COMMAND_HANDLER_H_
|
||||
|
||||
#include <memory>
|
||||
#include <map>
|
||||
#include <list>
|
||||
#include <mutex>
|
||||
|
||||
#include "base/command_callback.h"
|
||||
#include "base/io_thread.h"
|
||||
|
||||
#include "comm/protocol.h"
|
||||
#include "comm/comm_port.h"
|
||||
#include "comm/define.h"
|
||||
|
||||
#include "livox_lidar_def.h"
|
||||
#include "device_manager.h"
|
||||
|
||||
#include "command_handler.h"
|
||||
|
||||
namespace livox {
|
||||
namespace lidar {
|
||||
|
||||
class HapCommandHandler : public CommandHandler {
|
||||
public:
|
||||
HapCommandHandler(DeviceManager* device_manager);
|
||||
~HapCommandHandler() {}
|
||||
static HapCommandHandler& GetInstance();
|
||||
virtual bool Init(bool is_view);
|
||||
|
||||
virtual bool Init(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map);
|
||||
virtual void Handle(const uint32_t handle, uint16_t lidar_port, const Command& command);
|
||||
virtual void UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info);
|
||||
virtual void UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port);
|
||||
virtual livox_status SendCommand(const Command& command);
|
||||
virtual livox_status SendLoggerCommand(const Command &command);
|
||||
|
||||
static void UpdateLidarCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data);
|
||||
void AddDevice(const uint32_t handle);
|
||||
private:
|
||||
void SetCustomLidar(const uint32_t handle, const uint16_t lidar_cmd_port, const LivoxLidarCfg& lidar_cfg);
|
||||
void SetViewLidar(const ViewLidarIpInfo& view_lidar_info);
|
||||
livox_status SendCommand(const Command &command, const uint16_t lidar_cmd_port);
|
||||
|
||||
|
||||
bool GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port);
|
||||
|
||||
void CommandsHandle(TimePoint now);
|
||||
|
||||
void OnCommand(uint32_t handle, const Command &command);
|
||||
void OnCommandAck(uint32_t handle, const Command &command);
|
||||
void OnCommandCmd(const uint32_t handle, const uint16_t lidar_port, const Command &command);
|
||||
bool IsStatusException(const Command &command);
|
||||
void QueryDiagnosisInfo(uint32_t handle);
|
||||
void OnLidarInfoChange(const Command &command);
|
||||
private:
|
||||
std::unique_ptr<CommPort> comm_port_;
|
||||
std::mutex device_mutex_;
|
||||
std::set<uint32_t> devices_;
|
||||
std::map<uint32_t, LivoxLidarCfg> custom_lidars_;
|
||||
bool is_view_;
|
||||
};
|
||||
|
||||
} // namespace livox
|
||||
} // namespace lidar
|
||||
|
||||
#endif // HAP_COMMAND_HANDLER_H_
|
||||
@@ -0,0 +1,287 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#include "mid360_command_handler.h"
|
||||
#include "livox_lidar_def.h"
|
||||
|
||||
#include "base/command_callback.h"
|
||||
|
||||
#include "base/logging.h"
|
||||
#include "comm/protocol.h"
|
||||
#include "comm/generate_seq.h"
|
||||
|
||||
#include "build_request.h"
|
||||
#include "general_command_handler.h"
|
||||
|
||||
#include "parse_lidar_state_info.h"
|
||||
|
||||
namespace livox {
|
||||
namespace lidar {
|
||||
|
||||
Mid360CommandHandler::Mid360CommandHandler(DeviceManager* device_manager)
|
||||
: CommandHandler(device_manager),
|
||||
comm_port_(new CommPort),
|
||||
is_view_(false) {
|
||||
}
|
||||
|
||||
bool Mid360CommandHandler::Init(bool is_view) {
|
||||
is_view_ = is_view;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Mid360CommandHandler::Init(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map) {
|
||||
for (const auto& it : custom_lidars_cfg_map) {
|
||||
if (it.second.device_type == kLivoxLidarTypeMid360 && custom_lidars_.find(it.first) == custom_lidars_.end()) {
|
||||
custom_lidars_[it.first] = it.second;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void Mid360CommandHandler::Handle(const uint32_t handle, uint16_t lidar_port, const Command& command) {
|
||||
if (command.packet.cmd_type == kCommandTypeAck) {
|
||||
LOG_INFO(" Receive Ack: Id {} Seq {}", command.packet.cmd_id, command.packet.seq_num);
|
||||
OnCommandAck(handle, command);
|
||||
} else if (command.packet.cmd_type == kCommandTypeCmd) {
|
||||
LOG_INFO(" Receive Command: Id {} Seq {}", command.packet.cmd_id, command.packet.seq_num);
|
||||
OnCommandCmd(handle, lidar_port, command);
|
||||
}
|
||||
}
|
||||
|
||||
void Mid360CommandHandler::OnCommandAck(uint32_t handle, const Command &command) {
|
||||
if (command.cb == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (command.packet.data == nullptr) {
|
||||
(*command.cb)(kLivoxLidarStatusTimeout, handle, command.packet.data);
|
||||
return;
|
||||
}
|
||||
|
||||
(*command.cb)(kLivoxLidarStatusSuccess, handle, command.packet.data);
|
||||
}
|
||||
|
||||
void Mid360CommandHandler::OnCommandCmd(uint32_t handle, const uint16_t lidar_port, const Command &command) {
|
||||
if (command.packet.cmd_id == kCommandIDLidarPushMsg && lidar_port == kMid360LidarPushMsgPort) {
|
||||
std::string info;
|
||||
ParseLidarStateInfo::Parse(command.packet, info);
|
||||
GeneralCommandHandler::GetInstance().PushLivoxLidarInfo(handle, info);
|
||||
}
|
||||
}
|
||||
|
||||
void Mid360CommandHandler::UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(device_mutex_);
|
||||
if (devices_.find(view_lidar_info.handle) != devices_.end()) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
SetViewLidar(view_lidar_info);
|
||||
}
|
||||
|
||||
void Mid360CommandHandler::UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(device_mutex_);
|
||||
if (devices_.find(handle) != devices_.end()) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (custom_lidars_.find(handle) != custom_lidars_.end()) {
|
||||
const LivoxLidarCfg& lidar_cfg = custom_lidars_[handle];
|
||||
SetCustomLidar(handle, lidar_cmd_port, lidar_cfg);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void Mid360CommandHandler::SetViewLidar(const ViewLidarIpInfo& view_lidar_info) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
if (!BuildRequest::BuildUpdateViewLidarCfgRequest(view_lidar_info, req_buff, req_len)) {
|
||||
LOG_ERROR("Build update view lidar cfg request failed.");
|
||||
return;
|
||||
}
|
||||
|
||||
struct in_addr addr;
|
||||
addr.s_addr = view_lidar_info.handle;
|
||||
std::string lidar_ip = inet_ntoa(addr);
|
||||
uint16_t seq = GenerateSeq::GetSeq();
|
||||
Command command(seq, kCommandIDLidarWorkModeControl, kCommandTypeCmd, kHostSend, req_buff, req_len, view_lidar_info.handle,
|
||||
lidar_ip, MakeCommandCallback<LivoxLidarAsyncControlResponse>(Mid360CommandHandler::UpdateLidarCallback, this));
|
||||
SendCommand(command, view_lidar_info.lidar_cmd_port);
|
||||
}
|
||||
|
||||
void Mid360CommandHandler::SetCustomLidar(const uint32_t handle, const uint16_t lidar_cmd_port, const LivoxLidarCfg& lidar_cfg) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
if (!BuildRequest::BuildUpdateMid360LidarCfgRequest(lidar_cfg, req_buff, req_len)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed.");
|
||||
return;
|
||||
}
|
||||
|
||||
struct in_addr addr;
|
||||
addr.s_addr = handle;
|
||||
std::string lidar_ip = inet_ntoa(addr);
|
||||
uint16_t seq = GenerateSeq::GetSeq();
|
||||
Command command(seq, kCommandIDLidarWorkModeControl, kCommandTypeCmd, kHostSend, req_buff, req_len, handle,
|
||||
lidar_ip, MakeCommandCallback<LivoxLidarAsyncControlResponse>(Mid360CommandHandler::UpdateLidarCallback, this));
|
||||
SendCommand(command, lidar_cmd_port);
|
||||
}
|
||||
|
||||
void Mid360CommandHandler::UpdateLidarCallback(livox_status status, uint32_t handle,
|
||||
LivoxLidarAsyncControlResponse *response, void *client_data) {
|
||||
if (status != kLivoxLidarStatusSuccess) {
|
||||
LOG_INFO("Update lidar failed, the status:{}", status);
|
||||
return;
|
||||
}
|
||||
|
||||
if (response == nullptr) {
|
||||
LOG_ERROR("Update lidar failed, the handle:{}, status:{}, response is nullptr.", handle, status);
|
||||
return;
|
||||
}
|
||||
|
||||
if (response->ret_code == 0 && response->error_key == 0) {
|
||||
if (client_data != nullptr) {
|
||||
Mid360CommandHandler* self = (Mid360CommandHandler*)client_data;
|
||||
self->AddDevice(handle);
|
||||
}
|
||||
LOG_INFO("Update lidar:{} succ.", handle);
|
||||
GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
|
||||
} else {
|
||||
//GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
|
||||
LOG_ERROR("Update lidar failed, the ret_code:{}, error_key:{}", response->ret_code, response->error_key);
|
||||
}
|
||||
}
|
||||
|
||||
void Mid360CommandHandler::AddDevice(const uint32_t handle) {
|
||||
std::lock_guard<std::mutex> lock(device_mutex_);
|
||||
devices_.insert(handle);
|
||||
}
|
||||
|
||||
bool Mid360CommandHandler::IsStatusException(const Command &command) {
|
||||
if (!command.packet.data) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (command.packet.cmd_id != kCommandIDLidarWorkModeControl) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LivoxLidarAsyncControlResponse* data = (LivoxLidarAsyncControlResponse*)(command.packet.data);
|
||||
if (data->ret_code != 0) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
livox_status Mid360CommandHandler::SendCommand(const Command &command, const uint16_t lidar_cmd_port) {
|
||||
if (command.packet.cmd_type == kCommandTypeAck) {
|
||||
return kLivoxLidarStatusFailure;
|
||||
}
|
||||
|
||||
GeneralCommandHandler::GetInstance().AddCommand(command);
|
||||
|
||||
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
|
||||
int size = 0;
|
||||
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
|
||||
|
||||
|
||||
struct sockaddr_in servaddr;
|
||||
servaddr.sin_family = AF_INET;
|
||||
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
|
||||
servaddr.sin_port = htons(lidar_cmd_port);
|
||||
|
||||
int byte_send = device_manager_->SendCommand(kLivoxLidarTypeMid360, command.handle, buf, size, (const struct sockaddr *) &servaddr, sizeof(servaddr));
|
||||
if (byte_send < 0) {
|
||||
LOG_ERROR("Sent cmd to lidar failed, the send_byte:{}, cmd_id:{}, seq:{}, lidar_ip:{}",
|
||||
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
|
||||
if (command.cb) {
|
||||
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
|
||||
}
|
||||
return kLivoxLidarStatusSendFailed;
|
||||
}
|
||||
return kLivoxLidarStatusSuccess;
|
||||
}
|
||||
|
||||
livox_status Mid360CommandHandler::SendCommand(const Command &command) {
|
||||
if (command.packet.cmd_type == kCommandTypeAck) {
|
||||
return kLivoxLidarStatusFailure;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
|
||||
int size = 0;
|
||||
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
|
||||
|
||||
struct sockaddr_in servaddr;
|
||||
servaddr.sin_family = AF_INET;
|
||||
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
|
||||
servaddr.sin_port = htons(kMid360LidarCmdPort);
|
||||
|
||||
int byte_send = device_manager_->SendCommand(kLivoxLidarTypeMid360, command.handle, buf, size, (const struct sockaddr *) &servaddr, sizeof(servaddr));
|
||||
if (byte_send < 0) {
|
||||
LOG_ERROR("Sent cmd to lidar failed, the send_byte:{}, cmd_id:{}, seq:{}, lidar_ip:{}",
|
||||
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
|
||||
if (command.cb) {
|
||||
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
|
||||
}
|
||||
return kLivoxLidarStatusSendFailed;
|
||||
}
|
||||
return kLivoxLidarStatusSuccess;
|
||||
}
|
||||
|
||||
livox_status Mid360CommandHandler::SendLoggerCommand(const Command &command) {
|
||||
if (command.packet.cmd_type == kCommandTypeAck) {
|
||||
return kLivoxLidarStatusFailure;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
|
||||
int size = 0;
|
||||
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
|
||||
|
||||
struct sockaddr_in servaddr;
|
||||
servaddr.sin_family = AF_INET;
|
||||
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
|
||||
servaddr.sin_port = htons(kMid360LidarLogPort);
|
||||
|
||||
int byte_send = device_manager_->SendLoggerCommand(kLivoxLidarTypeMid360, command.handle, buf, size, (const struct sockaddr *) &servaddr, sizeof(servaddr));
|
||||
if (byte_send < 0) {
|
||||
LOG_ERROR("Sent cmd to lidar failed, the send_byte:{}, cmd_id:{}, seq:{}, lidar_ip:{}",
|
||||
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
|
||||
if (command.cb) {
|
||||
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
|
||||
}
|
||||
return kLivoxLidarStatusSendFailed;
|
||||
}
|
||||
return kLivoxLidarStatusSuccess;
|
||||
}
|
||||
|
||||
|
||||
bool Mid360CommandHandler::GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port) {
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
} // namespace livox
|
||||
} // namespace lidar
|
||||
|
||||
@@ -0,0 +1,90 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#ifndef MID360_COMMAND_HANDLER_H_
|
||||
#define MID360_COMMAND_HANDLER_H_
|
||||
|
||||
#include <memory>
|
||||
#include <map>
|
||||
#include <list>
|
||||
#include <mutex>
|
||||
|
||||
#include "base/command_callback.h"
|
||||
#include "base/io_thread.h"
|
||||
|
||||
#include "comm/protocol.h"
|
||||
#include "comm/comm_port.h"
|
||||
#include "comm/define.h"
|
||||
|
||||
#include "livox_lidar_def.h"
|
||||
#include "device_manager.h"
|
||||
|
||||
#include "command_handler.h"
|
||||
|
||||
namespace livox {
|
||||
namespace lidar {
|
||||
|
||||
class Mid360CommandHandler : public CommandHandler {
|
||||
public:
|
||||
Mid360CommandHandler(DeviceManager* device_manager);
|
||||
~Mid360CommandHandler() {}
|
||||
virtual bool Init(bool is_view);
|
||||
|
||||
virtual bool Init(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map);
|
||||
virtual void Handle(const uint32_t handle, uint16_t lidar_port, const Command& command);
|
||||
virtual void UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info);
|
||||
virtual void UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port);
|
||||
virtual livox_status SendCommand(const Command& command);
|
||||
virtual livox_status SendLoggerCommand(const Command &command);
|
||||
|
||||
static void UpdateLidarCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data);
|
||||
void AddDevice(const uint32_t handle);
|
||||
private:
|
||||
void SetCustomLidar(const uint32_t handle, const uint16_t lidar_cmd_port, const LivoxLidarCfg& lidar_cfg);
|
||||
void SetViewLidar(const ViewLidarIpInfo& view_lidar_info);
|
||||
livox_status SendCommand(const Command &command, const uint16_t lidar_cmd_port);
|
||||
|
||||
|
||||
bool GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port);
|
||||
|
||||
void CommandsHandle(TimePoint now);
|
||||
|
||||
void OnCommand(uint32_t handle, const Command &command);
|
||||
void OnCommandAck(const uint32_t handle, const Command &command);
|
||||
void OnCommandCmd(const uint32_t handle, const uint16_t lidar_port, const Command &command);
|
||||
bool IsStatusException(const Command &command);
|
||||
void QueryDiagnosisInfo(uint32_t handle);
|
||||
void OnLidarInfoChange(const Command &command);
|
||||
private:
|
||||
std::unique_ptr<CommPort> comm_port_;
|
||||
std::mutex device_mutex_;
|
||||
std::set<uint32_t> devices_;
|
||||
std::map<uint32_t, LivoxLidarCfg> custom_lidars_;
|
||||
bool is_view_;
|
||||
};
|
||||
|
||||
} // namespace livox
|
||||
} // namespace lidar
|
||||
|
||||
#endif // MID360_COMMAND_HANDLER_H_
|
||||
@@ -0,0 +1,287 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#include "mid360s_command_handler.h"
|
||||
#include "livox_lidar_def.h"
|
||||
|
||||
#include "base/command_callback.h"
|
||||
|
||||
#include "base/logging.h"
|
||||
#include "comm/protocol.h"
|
||||
#include "comm/generate_seq.h"
|
||||
|
||||
#include "build_request.h"
|
||||
#include "general_command_handler.h"
|
||||
|
||||
#include "parse_lidar_state_info.h"
|
||||
|
||||
namespace livox {
|
||||
namespace lidar {
|
||||
|
||||
Mid360sCommandHandler::Mid360sCommandHandler(DeviceManager* device_manager)
|
||||
: CommandHandler(device_manager),
|
||||
comm_port_(new CommPort),
|
||||
is_view_(false) {
|
||||
}
|
||||
|
||||
bool Mid360sCommandHandler::Init(bool is_view) {
|
||||
is_view_ = is_view;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Mid360sCommandHandler::Init(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map) {
|
||||
for (const auto& it : custom_lidars_cfg_map) {
|
||||
if (it.second.device_type == kLivoxLidarTypeMid360s && custom_lidars_.find(it.first) == custom_lidars_.end()) {
|
||||
custom_lidars_[it.first] = it.second;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void Mid360sCommandHandler::Handle(const uint32_t handle, uint16_t lidar_port, const Command& command) {
|
||||
if (command.packet.cmd_type == kCommandTypeAck) {
|
||||
LOG_INFO(" Receive Ack: Id {} Seq {}", command.packet.cmd_id, command.packet.seq_num);
|
||||
OnCommandAck(handle, command);
|
||||
} else if (command.packet.cmd_type == kCommandTypeCmd) {
|
||||
LOG_INFO(" Receive Command: Id {} Seq {}", command.packet.cmd_id, command.packet.seq_num);
|
||||
OnCommandCmd(handle, lidar_port, command);
|
||||
}
|
||||
}
|
||||
|
||||
void Mid360sCommandHandler::OnCommandAck(uint32_t handle, const Command &command) {
|
||||
if (command.cb == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (command.packet.data == nullptr) {
|
||||
(*command.cb)(kLivoxLidarStatusTimeout, handle, command.packet.data);
|
||||
return;
|
||||
}
|
||||
|
||||
(*command.cb)(kLivoxLidarStatusSuccess, handle, command.packet.data);
|
||||
}
|
||||
|
||||
void Mid360sCommandHandler::OnCommandCmd(uint32_t handle, const uint16_t lidar_port, const Command &command) {
|
||||
if (command.packet.cmd_id == kCommandIDLidarPushMsg && lidar_port == kMid360sLidarPushMsgPort) {
|
||||
std::string info;
|
||||
ParseLidarStateInfo::Parse(command.packet, info);
|
||||
GeneralCommandHandler::GetInstance().PushLivoxLidarInfo(handle, info);
|
||||
}
|
||||
}
|
||||
|
||||
void Mid360sCommandHandler::UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(device_mutex_);
|
||||
if (devices_.find(view_lidar_info.handle) != devices_.end()) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
SetViewLidar(view_lidar_info);
|
||||
}
|
||||
|
||||
void Mid360sCommandHandler::UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port) {
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(device_mutex_);
|
||||
if (devices_.find(handle) != devices_.end()) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (custom_lidars_.find(handle) != custom_lidars_.end()) {
|
||||
const LivoxLidarCfg& lidar_cfg = custom_lidars_[handle];
|
||||
SetCustomLidar(handle, lidar_cmd_port, lidar_cfg);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void Mid360sCommandHandler::SetViewLidar(const ViewLidarIpInfo& view_lidar_info) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
if (!BuildRequest::BuildUpdateViewLidarCfgRequest(view_lidar_info, req_buff, req_len)) {
|
||||
LOG_ERROR("Build update view lidar cfg request failed.");
|
||||
return;
|
||||
}
|
||||
|
||||
struct in_addr addr;
|
||||
addr.s_addr = view_lidar_info.handle;
|
||||
std::string lidar_ip = inet_ntoa(addr);
|
||||
uint16_t seq = GenerateSeq::GetSeq();
|
||||
Command command(seq, kCommandIDLidarWorkModeControl, kCommandTypeCmd, kHostSend, req_buff, req_len, view_lidar_info.handle,
|
||||
lidar_ip, MakeCommandCallback<LivoxLidarAsyncControlResponse>(Mid360sCommandHandler::UpdateLidarCallback, this));
|
||||
SendCommand(command, view_lidar_info.lidar_cmd_port);
|
||||
}
|
||||
|
||||
void Mid360sCommandHandler::SetCustomLidar(const uint32_t handle, const uint16_t lidar_cmd_port, const LivoxLidarCfg& lidar_cfg) {
|
||||
uint8_t req_buff[kMaxCommandBufferSize] = {0};
|
||||
uint16_t req_len = 0;
|
||||
if (!BuildRequest::BuildUpdateMid360LidarCfgRequest(lidar_cfg, req_buff, req_len)) {
|
||||
LOG_ERROR("Build update lidar cfg request failed.");
|
||||
return;
|
||||
}
|
||||
|
||||
struct in_addr addr;
|
||||
addr.s_addr = handle;
|
||||
std::string lidar_ip = inet_ntoa(addr);
|
||||
uint16_t seq = GenerateSeq::GetSeq();
|
||||
Command command(seq, kCommandIDLidarWorkModeControl, kCommandTypeCmd, kHostSend, req_buff, req_len, handle,
|
||||
lidar_ip, MakeCommandCallback<LivoxLidarAsyncControlResponse>(Mid360sCommandHandler::UpdateLidarCallback, this));
|
||||
SendCommand(command, lidar_cmd_port);
|
||||
}
|
||||
|
||||
void Mid360sCommandHandler::UpdateLidarCallback(livox_status status, uint32_t handle,
|
||||
LivoxLidarAsyncControlResponse *response, void *client_data) {
|
||||
if (status != kLivoxLidarStatusSuccess) {
|
||||
LOG_INFO("Update lidar failed, the status:{}", status);
|
||||
return;
|
||||
}
|
||||
|
||||
if (response == nullptr) {
|
||||
LOG_ERROR("Update lidar failed, the handle:{}, status:{}, response is nullptr.", handle, status);
|
||||
return;
|
||||
}
|
||||
|
||||
if (response->ret_code == 0 && response->error_key == 0) {
|
||||
if (client_data != nullptr) {
|
||||
Mid360sCommandHandler* self = (Mid360sCommandHandler*)client_data;
|
||||
self->AddDevice(handle);
|
||||
}
|
||||
LOG_INFO("Update lidar:{} succ.", handle);
|
||||
GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
|
||||
} else {
|
||||
//GeneralCommandHandler::GetInstance().LivoxLidarInfoChange(handle);
|
||||
LOG_ERROR("Update lidar failed, the ret_code:{}, error_key:{}", response->ret_code, response->error_key);
|
||||
}
|
||||
}
|
||||
|
||||
void Mid360sCommandHandler::AddDevice(const uint32_t handle) {
|
||||
std::lock_guard<std::mutex> lock(device_mutex_);
|
||||
devices_.insert(handle);
|
||||
}
|
||||
|
||||
bool Mid360sCommandHandler::IsStatusException(const Command &command) {
|
||||
if (!command.packet.data) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (command.packet.cmd_id != kCommandIDLidarWorkModeControl) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LivoxLidarAsyncControlResponse* data = (LivoxLidarAsyncControlResponse*)(command.packet.data);
|
||||
if (data->ret_code != 0) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
livox_status Mid360sCommandHandler::SendCommand(const Command &command, const uint16_t lidar_cmd_port) {
|
||||
if (command.packet.cmd_type == kCommandTypeAck) {
|
||||
return kLivoxLidarStatusFailure;
|
||||
}
|
||||
|
||||
GeneralCommandHandler::GetInstance().AddCommand(command);
|
||||
|
||||
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
|
||||
int size = 0;
|
||||
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
|
||||
|
||||
|
||||
struct sockaddr_in servaddr;
|
||||
servaddr.sin_family = AF_INET;
|
||||
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
|
||||
servaddr.sin_port = htons(lidar_cmd_port);
|
||||
|
||||
int byte_send = device_manager_->SendCommand(kLivoxLidarTypeMid360s, command.handle, buf, size, (const struct sockaddr *) &servaddr, sizeof(servaddr));
|
||||
if (byte_send < 0) {
|
||||
LOG_ERROR("Sent cmd to lidar failed, the send_byte:{}, cmd_id:{}, seq:{}, lidar_ip:{}",
|
||||
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
|
||||
if (command.cb) {
|
||||
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
|
||||
}
|
||||
return kLivoxLidarStatusSendFailed;
|
||||
}
|
||||
return kLivoxLidarStatusSuccess;
|
||||
}
|
||||
|
||||
livox_status Mid360sCommandHandler::SendCommand(const Command &command) {
|
||||
if (command.packet.cmd_type == kCommandTypeAck) {
|
||||
return kLivoxLidarStatusFailure;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
|
||||
int size = 0;
|
||||
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
|
||||
|
||||
struct sockaddr_in servaddr;
|
||||
servaddr.sin_family = AF_INET;
|
||||
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
|
||||
servaddr.sin_port = htons(kMid360sLidarCmdPort);
|
||||
|
||||
int byte_send = device_manager_->SendCommand(kLivoxLidarTypeMid360s, command.handle, buf, size, (const struct sockaddr *) &servaddr, sizeof(servaddr));
|
||||
if (byte_send < 0) {
|
||||
LOG_ERROR("Sent cmd to lidar failed, the send_byte:{}, cmd_id:{}, seq:{}, lidar_ip:{}",
|
||||
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
|
||||
if (command.cb) {
|
||||
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
|
||||
}
|
||||
return kLivoxLidarStatusSendFailed;
|
||||
}
|
||||
return kLivoxLidarStatusSuccess;
|
||||
}
|
||||
|
||||
livox_status Mid360sCommandHandler::SendLoggerCommand(const Command &command) {
|
||||
if (command.packet.cmd_type == kCommandTypeAck) {
|
||||
return kLivoxLidarStatusFailure;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> buf(kMaxCommandBufferSize + 1);
|
||||
int size = 0;
|
||||
comm_port_->Pack(buf.data(), kMaxCommandBufferSize, (uint32_t *)&size, command.packet);
|
||||
|
||||
struct sockaddr_in servaddr;
|
||||
servaddr.sin_family = AF_INET;
|
||||
servaddr.sin_addr.s_addr = inet_addr(command.lidar_ip.c_str());
|
||||
servaddr.sin_port = htons(kMid360sLidarLogPort);
|
||||
|
||||
int byte_send = device_manager_->SendLoggerCommand(kLivoxLidarTypeMid360s, command.handle, buf, size, (const struct sockaddr *) &servaddr, sizeof(servaddr));
|
||||
if (byte_send < 0) {
|
||||
LOG_ERROR("Sent cmd to lidar failed, the send_byte:{}, cmd_id:{}, seq:{}, lidar_ip:{}",
|
||||
byte_send, command.packet.cmd_id, command.packet.seq_num, command.lidar_ip.c_str());
|
||||
if (command.cb) {
|
||||
(*command.cb)(kLivoxLidarStatusSendFailed, command.handle, nullptr);
|
||||
}
|
||||
return kLivoxLidarStatusSendFailed;
|
||||
}
|
||||
return kLivoxLidarStatusSuccess;
|
||||
}
|
||||
|
||||
|
||||
bool Mid360sCommandHandler::GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port) {
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
} // namespace livox
|
||||
} // namespace lidar
|
||||
|
||||
@@ -0,0 +1,90 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#ifndef MID360S_COMMAND_HANDLER_H_
|
||||
#define MID360S_COMMAND_HANDLER_H_
|
||||
|
||||
#include <memory>
|
||||
#include <map>
|
||||
#include <list>
|
||||
#include <mutex>
|
||||
|
||||
#include "base/command_callback.h"
|
||||
#include "base/io_thread.h"
|
||||
|
||||
#include "comm/protocol.h"
|
||||
#include "comm/comm_port.h"
|
||||
#include "comm/define.h"
|
||||
|
||||
#include "livox_lidar_def.h"
|
||||
#include "device_manager.h"
|
||||
|
||||
#include "command_handler.h"
|
||||
|
||||
namespace livox {
|
||||
namespace lidar {
|
||||
|
||||
class Mid360sCommandHandler : public CommandHandler {
|
||||
public:
|
||||
Mid360sCommandHandler(DeviceManager* device_manager);
|
||||
~Mid360sCommandHandler() {}
|
||||
virtual bool Init(bool is_view);
|
||||
|
||||
virtual bool Init(const std::map<uint32_t, LivoxLidarCfg>& custom_lidars_cfg_map);
|
||||
virtual void Handle(const uint32_t handle, uint16_t lidar_port, const Command& command);
|
||||
virtual void UpdateLidarCfg(const ViewLidarIpInfo& view_lidar_info);
|
||||
virtual void UpdateLidarCfg(const uint32_t handle, const uint16_t lidar_cmd_port);
|
||||
virtual livox_status SendCommand(const Command& command);
|
||||
virtual livox_status SendLoggerCommand(const Command &command);
|
||||
|
||||
static void UpdateLidarCallback(livox_status status, uint32_t handle, LivoxLidarAsyncControlResponse *response, void *client_data);
|
||||
void AddDevice(const uint32_t handle);
|
||||
private:
|
||||
void SetCustomLidar(const uint32_t handle, const uint16_t lidar_cmd_port, const LivoxLidarCfg& lidar_cfg);
|
||||
void SetViewLidar(const ViewLidarIpInfo& view_lidar_info);
|
||||
livox_status SendCommand(const Command &command, const uint16_t lidar_cmd_port);
|
||||
|
||||
|
||||
bool GetHostInfo(const uint32_t handle, std::string& host_ip, uint16_t& cmd_port);
|
||||
|
||||
void CommandsHandle(TimePoint now);
|
||||
|
||||
void OnCommand(uint32_t handle, const Command &command);
|
||||
void OnCommandAck(const uint32_t handle, const Command &command);
|
||||
void OnCommandCmd(const uint32_t handle, const uint16_t lidar_port, const Command &command);
|
||||
bool IsStatusException(const Command &command);
|
||||
void QueryDiagnosisInfo(uint32_t handle);
|
||||
void OnLidarInfoChange(const Command &command);
|
||||
private:
|
||||
std::unique_ptr<CommPort> comm_port_;
|
||||
std::mutex device_mutex_;
|
||||
std::set<uint32_t> devices_;
|
||||
std::map<uint32_t, LivoxLidarCfg> custom_lidars_;
|
||||
bool is_view_;
|
||||
};
|
||||
|
||||
} // namespace livox
|
||||
} // namespace lidar
|
||||
|
||||
#endif // MID360S_COMMAND_HANDLER_H_
|
||||
@@ -0,0 +1,725 @@
|
||||
|
||||
#include "parse_lidar_state_info.h"
|
||||
#include "base/logging.h"
|
||||
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
|
||||
namespace livox {
|
||||
|
||||
namespace lidar {
|
||||
|
||||
bool ParseLidarStateInfo::Parse(const CommPacket& packet, std::string& info_str) {
|
||||
DirectLidarStateInfo info;
|
||||
std::set<ParamKeyName> key_mask;
|
||||
|
||||
if (!ParseStateInfo(packet, info, key_mask)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LivoxLidarStateInfoToJson(info, key_mask, info_str);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ParseLidarStateInfo::ParseStateInfo(const CommPacket& packet,
|
||||
DirectLidarStateInfo& info,
|
||||
std::set<ParamKeyName>& key_mask) {
|
||||
uint16_t offset = 0;
|
||||
uint16_t key_num = 0;
|
||||
memcpy(&key_num, &packet.data[offset], sizeof(uint16_t));
|
||||
offset += sizeof(uint16_t) * 2;
|
||||
|
||||
for (uint16_t i = 0; i < key_num; ++i) {
|
||||
if (offset + sizeof(LivoxLidarKeyValueParam) > packet.data_len) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LivoxLidarKeyValueParam* kv = (LivoxLidarKeyValueParam*)&packet.data[offset];
|
||||
offset += sizeof(uint16_t);
|
||||
|
||||
uint16_t val_len = 0;
|
||||
memcpy(&val_len, &packet.data[offset], sizeof(uint16_t));
|
||||
offset += sizeof(uint16_t);
|
||||
|
||||
switch (kv->key) {
|
||||
case static_cast<uint16_t>(kKeyPclDataType) :
|
||||
key_mask.insert(kKeyPclDataType);
|
||||
memcpy(&info.pcl_data_type, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyPatternMode) :
|
||||
key_mask.insert(kKeyPatternMode);
|
||||
memcpy(&info.pattern_mode, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyDualEmitEn) :
|
||||
key_mask.insert(kKeyDualEmitEn);
|
||||
memcpy(&info.dual_emit_en, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyPointSendEn) :
|
||||
key_mask.insert(kKeyPointSendEn);
|
||||
memcpy(&info.point_send_en, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyLidarIpCfg) :
|
||||
key_mask.insert(kKeyLidarIpCfg);
|
||||
ParseLidarIpAddr(packet, offset, info);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyStateInfoHostIpCfg) :
|
||||
key_mask.insert(kKeyStateInfoHostIpCfg);
|
||||
ParseStateInfoHostIPCfg(packet, offset, info);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyLidarPointDataHostIpCfg) :
|
||||
key_mask.insert(kKeyLidarPointDataHostIpCfg);
|
||||
ParsePointCloudHostIpCfg(packet, offset, info);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyLidarImuHostIpCfg) :
|
||||
key_mask.insert(kKeyLidarImuHostIpCfg);
|
||||
ParseImuDataHostIpCfg(packet, offset, info);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyCtlHostIpCfg) :
|
||||
key_mask.insert(kKeyCtlHostIpCfg);
|
||||
ParseIpCfg(packet, offset, info.ctl_host_ipcfg);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyLogHostIpCfg) :
|
||||
key_mask.insert(kKeyLogHostIpCfg);
|
||||
ParseIpCfg(packet, offset, info.log_host_ipcfg);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyVehicleSpeed) :
|
||||
key_mask.insert(kKeyVehicleSpeed);
|
||||
memcpy(&info.vehicle_speed, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyEnvironmentTemp) :
|
||||
key_mask.insert(kKeyEnvironmentTemp);
|
||||
memcpy(&info.environment_temp, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyInstallAttitude) :
|
||||
key_mask.insert(kKeyInstallAttitude);
|
||||
memcpy(&info.install_attitude, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyBlindSpotSet) :
|
||||
key_mask.insert(kKeyBlindSpotSet);
|
||||
memcpy(&info.blind_spot_set, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyFrameRate) :
|
||||
key_mask.insert(kKeyFrameRate);
|
||||
memcpy(&info.frame_rate, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyFovCfg0) :
|
||||
key_mask.insert(kKeyFovCfg0);
|
||||
memcpy(&info.fov_cfg0, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyFovCfg1) :
|
||||
key_mask.insert(kKeyFovCfg1);
|
||||
memcpy(&info.fov_cfg1, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyFovCfgEn) :
|
||||
key_mask.insert(kKeyFovCfgEn);
|
||||
memcpy(&info.fov_cfg_en, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyDetectMode) :
|
||||
key_mask.insert(kKeyDetectMode);
|
||||
memcpy(&info.detect_mode, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyFuncIoCfg) :
|
||||
key_mask.insert(kKeyFuncIoCfg);
|
||||
memcpy(&info.func_io_cfg, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyWorkMode) :
|
||||
key_mask.insert(kKeyWorkMode);
|
||||
memcpy(&info.work_tgt_mode, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyGlassHeat) :
|
||||
key_mask.insert(kKeyGlassHeat);
|
||||
memcpy(&info.glass_heat, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyImuDataEn) :
|
||||
key_mask.insert(kKeyImuDataEn);
|
||||
memcpy(&info.imu_data_en, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyFusaEn) :
|
||||
key_mask.insert(kKeyFusaEn);
|
||||
memcpy(&info.fusa_en, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeySn) :
|
||||
key_mask.insert(kKeySn);
|
||||
memcpy(info.sn, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyProductInfo) :
|
||||
key_mask.insert(kKeyProductInfo);
|
||||
memcpy(info.product_info, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyVersionApp) :
|
||||
key_mask.insert(kKeyVersionApp);
|
||||
memcpy(info.version_app, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyVersionLoader) :
|
||||
key_mask.insert(kKeyVersionLoader);
|
||||
memcpy(info.version_loader, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyVersionHardware):
|
||||
key_mask.insert(kKeyVersionHardware);
|
||||
memcpy(info.version_hardware, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyMac) :
|
||||
key_mask.insert(kKeyMac);
|
||||
memcpy(info.mac, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyCurWorkState) :
|
||||
key_mask.insert(kKeyCurWorkState);
|
||||
memcpy(&info.cur_work_state, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyCoreTemp) :
|
||||
key_mask.insert(kKeyCoreTemp);
|
||||
memcpy(&info.core_temp, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyPowerUpCnt) :
|
||||
key_mask.insert(kKeyPowerUpCnt);
|
||||
memcpy(&info.powerup_cnt, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyLocalTimeNow) :
|
||||
key_mask.insert(kKeyLocalTimeNow);
|
||||
memcpy(&info.local_time_now, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyLastSyncTime) :
|
||||
key_mask.insert(kKeyLastSyncTime);
|
||||
memcpy(&info.last_sync_time, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyTimeOffset) :
|
||||
key_mask.insert(kKeyTimeOffset);
|
||||
memcpy(&info.time_offset, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyTimeSyncType) :
|
||||
key_mask.insert(kKeyTimeSyncType);
|
||||
memcpy(&info.time_sync_type, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyStatusCode) :
|
||||
key_mask.insert(kKeyStatusCode);
|
||||
memcpy(&info.status_code, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyLidarDiagStatus) :
|
||||
key_mask.insert(kKeyLidarDiagStatus);
|
||||
memcpy(&info.lidar_diag_status, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyLidarFlashStatus) :
|
||||
key_mask.insert(kKeyLidarFlashStatus);
|
||||
memcpy(&info.lidar_flash_status, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyFwType) :
|
||||
key_mask.insert(kKeyFwType);
|
||||
memcpy(&info.fw_type, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyHmsCode) :
|
||||
key_mask.insert(kKeyHmsCode);
|
||||
memcpy(&info.hms_code, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeyRoiMode) :
|
||||
key_mask.insert(kKeyRoiMode);
|
||||
memcpy(&info.ROI_Mode, &packet.data[offset], val_len);
|
||||
break;
|
||||
case static_cast<uint16_t>(kKeySetEscMode) :
|
||||
key_mask.insert(kKeySetEscMode);
|
||||
memcpy(&info.esc_mode, &packet.data[offset], val_len);
|
||||
break;
|
||||
default :
|
||||
break;
|
||||
}
|
||||
offset += val_len;
|
||||
}
|
||||
|
||||
// printf("Lidar state info, pcl_data_type:%d, pattern_mode:%d, lidar_ip:%s, lidar_submask:%s, lidar_gatway:%s.\n",
|
||||
// info.pcl_data_type, info.pattern_mode, info.lidar_ip_info.ip_addr, info.lidar_ip_info.net_mask,
|
||||
// info.lidar_ip_info.gw_addr);
|
||||
|
||||
// printf("Lidar state info, host_ip_addr:%s, host_state_info_port:%u, lidar_state_info_port:%u.\n",
|
||||
// info.host_state_info.host_ip_addr, info.host_state_info.host_state_info_port, info.host_state_info.lidar_state_info_port);
|
||||
|
||||
// printf("Lidar state info, host_ip_addr:%s, host_point_data_port:%u, lidar_point_data_port:%u.\n",
|
||||
// info.pointcloud_host_ipcfg.host_ip_addr, info.pointcloud_host_ipcfg.host_point_data_port,
|
||||
// info.pointcloud_host_ipcfg.lidar_point_data_port);
|
||||
|
||||
// printf("Lidar state info, host_ip_addr:%s, host_imu_data_port:%u, lidar_imu_data_port:%u.\n",
|
||||
// info.imu_host_ipcfg.host_ip_addr, info.pointcloud_host_ipcfg.host_point_data_port,
|
||||
// info.imu_host_ipcfg.lidar_imu_data_port);
|
||||
|
||||
// printf("Lidar state info, roll:%f, pitch:%f, yaw:%f, x:%d, y:%d,z:%d.\n",
|
||||
// info.install_attitude.roll_deg, info.install_attitude.pitch_deg,
|
||||
// info.install_attitude.yaw_deg, info.install_attitude.x, info.install_attitude.y, info.install_attitude.z);
|
||||
|
||||
// printf("Lidar state info, fov cfg0, yaw_start:%d, yaw_stop:%d, pitch_start:%d, pitch_stop:%d.\n",
|
||||
// info.fov_cfg0.yaw_start, info.fov_cfg0.yaw_stop, info.fov_cfg0.pitch_start, info.fov_cfg0.pitch_stop);
|
||||
|
||||
// printf("Lidar state info, fov cfg1, yaw_start:%d, yaw_stop:%d, pitch_start:%d, pitch_stop:%d.\n",
|
||||
// info.fov_cfg1.yaw_start, info.fov_cfg1.yaw_stop, info.fov_cfg1.pitch_start, info.fov_cfg1.pitch_stop);
|
||||
|
||||
// printf("Lidar state info, fov_en:%u, work_mode:%u, imu_data_en:%u, sn:%s, product_info:%s.\n",
|
||||
// info.fov_en, info.work_mode, info.imu_data_en, info.sn, info.product_info);
|
||||
|
||||
// std::string version_app = std::to_string(info.version_app[0]) + ":" + std::to_string(info.version_app[1]) + ":" +
|
||||
// std::to_string(info.version_app[2]) + ":" + std::to_string(info.version_app[3]);
|
||||
|
||||
// std::string version_load = std::to_string(info.version_load[0]) + ":" + std::to_string(info.version_load[1]) + ":" +
|
||||
// std::to_string(info.version_load[2]) + ":" + std::to_string(info.version_load[3]);
|
||||
|
||||
// std::string version_hardware = std::to_string(info.version_hardware[0]) + ":" + std::to_string(info.version_hardware[1]) + ":" +
|
||||
// std::to_string(info.version_hardware[2]) + ":" + std::to_string(info.version_hardware[3]);
|
||||
|
||||
// std::string mac = std::to_string(info.mac[0]) + ":" + std::to_string(info.mac[1]) + ":" +
|
||||
// std::to_string(info.mac[2]) + ":" + std::to_string(info.mac[3]) + ":" +
|
||||
// std::to_string(info.mac[4]) + ":" + std::to_string(info.mac[5]);
|
||||
|
||||
// printf("Lidar state info, version_app:%s, version_load:%s, version_hardware:%s, mac:%s.\n",
|
||||
// version_app.c_str(), version_load.c_str(), version_hardware.c_str(), mac.c_str());
|
||||
|
||||
|
||||
// printf("Lidar state info, cur_work_state:%u, core_temp:%d, powerup_cnt:%u, local_time_now:%lu, last_sync_time:%lu, time_offset:%ld.\n",
|
||||
// info.cur_work_state, info.core_temp, info.powerup_cnt, info.local_time_now, info.last_sync_time, info.time_offset);
|
||||
|
||||
printf("Lidar state info, time_sync_type:%u, fw_type:%u.\n", info.time_sync_type, info.fw_type);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void ParseLidarStateInfo::ParseLidarIpAddr(const CommPacket& packet, uint16_t off, DirectLidarStateInfo& info) {
|
||||
uint8_t lidar_ip[4];
|
||||
memcpy(lidar_ip, &packet.data[off], sizeof(uint8_t) * 4);
|
||||
std::string lidar_ip_str = std::to_string(lidar_ip[0]) + "." + std::to_string(lidar_ip[1]) + "." +
|
||||
std::to_string(lidar_ip[2]) + "." + std::to_string(lidar_ip[3]);
|
||||
strcpy(info.lidar_ipcfg.ip_addr, lidar_ip_str.c_str());
|
||||
off += sizeof(uint8_t) * 4;
|
||||
|
||||
uint8_t lidar_submask[4];
|
||||
memcpy(lidar_submask, &packet.data[off], sizeof(uint8_t) * 4);
|
||||
std::string lidar_submask_str = std::to_string(lidar_submask[0]) + "." + std::to_string(lidar_submask[1]) +
|
||||
"." + std::to_string(lidar_submask[2]) + "." + std::to_string(lidar_submask[3]);
|
||||
strcpy(info.lidar_ipcfg.net_mask, lidar_submask_str.c_str());
|
||||
off += sizeof(uint8_t) * 4;
|
||||
|
||||
uint8_t lidar_gateway[4];
|
||||
memcpy(lidar_gateway, &packet.data[off], sizeof(uint8_t) * 4);
|
||||
std::string lidar_gateway_str = std::to_string(lidar_gateway[0]) + "." + std::to_string(lidar_gateway[1]) +
|
||||
"." + std::to_string(lidar_gateway[2]) + "." + std::to_string(lidar_gateway[3]);
|
||||
strcpy(info.lidar_ipcfg.gw_addr, lidar_gateway_str.c_str());
|
||||
}
|
||||
|
||||
void ParseLidarStateInfo::ParseStateInfoHostIPCfg(const CommPacket& packet, uint16_t off, DirectLidarStateInfo& info) {
|
||||
uint8_t host_state_info_ip[4];
|
||||
memcpy(host_state_info_ip, &packet.data[off], sizeof(uint8_t) * 4);
|
||||
std::string host_state_info_ip_str = std::to_string(host_state_info_ip[0]) + "." +
|
||||
std::to_string(host_state_info_ip[1]) + "." + std::to_string(host_state_info_ip[2]) + "." +
|
||||
std::to_string(host_state_info_ip[3]);
|
||||
|
||||
strcpy(info.host_state_info.host_ip_addr, host_state_info_ip_str.c_str());
|
||||
off += sizeof(uint8_t) * 4;
|
||||
|
||||
memcpy(&info.host_state_info.host_state_info_port, &packet.data[off], sizeof(uint16_t));
|
||||
off += sizeof(uint16_t);
|
||||
|
||||
memcpy(&info.host_state_info.lidar_state_info_port, &packet.data[off], sizeof(uint16_t));
|
||||
}
|
||||
|
||||
void ParseLidarStateInfo::ParsePointCloudHostIpCfg(const CommPacket& packet, uint16_t off, DirectLidarStateInfo& info) {
|
||||
uint8_t host_point_cloud_ip[4];
|
||||
memcpy(host_point_cloud_ip, &packet.data[off], sizeof(uint8_t) * 4);
|
||||
std::string host_point_cloud_ip_str = std::to_string(host_point_cloud_ip[0]) + "." +
|
||||
std::to_string(host_point_cloud_ip[1]) + "." + std::to_string(host_point_cloud_ip[2]) + "." +
|
||||
std::to_string(host_point_cloud_ip[3]);
|
||||
|
||||
strcpy(info.pointcloud_host_ipcfg.host_ip_addr, host_point_cloud_ip_str.c_str());
|
||||
off += sizeof(uint8_t) * 4;
|
||||
|
||||
memcpy(&info.pointcloud_host_ipcfg.host_point_data_port, &packet.data[off], sizeof(uint16_t));
|
||||
off += sizeof(uint16_t);
|
||||
|
||||
memcpy(&info.pointcloud_host_ipcfg.lidar_point_data_port, &packet.data[off], sizeof(uint16_t));
|
||||
off += sizeof(uint16_t);
|
||||
}
|
||||
|
||||
void ParseLidarStateInfo::ParseImuDataHostIpCfg(const CommPacket& packet, uint16_t off, DirectLidarStateInfo& info) {
|
||||
uint8_t host_imu_data_ip[4];
|
||||
memcpy(host_imu_data_ip, &packet.data[off], sizeof(uint8_t) * 4);
|
||||
std::string host_imu_data_ip_str = std::to_string(host_imu_data_ip[0]) + "." +
|
||||
std::to_string(host_imu_data_ip[1]) + "." + std::to_string(host_imu_data_ip[2]) + "." +
|
||||
std::to_string(host_imu_data_ip[3]);
|
||||
|
||||
strcpy(info.imu_host_ipcfg.host_ip_addr, host_imu_data_ip_str.c_str());
|
||||
off += sizeof(uint8_t) * 4;
|
||||
|
||||
memcpy(&info.imu_host_ipcfg.host_imu_data_port, &packet.data[off], sizeof(uint16_t));
|
||||
off += sizeof(uint16_t);
|
||||
|
||||
memcpy(&info.imu_host_ipcfg.lidar_imu_data_port, &packet.data[off], sizeof(uint16_t));
|
||||
off += sizeof(uint16_t);
|
||||
}
|
||||
|
||||
void ParseLidarStateInfo::ParseIpCfg(const CommPacket& packet, uint16_t off, LivoxIpCfg& cfg) {
|
||||
std::string ip_str = std::to_string(packet.data[off]) + "." +
|
||||
std::to_string(packet.data[off + 1]) + "." +
|
||||
std::to_string(packet.data[off + 2]) + "." +
|
||||
std::to_string(packet.data[off + 3]);
|
||||
strcpy(cfg.ip_addr, ip_str.c_str());
|
||||
off += sizeof(uint8_t) * 4;
|
||||
cfg.dst_port = *(uint16_t*)&packet.data[off];
|
||||
off += sizeof(uint16_t);
|
||||
cfg.src_port = *(uint16_t*)&packet.data[off];
|
||||
return;
|
||||
}
|
||||
|
||||
void ParseLidarStateInfo::LivoxLidarStateInfoToJson(const DirectLidarStateInfo& info, const std::set<ParamKeyName>& key_mask, std::string& lidar_info) {
|
||||
rapidjson::StringBuffer buf;
|
||||
rapidjson::PrettyWriter<rapidjson::StringBuffer> write(buf);
|
||||
write.StartObject();
|
||||
|
||||
// write.Key("dev_type");
|
||||
// write.String("MID360");
|
||||
|
||||
if (key_mask.find(kKeyPclDataType) != key_mask.end()) {
|
||||
write.Key("pcl_data_type");
|
||||
write.Uint(info.pcl_data_type);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyPatternMode) != key_mask.end()) {
|
||||
write.Key("pattern_mode");
|
||||
write.Uint(info.pattern_mode);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyDualEmitEn) != key_mask.end()) {
|
||||
write.Key("dual_emit_en");
|
||||
write.Uint(info.dual_emit_en);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyPointSendEn) != key_mask.end()) {
|
||||
write.Key("point_send_en");
|
||||
write.Uint(info.point_send_en);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyLidarIpCfg) != key_mask.end()) {
|
||||
write.Key("lidar_ipcfg");
|
||||
write.StartObject();
|
||||
write.Key("lidar_ip");
|
||||
write.String(info.lidar_ipcfg.ip_addr);
|
||||
write.Key("lidar_subnet_mask");
|
||||
write.String(info.lidar_ipcfg.net_mask);
|
||||
write.Key("lidar_gateway");
|
||||
write.String(info.lidar_ipcfg.gw_addr);
|
||||
write.EndObject();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyStateInfoHostIpCfg) != key_mask.end()) {
|
||||
write.Key("state_info_host_ipcfg");
|
||||
write.StartObject();
|
||||
write.Key("ip");
|
||||
write.String(info.host_state_info.host_ip_addr);
|
||||
write.Key("dst_port");
|
||||
write.Uint(info.host_state_info.host_state_info_port);
|
||||
write.Key("src_port");
|
||||
write.Uint(info.host_state_info.lidar_state_info_port);
|
||||
write.EndObject();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyLidarPointDataHostIpCfg) != key_mask.end()) {
|
||||
write.Key("ponitcloud_host_ipcfg");
|
||||
write.StartObject();
|
||||
write.Key("ip");
|
||||
write.String(info.pointcloud_host_ipcfg.host_ip_addr);
|
||||
write.Key("dst_port");
|
||||
write.Uint(info.pointcloud_host_ipcfg.host_point_data_port);
|
||||
write.Key("src_port");
|
||||
write.Uint(info.pointcloud_host_ipcfg.lidar_point_data_port);
|
||||
write.EndObject();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyLidarImuHostIpCfg) != key_mask.end()) {
|
||||
write.Key("imu_host_ipcfg");
|
||||
write.StartObject();
|
||||
write.Key("ip");
|
||||
write.String(info.imu_host_ipcfg.host_ip_addr);
|
||||
write.Key("dst_port");
|
||||
write.Uint(info.imu_host_ipcfg.host_imu_data_port);
|
||||
write.Key("src_port");
|
||||
write.Uint(info.imu_host_ipcfg.lidar_imu_data_port);
|
||||
write.EndObject();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyCtlHostIpCfg) != key_mask.end()) {
|
||||
write.Key("ctl_host_ipcfg");
|
||||
write.StartObject();
|
||||
write.Key("ip");
|
||||
write.String(info.ctl_host_ipcfg.ip_addr);
|
||||
write.Key("dst_port");
|
||||
write.Uint(info.ctl_host_ipcfg.dst_port);
|
||||
write.Key("src_port");
|
||||
write.Uint(info.ctl_host_ipcfg.src_port);
|
||||
write.EndObject();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyLogHostIpCfg) != key_mask.end()) {
|
||||
write.Key("log_host_ipcfg");
|
||||
write.StartObject();
|
||||
write.Key("ip");
|
||||
write.String(info.log_host_ipcfg.ip_addr);
|
||||
write.Key("dst_port");
|
||||
write.Uint(info.log_host_ipcfg.dst_port);
|
||||
write.Key("src_port");
|
||||
write.Uint(info.log_host_ipcfg.src_port);
|
||||
write.EndObject();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyVehicleSpeed) != key_mask.end()) {
|
||||
write.Key("vehicle_speed");
|
||||
write.Int(info.vehicle_speed);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyEnvironmentTemp) != key_mask.end()) {
|
||||
write.Key("environment_temp");
|
||||
write.Int(info.environment_temp);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyInstallAttitude) != key_mask.end()) {
|
||||
write.Key("install_attitude");
|
||||
write.StartObject();
|
||||
write.Key("roll_deg");
|
||||
write.Double(info.install_attitude.roll_deg);
|
||||
write.Key("pitch_deg");
|
||||
write.Double(info.install_attitude.pitch_deg);
|
||||
write.Key("yaw_deg");
|
||||
write.Double(info.install_attitude.yaw_deg);
|
||||
write.Key("x_mm");
|
||||
write.Uint(info.install_attitude.x);
|
||||
write.Key("y_mm");
|
||||
write.Uint(info.install_attitude.y);
|
||||
write.Key("z_mm");
|
||||
write.Uint(info.install_attitude.z);
|
||||
write.EndObject();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyBlindSpotSet) != key_mask.end()) {
|
||||
write.Key("blind_spot_set");
|
||||
write.Uint(info.blind_spot_set);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyFrameRate) != key_mask.end()) {
|
||||
write.Key("frame_rate");
|
||||
write.Uint(info.frame_rate);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyFovCfg0) != key_mask.end()) {
|
||||
write.Key("fov_cfg0");
|
||||
write.StartObject();
|
||||
write.Key("yaw_start");
|
||||
write.Int(info.fov_cfg0.yaw_start);
|
||||
write.Key("yaw_stop");
|
||||
write.Int(info.fov_cfg0.yaw_stop);
|
||||
write.Key("pitch_start");
|
||||
write.Int(info.fov_cfg0.pitch_start);
|
||||
write.Key("pitch_stop");
|
||||
write.Int(info.fov_cfg0.pitch_stop);
|
||||
write.EndObject();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyFovCfg1) != key_mask.end()) {
|
||||
write.Key("fov_cfg1");
|
||||
write.StartObject();
|
||||
write.Key("yaw_start");
|
||||
write.Int(info.fov_cfg1.yaw_start);
|
||||
write.Key("yaw_stop");
|
||||
write.Int(info.fov_cfg1.yaw_stop);
|
||||
write.Key("pitch_start");
|
||||
write.Int(info.fov_cfg1.pitch_start);
|
||||
write.Key("pitch_stop");
|
||||
write.Int(info.fov_cfg1.pitch_stop);
|
||||
write.EndObject();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyFovCfgEn) != key_mask.end()) {
|
||||
write.Key("fov_cfg_en");
|
||||
write.Uint(info.fov_cfg_en);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyDetectMode) != key_mask.end()) {
|
||||
write.Key("detect_mode");
|
||||
write.Uint(info.detect_mode);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyFuncIoCfg) != key_mask.end()) {
|
||||
write.Key("func_io_cfg");
|
||||
write.StartObject();
|
||||
write.Key("IN0");
|
||||
write.Uint(info.func_io_cfg[0]);
|
||||
write.Key("IN1");
|
||||
write.Uint(info.func_io_cfg[1]);
|
||||
write.Key("OUT0");
|
||||
write.Uint(info.func_io_cfg[2]);
|
||||
write.Key("OUT1");
|
||||
write.Uint(info.func_io_cfg[3]);
|
||||
write.EndObject();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyWorkMode) != key_mask.end()) {
|
||||
write.Key("work_tgt_mode");
|
||||
write.Uint(info.work_tgt_mode);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyGlassHeat) != key_mask.end()) {
|
||||
write.Key("glass_heat");
|
||||
write.Uint(info.glass_heat);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyImuDataEn) != key_mask.end()) {
|
||||
write.Key("imu_data_en");
|
||||
write.Uint(info.imu_data_en);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyFusaEn) != key_mask.end()) {
|
||||
write.Key("fusa_en");
|
||||
write.Uint(info.fusa_en);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeySetEscMode) != key_mask.end()) {
|
||||
write.Key("esc_mode");
|
||||
write.Uint(info.esc_mode);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeySn) != key_mask.end()) {
|
||||
write.Key("sn");
|
||||
write.String(info.sn);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyProductInfo) != key_mask.end()) {
|
||||
write.Key("product_info");
|
||||
write.String(info.product_info);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyVersionApp) != key_mask.end()) {
|
||||
write.Key("version_app");
|
||||
write.StartArray();
|
||||
write.Uint(info.version_app[0]);
|
||||
write.Uint(info.version_app[1]);
|
||||
write.Uint(info.version_app[2]);
|
||||
write.Uint(info.version_app[3]);
|
||||
write.EndArray();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyVersionLoader) != key_mask.end()) {
|
||||
write.Key("version_loader");
|
||||
write.StartArray();
|
||||
write.Uint(info.version_loader[0]);
|
||||
write.Uint(info.version_loader[1]);
|
||||
write.Uint(info.version_loader[2]);
|
||||
write.Uint(info.version_loader[3]);
|
||||
write.EndArray();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyVersionHardware) != key_mask.end()) {
|
||||
write.Key("version_hardware");
|
||||
write.StartArray();
|
||||
write.Uint(info.version_hardware[0]);
|
||||
write.Uint(info.version_hardware[1]);
|
||||
write.Uint(info.version_hardware[2]);
|
||||
write.Uint(info.version_hardware[3]);
|
||||
write.EndArray();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyMac) != key_mask.end()) {
|
||||
write.Key("mac");
|
||||
write.StartArray();
|
||||
write.Uint(info.mac[0]);
|
||||
write.Uint(info.mac[1]);
|
||||
write.Uint(info.mac[2]);
|
||||
write.Uint(info.mac[3]);
|
||||
write.Uint(info.mac[4]);
|
||||
write.Uint(info.mac[5]);
|
||||
write.EndArray();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyCurWorkState) != key_mask.end()) {
|
||||
write.Key("cur_work_state");
|
||||
write.Uint(info.cur_work_state);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyCoreTemp) != key_mask.end()) {
|
||||
write.Key("core_temp");
|
||||
write.Int(info.core_temp);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyPowerUpCnt) != key_mask.end()) {
|
||||
write.Key("powerup_cnt");
|
||||
write.Uint(info.powerup_cnt);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyLocalTimeNow) != key_mask.end()) {
|
||||
write.Key("local_time_now");
|
||||
write.Uint64(info.local_time_now);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyLastSyncTime) != key_mask.end()) {
|
||||
write.Key("last_sync_time");
|
||||
write.Uint64(info.last_sync_time);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyTimeOffset) != key_mask.end()) {
|
||||
write.Key("time_offset");
|
||||
write.Int64(info.time_offset);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyTimeSyncType) != key_mask.end()) {
|
||||
write.Key("time_sync_type");
|
||||
write.Uint(info.time_sync_type);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyStatusCode) != key_mask.end()) {
|
||||
write.Key("status_code");
|
||||
std::ostringstream ss;
|
||||
for (int idx = 31; idx >= 0; --idx) {
|
||||
ss << std::hex << static_cast<uint32_t>(info.status_code[idx]);
|
||||
if (idx != 0) {
|
||||
ss << " ";
|
||||
}
|
||||
}
|
||||
write.String(ss.str().c_str());
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyLidarDiagStatus) != key_mask.end()) {
|
||||
write.Key("lidar_diag_status");
|
||||
write.Uint(info.lidar_diag_status);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyLidarFlashStatus) != key_mask.end()) {
|
||||
write.Key("lidar_flash_status");
|
||||
write.Uint(info.lidar_flash_status);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyFwType) != key_mask.end()) {
|
||||
write.Key("FW_TYPE");
|
||||
write.Uint(info.fw_type);
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyHmsCode) != key_mask.end()) {
|
||||
write.Key("hms_code");
|
||||
write.StartArray();
|
||||
write.Uint(info.hms_code[0]);
|
||||
write.Uint(info.hms_code[1]);
|
||||
write.Uint(info.hms_code[2]);
|
||||
write.Uint(info.hms_code[3]);
|
||||
write.Uint(info.hms_code[4]);
|
||||
write.Uint(info.hms_code[5]);
|
||||
write.Uint(info.hms_code[6]);
|
||||
write.Uint(info.hms_code[7]);
|
||||
write.EndArray();
|
||||
}
|
||||
|
||||
if (key_mask.find(kKeyRoiMode) != key_mask.end()) {
|
||||
write.Key("ROI_Mode");
|
||||
write.Uint(info.ROI_Mode);
|
||||
}
|
||||
|
||||
write.EndObject();
|
||||
|
||||
lidar_info = buf.GetString();
|
||||
// LOG_INFO("###################################lidar_info_to_json:{}", lidar_info.c_str());
|
||||
}
|
||||
|
||||
} // namespace livox
|
||||
} // namespace direct
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,83 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2022 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#ifndef PARSE_LIDAR_STATE_INFO_H_
|
||||
#define PARSE_LIDAR_STATE_INFO_H_
|
||||
|
||||
#include <memory>
|
||||
#include <map>
|
||||
#include <mutex>
|
||||
#include <set>
|
||||
|
||||
#include "comm/define.h"
|
||||
#include "comm/protocol.h"
|
||||
|
||||
#include "livox_lidar_def.h"
|
||||
|
||||
#include "rapidjson/document.h"
|
||||
#include "rapidjson/filereadstream.h"
|
||||
#include "rapidjson/stringbuffer.h"
|
||||
#include "rapidjson/prettywriter.h"
|
||||
|
||||
namespace livox {
|
||||
|
||||
namespace lidar {
|
||||
|
||||
class ParseLidarStateInfo {
|
||||
public:
|
||||
static bool Parse(const CommPacket& packet, std::string& info);
|
||||
private:
|
||||
static bool ParseStateInfo(const CommPacket& packet, DirectLidarStateInfo& info, std::set<ParamKeyName>& key_mask);
|
||||
static void ParseLidarIpAddr(const CommPacket& packet, uint16_t off, DirectLidarStateInfo& info);
|
||||
static void ParseStateInfoHostIPCfg(const CommPacket& packet, uint16_t off, DirectLidarStateInfo& info);
|
||||
static void ParsePointCloudHostIpCfg(const CommPacket& packet, uint16_t off, DirectLidarStateInfo& info);
|
||||
static void ParseImuDataHostIpCfg(const CommPacket& packet, uint16_t off, DirectLidarStateInfo& info);
|
||||
static void ParseIpCfg(const CommPacket& packet, uint16_t off, LivoxIpCfg& cfg);
|
||||
static void LivoxLidarStateInfoToJson(const DirectLidarStateInfo& info, const std::set<ParamKeyName>& key_mask, std::string& lidar_info);
|
||||
};
|
||||
|
||||
} // namespace livox
|
||||
} // namespace lidar
|
||||
|
||||
# endif // PARSE_LIDAR_STATE_INFO_H_
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user