feat(agv_pro_bringup): add multi-lidar support and drivers
This commit is contained in:
X-lanni
@@ -0,0 +1,457 @@
|
||||
//
|
||||
// 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 "pub_handler.h"
|
||||
|
||||
#include <cstdlib>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
|
||||
namespace livox_ros {
|
||||
|
||||
std::atomic<bool> PubHandler::is_timestamp_sync_;
|
||||
|
||||
PubHandler &pub_handler() {
|
||||
static PubHandler handler;
|
||||
return handler;
|
||||
}
|
||||
|
||||
void PubHandler::Init() {
|
||||
}
|
||||
|
||||
void PubHandler::Uninit() {
|
||||
if (lidar_listen_id_ > 0) {
|
||||
LivoxLidarRemovePointCloudObserver(lidar_listen_id_);
|
||||
lidar_listen_id_ = 0;
|
||||
}
|
||||
|
||||
RequestExit();
|
||||
|
||||
if (point_process_thread_ &&
|
||||
point_process_thread_->joinable()) {
|
||||
point_process_thread_->join();
|
||||
point_process_thread_ = nullptr;
|
||||
} else {
|
||||
/* */
|
||||
}
|
||||
}
|
||||
|
||||
void PubHandler::RequestExit() {
|
||||
is_quit_.store(true);
|
||||
}
|
||||
|
||||
void PubHandler::SetPointCloudConfig(const double publish_freq) {
|
||||
publish_interval_ = (kNsPerSecond / (publish_freq * 10)) * 10;
|
||||
publish_interval_tolerance_ = publish_interval_ - kNsTolerantFrameTimeDeviation;
|
||||
publish_interval_ms_ = publish_interval_ / kRatioOfMsToNs;
|
||||
if (!point_process_thread_) {
|
||||
point_process_thread_ = std::make_shared<std::thread>(&PubHandler::RawDataProcess, this);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void PubHandler::SetImuDataCallback(ImuDataCallback cb, void* client_data) {
|
||||
imu_client_data_ = client_data;
|
||||
imu_callback_ = cb;
|
||||
}
|
||||
|
||||
void PubHandler::AddLidarsExtParam(LidarExtParameter& lidar_param) {
|
||||
std::unique_lock<std::mutex> lock(packet_mutex_);
|
||||
uint32_t id = 0;
|
||||
GetLidarId(lidar_param.lidar_type, lidar_param.handle, id);
|
||||
lidar_extrinsics_[id] = lidar_param;
|
||||
}
|
||||
|
||||
void PubHandler::ClearAllLidarsExtrinsicParams() {
|
||||
std::unique_lock<std::mutex> lock(packet_mutex_);
|
||||
lidar_extrinsics_.clear();
|
||||
}
|
||||
|
||||
void PubHandler::SetPointCloudsCallback(PointCloudsCallback cb, void* client_data) {
|
||||
pub_client_data_ = client_data;
|
||||
points_callback_ = cb;
|
||||
lidar_listen_id_ = LivoxLidarAddPointCloudObserver(OnLivoxLidarPointCloudCallback, this);
|
||||
}
|
||||
|
||||
void PubHandler::OnLivoxLidarPointCloudCallback(uint32_t handle, const uint8_t dev_type,
|
||||
LivoxLidarEthernetPacket *data, void *client_data) {
|
||||
PubHandler* self = (PubHandler*)client_data;
|
||||
if (!self) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (data->time_type != kTimestampTypeNoSync) {
|
||||
is_timestamp_sync_.store(true);
|
||||
} else {
|
||||
is_timestamp_sync_.store(false);
|
||||
}
|
||||
|
||||
if (data->data_type == kLivoxLidarImuData) {
|
||||
if (self->imu_callback_) {
|
||||
RawImuPoint* imu = (RawImuPoint*) data->data;
|
||||
ImuData imu_data;
|
||||
imu_data.lidar_type = static_cast<uint8_t>(LidarProtoType::kLivoxLidarType);
|
||||
imu_data.handle = handle;
|
||||
imu_data.time_stamp = GetEthPacketTimestamp(data->time_type,
|
||||
data->timestamp, sizeof(data->timestamp));
|
||||
imu_data.gyro_x = imu->gyro_x;
|
||||
imu_data.gyro_y = imu->gyro_y;
|
||||
imu_data.gyro_z = imu->gyro_z;
|
||||
imu_data.acc_x = imu->acc_x;
|
||||
imu_data.acc_y = imu->acc_y;
|
||||
imu_data.acc_z = imu->acc_z;
|
||||
self->imu_callback_(&imu_data, self->imu_client_data_);
|
||||
}
|
||||
return;
|
||||
}
|
||||
RawPacket packet = {};
|
||||
packet.handle = handle;
|
||||
packet.lidar_type = LidarProtoType::kLivoxLidarType;
|
||||
packet.extrinsic_enable = false;
|
||||
if (dev_type == LivoxLidarDeviceType::kLivoxLidarTypeIndustrialHAP) {
|
||||
packet.line_num = kLineNumberHAP;
|
||||
} else if (dev_type == LivoxLidarDeviceType::kLivoxLidarTypeMid360) {
|
||||
packet.line_num = kLineNumberMid360;
|
||||
} else {
|
||||
packet.line_num = kLineNumberDefault;
|
||||
}
|
||||
packet.data_type = data->data_type;
|
||||
packet.point_num = data->dot_num;
|
||||
packet.point_interval = data->time_interval * 100 / data->dot_num; //ns
|
||||
packet.time_stamp = GetEthPacketTimestamp(data->time_type,
|
||||
data->timestamp, sizeof(data->timestamp));
|
||||
uint32_t length = data->length - sizeof(LivoxLidarEthernetPacket) + 1;
|
||||
packet.raw_data.insert(packet.raw_data.end(), data->data, data->data + length);
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(self->packet_mutex_);
|
||||
self->raw_packet_queue_.push_back(packet);
|
||||
}
|
||||
self->packet_condition_.notify_one();
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void PubHandler::PublishPointCloud() {
|
||||
//publish point
|
||||
if (points_callback_) {
|
||||
points_callback_(&frame_, pub_client_data_);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void PubHandler::CheckTimer(uint32_t id) {
|
||||
|
||||
if (PubHandler::is_timestamp_sync_.load()) { // Enable time synchronization
|
||||
auto& process_handler = lidar_process_handlers_[id];
|
||||
uint64_t recent_time_ms = process_handler->GetRecentTimeStamp() / kRatioOfMsToNs;
|
||||
if ((recent_time_ms % publish_interval_ms_ != 0) || recent_time_ms == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t diff = process_handler->GetRecentTimeStamp() - process_handler->GetLidarBaseTime();
|
||||
if (diff < publish_interval_tolerance_) {
|
||||
return;
|
||||
}
|
||||
|
||||
frame_.base_time[frame_.lidar_num] = process_handler->GetLidarBaseTime();
|
||||
points_[id].clear();
|
||||
process_handler->GetLidarPointClouds(points_[id]);
|
||||
if (points_[id].empty()) {
|
||||
return;
|
||||
}
|
||||
PointPacket& lidar_point = frame_.lidar_point[frame_.lidar_num];
|
||||
lidar_point.lidar_type = LidarProtoType::kLivoxLidarType; // TODO:
|
||||
lidar_point.handle = id;
|
||||
lidar_point.points_num = points_[id].size();
|
||||
lidar_point.points = points_[id].data();
|
||||
frame_.lidar_num++;
|
||||
|
||||
if (frame_.lidar_num != 0) {
|
||||
PublishPointCloud();
|
||||
frame_.lidar_num = 0;
|
||||
}
|
||||
} else { // Disable time synchronization
|
||||
auto now_time = std::chrono::high_resolution_clock::now();
|
||||
//First Set
|
||||
static bool first = true;
|
||||
if (first) {
|
||||
last_pub_time_ = now_time;
|
||||
first = false;
|
||||
return;
|
||||
}
|
||||
if (now_time - last_pub_time_ < std::chrono::nanoseconds(publish_interval_)) {
|
||||
return;
|
||||
}
|
||||
last_pub_time_ += std::chrono::nanoseconds(publish_interval_);
|
||||
for (auto &process_handler : lidar_process_handlers_) {
|
||||
frame_.base_time[frame_.lidar_num] = process_handler.second->GetLidarBaseTime();
|
||||
uint32_t handle = process_handler.first;
|
||||
points_[handle].clear();
|
||||
process_handler.second->GetLidarPointClouds(points_[handle]);
|
||||
if (points_[handle].empty()) {
|
||||
continue;
|
||||
}
|
||||
PointPacket& lidar_point = frame_.lidar_point[frame_.lidar_num];
|
||||
lidar_point.lidar_type = LidarProtoType::kLivoxLidarType; // TODO:
|
||||
lidar_point.handle = handle;
|
||||
lidar_point.points_num = points_[handle].size();
|
||||
lidar_point.points = points_[handle].data();
|
||||
frame_.lidar_num++;
|
||||
}
|
||||
PublishPointCloud();
|
||||
frame_.lidar_num = 0;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void PubHandler::RawDataProcess() {
|
||||
RawPacket raw_data;
|
||||
while (!is_quit_.load()) {
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(packet_mutex_);
|
||||
if (raw_packet_queue_.empty()) {
|
||||
packet_condition_.wait_for(lock, std::chrono::milliseconds(500));
|
||||
if (raw_packet_queue_.empty()) {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
raw_data = raw_packet_queue_.front();
|
||||
raw_packet_queue_.pop_front();
|
||||
}
|
||||
uint32_t id = 0;
|
||||
GetLidarId(raw_data.lidar_type, raw_data.handle, id);
|
||||
if (lidar_process_handlers_.find(id) == lidar_process_handlers_.end()) {
|
||||
lidar_process_handlers_[id].reset(new LidarPubHandler());
|
||||
}
|
||||
auto &process_handler = lidar_process_handlers_[id];
|
||||
if (lidar_extrinsics_.find(id) != lidar_extrinsics_.end()) {
|
||||
lidar_process_handlers_[id]->SetLidarsExtParam(lidar_extrinsics_[id]);
|
||||
}
|
||||
process_handler->PointCloudProcess(raw_data);
|
||||
CheckTimer(id);
|
||||
}
|
||||
}
|
||||
|
||||
bool PubHandler::GetLidarId(LidarProtoType lidar_type, uint32_t handle, uint32_t& id) {
|
||||
if (lidar_type == kLivoxLidarType) {
|
||||
id = handle;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
uint64_t PubHandler::GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, uint8_t size) {
|
||||
LdsStamp time;
|
||||
memcpy(time.stamp_bytes, time_stamp, size);
|
||||
|
||||
if (timestamp_type == kTimestampTypeGptpOrPtp ||
|
||||
timestamp_type == kTimestampTypeGps) {
|
||||
return time.stamp;
|
||||
}
|
||||
|
||||
return std::chrono::high_resolution_clock::now().time_since_epoch().count();
|
||||
}
|
||||
|
||||
/*******************************/
|
||||
/* LidarPubHandler Definitions*/
|
||||
LidarPubHandler::LidarPubHandler() : is_set_extrinsic_params_(false) {}
|
||||
|
||||
uint64_t LidarPubHandler::GetLidarBaseTime() {
|
||||
if (points_clouds_.empty()) {
|
||||
return 0;
|
||||
}
|
||||
return points_clouds_.at(0).offset_time;
|
||||
}
|
||||
|
||||
void LidarPubHandler::GetLidarPointClouds(std::vector<PointXyzlt>& points_clouds) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
points_clouds.swap(points_clouds_);
|
||||
}
|
||||
|
||||
uint64_t LidarPubHandler::GetRecentTimeStamp() {
|
||||
if (points_clouds_.empty()) {
|
||||
return 0;
|
||||
}
|
||||
return points_clouds_.back().offset_time;
|
||||
}
|
||||
|
||||
uint32_t LidarPubHandler::GetLidarPointCloudsSize() {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return points_clouds_.size();
|
||||
}
|
||||
|
||||
//convert to standard format and extrinsic compensate
|
||||
void LidarPubHandler::PointCloudProcess(RawPacket & pkt) {
|
||||
if (pkt.lidar_type == LidarProtoType::kLivoxLidarType) {
|
||||
LivoxLidarPointCloudProcess(pkt);
|
||||
} else {
|
||||
static bool flag = false;
|
||||
if (!flag) {
|
||||
std::cout << "error, unsupported protocol type: " << static_cast<int>(pkt.lidar_type) << std::endl;
|
||||
flag = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void LidarPubHandler::LivoxLidarPointCloudProcess(RawPacket & pkt) {
|
||||
switch (pkt.data_type) {
|
||||
case kLivoxLidarCartesianCoordinateHighData:
|
||||
ProcessCartesianHighPoint(pkt);
|
||||
break;
|
||||
case kLivoxLidarCartesianCoordinateLowData:
|
||||
ProcessCartesianLowPoint(pkt);
|
||||
break;
|
||||
case kLivoxLidarSphericalCoordinateData:
|
||||
ProcessSphericalPoint(pkt);
|
||||
break;
|
||||
default:
|
||||
std::cout << "unknown data type: " << static_cast<int>(pkt.data_type)
|
||||
<< " !!" << std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void LidarPubHandler::SetLidarsExtParam(LidarExtParameter lidar_param) {
|
||||
if (is_set_extrinsic_params_) {
|
||||
return;
|
||||
}
|
||||
extrinsic_.trans[0] = lidar_param.param.x;
|
||||
extrinsic_.trans[1] = lidar_param.param.y;
|
||||
extrinsic_.trans[2] = lidar_param.param.z;
|
||||
|
||||
double cos_roll = cos(static_cast<double>(lidar_param.param.roll * PI / 180.0));
|
||||
double cos_pitch = cos(static_cast<double>(lidar_param.param.pitch * PI / 180.0));
|
||||
double cos_yaw = cos(static_cast<double>(lidar_param.param.yaw * PI / 180.0));
|
||||
double sin_roll = sin(static_cast<double>(lidar_param.param.roll * PI / 180.0));
|
||||
double sin_pitch = sin(static_cast<double>(lidar_param.param.pitch * PI / 180.0));
|
||||
double sin_yaw = sin(static_cast<double>(lidar_param.param.yaw * PI / 180.0));
|
||||
|
||||
extrinsic_.rotation[0][0] = cos_pitch * cos_yaw;
|
||||
extrinsic_.rotation[0][1] = sin_roll * sin_pitch * cos_yaw - cos_roll * sin_yaw;
|
||||
extrinsic_.rotation[0][2] = cos_roll * sin_pitch * cos_yaw + sin_roll * sin_yaw;
|
||||
|
||||
extrinsic_.rotation[1][0] = cos_pitch * sin_yaw;
|
||||
extrinsic_.rotation[1][1] = sin_roll * sin_pitch * sin_yaw + cos_roll * cos_yaw;
|
||||
extrinsic_.rotation[1][2] = cos_roll * sin_pitch * sin_yaw - sin_roll * cos_yaw;
|
||||
|
||||
extrinsic_.rotation[2][0] = -sin_pitch;
|
||||
extrinsic_.rotation[2][1] = sin_roll * cos_pitch;
|
||||
extrinsic_.rotation[2][2] = cos_roll * cos_pitch;
|
||||
|
||||
is_set_extrinsic_params_ = true;
|
||||
}
|
||||
|
||||
void LidarPubHandler::ProcessCartesianHighPoint(RawPacket & pkt) {
|
||||
LivoxLidarCartesianHighRawPoint* raw = (LivoxLidarCartesianHighRawPoint*)pkt.raw_data.data();
|
||||
PointXyzlt point = {};
|
||||
for (uint32_t i = 0; i < pkt.point_num; i++) {
|
||||
if (pkt.extrinsic_enable) {
|
||||
point.x = raw[i].x / 1000.0;
|
||||
point.y = raw[i].y / 1000.0;
|
||||
point.z = raw[i].z / 1000.0;
|
||||
} else {
|
||||
point.x = (raw[i].x * extrinsic_.rotation[0][0] +
|
||||
raw[i].y * extrinsic_.rotation[0][1] +
|
||||
raw[i].z * extrinsic_.rotation[0][2] + extrinsic_.trans[0]) / 1000.0;
|
||||
point.y = (raw[i].x* extrinsic_.rotation[1][0] +
|
||||
raw[i].y * extrinsic_.rotation[1][1] +
|
||||
raw[i].z * extrinsic_.rotation[1][2] + extrinsic_.trans[1]) / 1000.0;
|
||||
point.z = (raw[i].x * extrinsic_.rotation[2][0] +
|
||||
raw[i].y * extrinsic_.rotation[2][1] +
|
||||
raw[i].z * extrinsic_.rotation[2][2] + extrinsic_.trans[2]) / 1000.0;
|
||||
}
|
||||
point.intensity = raw[i].reflectivity;
|
||||
point.line = i % pkt.line_num;
|
||||
point.tag = raw[i].tag;
|
||||
point.offset_time = pkt.time_stamp + i * pkt.point_interval;
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
points_clouds_.push_back(point);
|
||||
}
|
||||
}
|
||||
|
||||
void LidarPubHandler::ProcessCartesianLowPoint(RawPacket & pkt) {
|
||||
LivoxLidarCartesianLowRawPoint* raw = (LivoxLidarCartesianLowRawPoint*)pkt.raw_data.data();
|
||||
PointXyzlt point = {};
|
||||
for (uint32_t i = 0; i < pkt.point_num; i++) {
|
||||
if (pkt.extrinsic_enable) {
|
||||
point.x = raw[i].x / 100.0;
|
||||
point.y = raw[i].y / 100.0;
|
||||
point.z = raw[i].z / 100.0;
|
||||
} else {
|
||||
point.x = (raw[i].x * extrinsic_.rotation[0][0] +
|
||||
raw[i].y * extrinsic_.rotation[0][1] +
|
||||
raw[i].z * extrinsic_.rotation[0][2] + extrinsic_.trans[0]) / 100.0;
|
||||
point.y = (raw[i].x* extrinsic_.rotation[1][0] +
|
||||
raw[i].y * extrinsic_.rotation[1][1] +
|
||||
raw[i].z * extrinsic_.rotation[1][2] + extrinsic_.trans[1]) / 100.0;
|
||||
point.z = (raw[i].x * extrinsic_.rotation[2][0] +
|
||||
raw[i].y * extrinsic_.rotation[2][1] +
|
||||
raw[i].z * extrinsic_.rotation[2][2] + extrinsic_.trans[2]) / 100.0;
|
||||
}
|
||||
point.intensity = raw[i].reflectivity;
|
||||
point.line = i % pkt.line_num;
|
||||
point.tag = raw[i].tag;
|
||||
point.offset_time = pkt.time_stamp + i * pkt.point_interval;
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
points_clouds_.push_back(point);
|
||||
}
|
||||
}
|
||||
|
||||
void LidarPubHandler::ProcessSphericalPoint(RawPacket& pkt) {
|
||||
LivoxLidarSpherPoint* raw = (LivoxLidarSpherPoint*)pkt.raw_data.data();
|
||||
PointXyzlt point = {};
|
||||
for (uint32_t i = 0; i < pkt.point_num; i++) {
|
||||
double radius = raw[i].depth / 1000.0;
|
||||
double theta = raw[i].theta / 100.0 / 180 * PI;
|
||||
double phi = raw[i].phi / 100.0 / 180 * PI;
|
||||
double src_x = radius * sin(theta) * cos(phi);
|
||||
double src_y = radius * sin(theta) * sin(phi);
|
||||
double src_z = radius * cos(theta);
|
||||
if (pkt.extrinsic_enable) {
|
||||
point.x = src_x;
|
||||
point.y = src_y;
|
||||
point.z = src_z;
|
||||
} else {
|
||||
point.x = src_x * extrinsic_.rotation[0][0] +
|
||||
src_y * extrinsic_.rotation[0][1] +
|
||||
src_z * extrinsic_.rotation[0][2] + (extrinsic_.trans[0] / 1000.0);
|
||||
point.y = src_x * extrinsic_.rotation[1][0] +
|
||||
src_y * extrinsic_.rotation[1][1] +
|
||||
src_z * extrinsic_.rotation[1][2] + (extrinsic_.trans[1] / 1000.0);
|
||||
point.z = src_x * extrinsic_.rotation[2][0] +
|
||||
src_y * extrinsic_.rotation[2][1] +
|
||||
src_z * extrinsic_.rotation[2][2] + (extrinsic_.trans[2] / 1000.0);
|
||||
}
|
||||
|
||||
point.intensity = raw[i].reflectivity;
|
||||
point.line = i % pkt.line_num;
|
||||
point.tag = raw[i].tag;
|
||||
point.offset_time = pkt.time_stamp + i * pkt.point_interval;
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
points_clouds_.push_back(point);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace livox_ros
|
||||
Reference in New Issue
Block a user