Skip to content

Commit

Permalink
Fix/esol/livox driver utest (#9)
Browse files Browse the repository at this point in the history 
* Refactor 1st

* pull/6#discussion_r681704754

* pull/6#discussion_r682172984

* pull/6#discussion_r681705368

* pull/6#discussion_r680921593 and pull/6#discussion_r680922194

* pull/6#discussion_r681711654

* 6#discussion_r682170555

* livox_driver utest 1st

* T4PUB-437 bug fix.

* Fix merge failure 3371552e395f1817adb3a32ea9f95360d72019f8

* pull/9#discussion_r689167375 and pull/9#discussion_r689204716
  • Loading branch information
@h-mitsui-esol
h-mitsui-esol authored and Daisuke Nishimatsu committed Mar 7, 2023
1 parent 1ef70b5 commit 2bd9534
Show file tree
Hide file tree
Showing 5 changed files with 92 additions and 38 deletions.
7 changes: 6 additions & 1 deletion livox_driver/include/LidarDriver/lidar_driver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,9 +138,11 @@ class LidarDriver
volatile uint32_t packet_interval_max; /**< If more than it, have packet loss */
volatile uint32_t onetime_publish_packets;
DeviceType device_info_type;
uint32_t line_num;
LidarPacketStatistic statistic_info_data;
LidarPacketStatistic statistic_info_imu;
} lidars_;
bool data_is_published;
} lidar_device_;

PublishLidarDataCallback publish_lidar_data_cb_;
PublishImuPacketCallback publish_imu_packet_cb_;
Expand All @@ -156,10 +158,13 @@ class LidarDriver
uint32_t publish_period_ns_;
uint64_t last_timestamp_;
uint32_t accumulate_count_;
bool skip_start_packet_;
uint32_t last_remaning_time_;

std::unique_ptr<livox_driver::LivoxPublishData> temp_publish_data_;
std::unique_ptr<livox_driver::LivoxPublishData> publish_data_;

bool SetLastTimeStamp(uint64_t timestamp);
void LivoxExtendRawPointToPxyzrtl(
uint8_t * point_buf, const std::vector<uint8_t> & raw_packet, uint32_t line_num);

Expand Down
10 changes: 5 additions & 5 deletions livox_driver/include/LidarDriver/livox_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,12 +53,12 @@ struct LivoxImuPoint
#pragma pack(1)
struct LivoxPointXyzrtl
{
float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */
float x; /**< X axis, Unit:m */
float y; /**< Y axis, Unit:m */
float z; /**< Z axis, Unit:m */
float reflectivity; /**< Reflectivity */
uint8_t tag; /**< Livox point tag */
uint8_t line; /**< Laser line id */
uint8_t tag; /**< Livox point tag */
uint8_t line; /**< Laser line id */
};
#pragma pack()

Expand Down
6 changes: 4 additions & 2 deletions livox_driver/src/lidar_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,8 @@ bool LidarDriver::SetConfiguration(const livox_driver::LivoxSensorConfiguration
if (is_success) {
switch (param.sensor_model) {
case livox_driver::LivoxSensorModel::HORIZON:
lidars_.device_info_type = DeviceType::kLidarHorizon;
lidar_device_.device_info_type = DeviceType::kLidarHorizon;
lidar_device_.line_num = GetLaserLineNumber(lidar_device_.device_info_type);
break;
default:
is_success = false;
Expand Down Expand Up @@ -277,7 +278,7 @@ CommandResult LidarDriver::StartHwTxInterface()
/// @brief Tx socket close and stop send thread
void LidarDriver::StopHwTxInterface()
{
lidarstatus_ = LidarDriverStatus::kDisconnect;
driverstatus_ = DriverStatus::kTerminate;
thread_txheartbeat_->join();
commandsock_.Close();
}
Expand Down Expand Up @@ -353,6 +354,7 @@ void LidarDriver::LivoxHwRxInterfaceCommand()
if (lidarstatus_ == LidarDriverStatus::kConnect) {
lidarstatus_ = LidarDriverStatus::kStreaming;
}
DataRecvInit();

//printf("StartStreaming Loop start\n" );
CommandResult result;
Expand Down
99 changes: 69 additions & 30 deletions livox_driver/src/livox_data_recv.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,16 @@
namespace lidar_driver
{
/// @brief receive before initialize.
void LidarDriver::DataRecvInit() { lidars_.raw_data_type = 0xFF; }
void LidarDriver::DataRecvInit()
{
lidar_device_.raw_data_type = 0xFF;
lidar_device_.data_is_published = false;
if (temp_publish_data_.get()) {
temp_publish_data_->num = 0;
}
accumulate_count_ = 0;
skip_start_packet_ = false;
}

/// @brief Raw data timestamp to nano sec.
/// @param timestamp : Raw data timestamp
Expand Down Expand Up @@ -81,12 +90,12 @@ int64_t LidarDriver::RecvTimeBase(
/// @param data_type : raw data type
void LidarDriver::UpdateLidarInfoByEthPacket(uint8_t data_type)
{
if (lidars_.raw_data_type != data_type) {
lidars_.raw_data_type = data_type;
lidars_.packet_interval = GetPacketInterval(lidars_.device_info_type, data_type);
lidars_.packet_interval_max = lidars_.packet_interval * 1.8f;
lidars_.onetime_publish_packets =
GetPacketNumPerSec(lidars_.device_info_type, data_type) * buffer_time_ms_ / 1000;
if (lidar_device_.raw_data_type != data_type) {
lidar_device_.raw_data_type = data_type;
lidar_device_.packet_interval = GetPacketInterval(lidar_device_.device_info_type, data_type);
lidar_device_.packet_interval_max = lidar_device_.packet_interval * 1.8f;
lidar_device_.onetime_publish_packets =
GetPacketNumPerSec(lidar_device_.device_info_type, data_type) * buffer_time_ms_ / 1000;
}
}

Expand Down Expand Up @@ -124,7 +133,8 @@ void LidarDriver::StorageRawPacket(const std::vector<uint8_t> & buff, int rcv_le
if (eth_packet->data_type == PointDataType::kImu) {
if (publish_imu_packet_cb_) {
if (timestamp_type == TimestampType::kPps) {
time_base = RecvTimeBase(lidars_.statistic_info_imu, timestamp_type, cur_timestamp.stamp);
time_base =
RecvTimeBase(lidar_device_.statistic_info_imu, timestamp_type, cur_timestamp.stamp);
time_stamp += time_base;
}
const livox_driver::LivoxImuPoint * ptr =
Expand All @@ -134,7 +144,8 @@ void LidarDriver::StorageRawPacket(const std::vector<uint8_t> & buff, int rcv_le
} else { /* if (PointDataType::kImu != eth_packet->data_type) */
if (publish_lidar_data_cb_) {
if (timestamp_type == TimestampType::kPps) {
time_base = RecvTimeBase(lidars_.statistic_info_data, timestamp_type, cur_timestamp.stamp);
time_base =
RecvTimeBase(lidar_device_.statistic_info_data, timestamp_type, cur_timestamp.stamp);
time_stamp += time_base;
}
UpdateLidarInfoByEthPacket(eth_packet->data_type);
Expand All @@ -148,8 +159,6 @@ void LidarDriver::StorageRawPacket(const std::vector<uint8_t> & buff, int rcv_le
/// @brief data port receive
void LidarDriver::LivoxHwRxInterfaceData()
{
DataRecvInit();

while (driverstatus_ == DriverStatus::kRunning) {
std::vector<uint8_t> buff(kMaxBufferSize);
int rcv_len = rx_datasock_.Recv(buff);
Expand Down Expand Up @@ -182,12 +191,51 @@ void LidarDriver::LivoxHwRxInterfaceImu()
} else { /* nothing */
}
}

//printf("Thread end %s\n", __func__);
return;
}

/// @brief set start timestamp to last_timestamp_
/// @param timestamp : start timestamp
/// @retval true: success
/// @retval false: skip start packet
bool LidarDriver::SetLastTimeStamp(uint64_t timestamp)
{
uint32_t remaining_time = timestamp % publish_period_ns_;
uint32_t diff_time = publish_period_ns_ - remaining_time;

if (skip_start_packet_) {
if (last_remaning_time_ > remaining_time) {
skip_start_packet_ = false;
} else if (diff_time <= lidar_device_.packet_interval) {
skip_start_packet_ = false;
} else {
last_remaning_time_ = remaining_time;
return false;
}
}

/** Get start time, down to the period boundary */
if (diff_time > (publish_period_ns_ / 4)) {
//printf("GetPublishStartTime : 0 : diff_time=%u timestamp=%lu\n", diff_time, timestamp);
last_timestamp_ = timestamp - remaining_time;
} else if (diff_time <= lidar_device_.packet_interval_max) {
//printf("GetPublishStartTime : 1 : diff_time=%u timestamp=%lu\n", diff_time, timestamp);
last_timestamp_ = timestamp;
} else {
//printf("GetPublishStartTime : 2 : diff_time=%u timestamp=%lu\n", diff_time, timestamp);
/* the remaning packets in queue maybe not enough after skip */
last_remaning_time_ = remaining_time;
skip_start_packet_ = true;
return false;
}

return true;
}

/// @brief packet data parse
/// @retval 0: accumulating
/// @retval >0: Accumulation completed data count
int LidarDriver::ParsePacket(livox_driver::LivoxLidarPacket & packet)
{
//printf("parse packet packet.time %ld temp.time %ld \n", packet.time, temp_publish_data_->time);
Expand All @@ -200,29 +248,18 @@ int LidarDriver::ParsePacket(livox_driver::LivoxLidarPacket & packet)
}

if (temp_publish_data_->num == 0) {
uint32_t remaining_time = timestamp % publish_period_ns_;
uint32_t diff_time = publish_period_ns_ - remaining_time;
/** Get start time, down to the period boundary */
if (diff_time > (publish_period_ns_ / 4)) {
//printf("GetPublishStartTime : 0 : diff_time=%u timestamp=%lu\n", diff_time, timestamp);
last_timestamp_ = timestamp - remaining_time;
} else if (diff_time <= lidars_.packet_interval_max) {
//printf("GetPublishStartTime : 1 : diff_time=%u timestamp=%lu\n", diff_time, timestamp);
last_timestamp_ = timestamp;
} else {
//printf("GetPublishStartTime : 2 : diff_time=%u timestamp=%lu\n", diff_time, timestamp);
/* the remaning packets in queue maybe not enough after skip */
if (!SetLastTimeStamp(timestamp)) {
return 0;
}
}

do {
int64_t time_gap = timestamp - last_timestamp_;
// if timeout -> packet publish
if (time_gap > lidars_.packet_interval_max) {
if (time_gap > lidar_device_.packet_interval_max && lidar_device_.data_is_published) {
//printf( "NG:time_gap=%ld timestamp=%ld last_timestamp=%ld\n", time_gap, timestamp, last_timestamp_ );
packet.data.clear();
last_timestamp_ += lidars_.packet_interval;
last_timestamp_ += lidar_device_.packet_interval;
timegap_over = true;
} else {
//printf( "OK:time_gap=%ld timestamp=%ld last_timestamp=%ld\n", time_gap, timestamp, last_timestamp_ );
Expand All @@ -234,25 +271,27 @@ int LidarDriver::ParsePacket(livox_driver::LivoxLidarPacket & packet)
/* new timestamp */
temp_publish_data_->time = timestamp;
temp_publish_data_->data.resize(
lidars_.onetime_publish_packets * kMaxPointPerEthPacket *
lidar_device_.onetime_publish_packets * kMaxPointPerEthPacket *
sizeof(livox_driver::LivoxPointXyzrtl));
}

uint8_t * point_base =
&temp_publish_data_->data[temp_publish_data_->num * sizeof(livox_driver::LivoxPointXyzrtl)];

LivoxExtendRawPointToPxyzrtl(point_base, packet.data, 1);
LivoxExtendRawPointToPxyzrtl(point_base, packet.data, lidar_device_.line_num);
temp_publish_data_->num += packet.data_cnt;

accumulate_count_++;

if (accumulate_count_ >= lidars_.onetime_publish_packets) {
uint32_t echo_num = GetEchoNumPerPoint(lidars_.raw_data_type);
if (accumulate_count_ >= lidar_device_.onetime_publish_packets) {
uint32_t echo_num = GetEchoNumPerPoint(lidar_device_.raw_data_type);
temp_publish_data_->num = temp_publish_data_->num * echo_num;
publish_data_ = std::move(temp_publish_data_);
iret = publish_data_->num;
lidar_device_.data_is_published = true;

accumulate_count_ = 0;
skip_start_packet_ = false;
break;
}
} while (timegap_over);
Expand Down
8 changes: 8 additions & 0 deletions livox_driver/src/livox_data_recv.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -244,6 +244,14 @@ inline int32_t GetEthPacketLen(uint8_t data_type)
return g_data_type_info_pair_table[data_type].packet_length;
}

/// @brief laser line number
/// @param product_type : product type
/// @return laser line number
inline uint32_t GetLaserLineNumber(uint32_t product_type)
{
return g_product_type_info_pair_table[product_type].line_num;
}

/// @brief length of raw ethenet packet unit:bytes
/// @param data_type : receive data type
/// @return points per packet
Expand Down

0 comments on commit 2bd9534

Please sign in to comment.