mirrors/livox_ros_driver
1
0
Fork 0
mirror of https://github.com/Livox-SDK/livox_ros_driver.git synced 2023-04-06 15:49:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat:remove restrictions on timestamps

Browse Source
This commit is contained in:
Livox-SDK
2020-05-09 22:20:12 +08:00
parent 90393f32f4
commit 97dd7971a8
4 changed files with 13 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
livox_ros_driver/livox_ros_driver/lddc.cpp
View File

@@ -476,7 +476,7 @@ void Lddc::PollingLidarPointCloudData(uint8_t handle, LidarDevice *lidar) {
while (!QueueIsEmpty(p_queue)) { while (!QueueIsEmpty(p_queue)) {
uint32_t used_size = QueueUsedSize(p_queue); uint32_t used_size = QueueUsedSize(p_queue);
uint32_t onetime_publish_packets = uint32_t onetime_publish_packets =
GetPacketNumPerSec(lidar->info.type) / publish_frq_; GetPacketNumPerSec(lidar->pointcloud_data_type) / publish_frq_;
if (used_size < onetime_publish_packets) { if (used_size < onetime_publish_packets) {
break; break;
} }

3
livox_ros_driver/livox_ros_driver/lds.h
View File

@@ -169,6 +169,7 @@ typedef struct {
bool data_is_pubulished; /**< Indicate the data of lidar whether is bool data_is_pubulished; /**< Indicate the data of lidar whether is
pubulished. */ pubulished. */
volatile LidarConnectState connect_state; volatile LidarConnectState connect_state;
uint8_t pointcloud_data_type;
DeviceInfo info; DeviceInfo info;
LidarPacketStatistic statistic_info; LidarPacketStatistic statistic_info;
LidarDataQueue data; LidarDataQueue data;
@@ -220,7 +221,7 @@ const PacketInfoPair packet_info_pair_table[kMaxPointDataType] = {
{100, 100000, 10000, 1000000, 1318}, {100, 100000, 10000, 1000000, 918}, {100, 100000, 10000, 1000000, 1318}, {100, 100000, 10000, 1000000, 918},
{96, 240000, 4167, 400000, 1362}, {96, 240000, 4167, 400000, 978}, {96, 240000, 4167, 400000, 1362}, {96, 240000, 4167, 400000, 978},
{96, 480000, 4167, 400000, 1362}, {48, 480000, 4167, 400000, 978}, {96, 480000, 4167, 400000, 1362}, {48, 480000, 4167, 400000, 978},
{1, 100, 10000000, 10000000, 42}}; {1, 200, 10000000, 10000000, 42}};
/** /**
* Global function for general use. * Global function for general use.

6
livox_ros_driver/livox_ros_driver/lds_hub.cpp
View File

@@ -162,6 +162,10 @@ void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
sizeof(cur_timestamp)); sizeof(cur_timestamp));
if (kImu != eth_packet->data_type) { if (kImu != eth_packet->data_type) {
if (p_lidar->pointcloud_data_type != eth_packet->data_type) {
p_lidar->pointcloud_data_type = eth_packet->data_type;
}
if (eth_packet->timestamp_type == kTimestampTypePps) { if (eth_packet->timestamp_type == kTimestampTypePps) {
/** Whether a new sync frame */ /** Whether a new sync frame */
if ((cur_timestamp.stamp < packet_statistic->last_timestamp) && if ((cur_timestamp.stamp < packet_statistic->last_timestamp) &&
@@ -210,7 +214,7 @@ void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data,
p_lidar->info.broadcast_code, queue_size, p_queue->size); p_lidar->info.broadcast_code, queue_size, p_queue->size);
} }
if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) { if (!QueueIsFull(p_queue)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet, QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(eth_packet->data_type), GetEthPacketLen(eth_packet->data_type),
packet_statistic->imu_timebase, packet_statistic->imu_timebase,

6
livox_ros_driver/livox_ros_driver/lds_lidar.cpp
View File

@@ -175,6 +175,10 @@ void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,
sizeof(cur_timestamp)); sizeof(cur_timestamp));
if (kImu != eth_packet->data_type) { if (kImu != eth_packet->data_type) {
if (p_lidar->pointcloud_data_type != eth_packet->data_type) {
p_lidar->pointcloud_data_type = eth_packet->data_type;
}
if (eth_packet->timestamp_type == kTimestampTypePps) { if (eth_packet->timestamp_type == kTimestampTypePps) {
if ((cur_timestamp.stamp < packet_statistic->last_timestamp) && if ((cur_timestamp.stamp < packet_statistic->last_timestamp) &&
(cur_timestamp.stamp < kPacketTimeGap)) { // whether a new sync frame (cur_timestamp.stamp < kPacketTimeGap)) { // whether a new sync frame
@@ -225,7 +229,7 @@ void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *data,
p_lidar->info.broadcast_code, queue_size, p_queue->size); p_lidar->info.broadcast_code, queue_size, p_queue->size);
} }
if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) { if (!QueueIsFull(p_queue)) {
QueuePushAny(p_queue, (uint8_t *)eth_packet, QueuePushAny(p_queue, (uint8_t *)eth_packet,
GetEthPacketLen(eth_packet->data_type), GetEthPacketLen(eth_packet->data_type),
packet_statistic->timebase, packet_statistic->timebase,