diff --git a/livox_ros_driver/livox_ros_driver/lddc.cpp b/livox_ros_driver/livox_ros_driver/lddc.cpp index 7890d28..14971f8 100644 --- a/livox_ros_driver/livox_ros_driver/lddc.cpp +++ b/livox_ros_driver/livox_ros_driver/lddc.cpp @@ -476,7 +476,7 @@ void Lddc::PollingLidarPointCloudData(uint8_t handle, LidarDevice *lidar) { while (!QueueIsEmpty(p_queue)) { uint32_t used_size = QueueUsedSize(p_queue); uint32_t onetime_publish_packets = - GetPacketNumPerSec(lidar->info.type) / publish_frq_; + GetPacketNumPerSec(lidar->pointcloud_data_type) / publish_frq_; if (used_size < onetime_publish_packets) { break; } diff --git a/livox_ros_driver/livox_ros_driver/lds.h b/livox_ros_driver/livox_ros_driver/lds.h index 0be22d2..198bf3e 100644 --- a/livox_ros_driver/livox_ros_driver/lds.h +++ b/livox_ros_driver/livox_ros_driver/lds.h @@ -169,6 +169,7 @@ typedef struct { bool data_is_pubulished; /**< Indicate the data of lidar whether is pubulished. */ volatile LidarConnectState connect_state; + uint8_t pointcloud_data_type; DeviceInfo info; LidarPacketStatistic statistic_info; LidarDataQueue data; @@ -220,7 +221,7 @@ const PacketInfoPair packet_info_pair_table[kMaxPointDataType] = { {100, 100000, 10000, 1000000, 1318}, {100, 100000, 10000, 1000000, 918}, {96, 240000, 4167, 400000, 1362}, {96, 240000, 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. diff --git a/livox_ros_driver/livox_ros_driver/lds_hub.cpp b/livox_ros_driver/livox_ros_driver/lds_hub.cpp index d6894f0..7bda0fc 100644 --- a/livox_ros_driver/livox_ros_driver/lds_hub.cpp +++ b/livox_ros_driver/livox_ros_driver/lds_hub.cpp @@ -162,6 +162,10 @@ void LdsHub::OnHubDataCb(uint8_t hub_handle, LivoxEthPacket *data, sizeof(cur_timestamp)); 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) { /** Whether a new sync frame */ 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); } - if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) { + if (!QueueIsFull(p_queue)) { QueuePushAny(p_queue, (uint8_t *)eth_packet, GetEthPacketLen(eth_packet->data_type), packet_statistic->imu_timebase, diff --git a/livox_ros_driver/livox_ros_driver/lds_lidar.cpp b/livox_ros_driver/livox_ros_driver/lds_lidar.cpp index 5350a08..2c35b8f 100644 --- a/livox_ros_driver/livox_ros_driver/lds_lidar.cpp +++ b/livox_ros_driver/livox_ros_driver/lds_lidar.cpp @@ -175,6 +175,10 @@ void LdsLidar::OnLidarDataCb(uint8_t handle, LivoxEthPacket *data, sizeof(cur_timestamp)); 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 ((cur_timestamp.stamp < packet_statistic->last_timestamp) && (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); } - if (!QueueIsFull(p_queue) && (cur_timestamp.stamp >= 0)) { + if (!QueueIsFull(p_queue)) { QueuePushAny(p_queue, (uint8_t *)eth_packet, GetEthPacketLen(eth_packet->data_type), packet_statistic->timebase,