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:support Tele

Browse Source
This commit is contained in:
Livox-SDK
2020-06-17 22:14:33 +08:00
parent 38b9079763
commit 8d887a85c7
21 changed files with 300 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files

70
livox_ros_driver/config/display_hub_points.rviz
View File

@@ -9,8 +9,8 @@ Panels:
- /Grid1
- /PointCloud1/Autocompute Value Bounds1
- /PointCloud21
Splitter Ratio: 0.500694990158081
Tree Height: 728
Splitter Ratio: 0.500695
Tree Height: 680
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -19,7 +19,7 @@ Panels:
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
@@ -30,10 +30,6 @@ Panels:
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
@@ -43,7 +39,7 @@ Visualization Manager:
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
@@ -58,8 +54,8 @@ Visualization Manager:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.8560000061988831
Min Value: -0.7350000143051147
Max Value: 0.856
Min Value: -0.735
Value: true
Axis: Z
Channel Name: x
@@ -70,15 +66,15 @@ Visualization Manager:
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: -0.08799999952316284
Max Intensity: -0.088
Min Color: 0; 0; 0
Min Intensity: -1.9509999752044678
Min Intensity: -1.951
Name: PointCloud
Position Transformer: XYZ
Queue Size: 1000
Selectable: true
Size (Pixels): 2
Size (m): 0.004999999888241291
Size (m): 0.005
Style: Flat Squares
Topic: /livox/lidar
Unreliable: false
@@ -88,8 +84,8 @@ Visualization Manager:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.8159999847412109
Min Value: -0.6740000247955322
Max Value: 0.816
Min Value: -0.674
Value: true
Axis: Z
Channel Name: intensity
@@ -108,7 +104,7 @@ Visualization Manager:
Queue Size: 10
Selectable: true
Size (Pixels): 2
Size (m): 0.004999999888241291
Size (m): 0.005
Style: Points
Topic: /livox/lidar
Unreliable: false
@@ -118,7 +114,6 @@ Visualization Manager:
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: livox_frame
Frame Rate: 50
Name: root
@@ -130,10 +125,7 @@ Visualization Manager:
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
@@ -143,30 +135,27 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 29.202434539794922
Distance: 5.292
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.2672550082206726
Y: 0.061853598803281784
Z: 0.15087400376796722
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
X: 0.267255
Y: 0.0618536
Z: 0.150874
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.679796040058136
Near Clip Distance: 0.01
Pitch: 0.209796
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.0174102783203125
Yaw: 3.21241
Saved:
- Class: rviz/Orbit
Distance: 10
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
@@ -174,22 +163,19 @@ Visualization Manager:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Orbit
Near Clip Distance: 0.009999999776482582
Pitch: 1.1103999614715576
Near Clip Distance: 0.01
Pitch: 1.1104
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.5703970193862915
Yaw: 0.570397
Window Geometry:
Displays:
collapsed: false
Height: 1025
Height: 961
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001a900000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000058e0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000001a900000337fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000337000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000058e0000033700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -199,5 +185,5 @@ Window Geometry:
Views:
collapsed: true
Width: 1853
X: 67
Y: 27
X: 124
Y: 81

10
livox_ros_driver/config/livox_lidar_config.json
View File

@@ -6,8 +6,9 @@
"enable_fan": true,
"return_mode": 0,
"coordinate": 0,
"imu_rate": 1,
"extrinsic_parameter_source": 0
"imu_rate": 0,
"extrinsic_parameter_source": 0,
"enable_high_sensitivity": false
},
{
"broadcast_code": "0TFDG3U99101431",
@@ -15,8 +16,9 @@
"enable_fan": true,
"return_mode": 0,
"coordinate": 0,
"imu_rate": 1,
"extrinsic_parameter_source": 0
"imu_rate": 0,
"extrinsic_parameter_source": 0,
"enable_high_sensitivity": false
}
],

2
livox_ros_driver/launch/livox_hub.launch
View File

@@ -10,6 +10,7 @@
<arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -19,6 +20,7 @@
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"

2
livox_ros_driver/launch/livox_hub_msg.launch
View File

@@ -10,6 +10,7 @@
<arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -19,6 +20,7 @@
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"

2
livox_ros_driver/launch/livox_hub_rviz.launch
View File

@@ -10,6 +10,7 @@
<arg name="rviz_enable" default="true"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -19,6 +20,7 @@
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"

4
livox_ros_driver/launch/livox_lidar.launch
View File

@@ -10,6 +10,7 @@
<arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -19,7 +20,8 @@
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>

4
livox_ros_driver/launch/livox_lidar_msg.launch
View File

@@ -10,6 +10,7 @@
<arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -19,7 +20,8 @@
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>

6
livox_ros_driver/launch/livox_lidar_rviz.launch
View File

@@ -10,6 +10,7 @@
<arg name="rviz_enable" default="true"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -18,8 +19,9 @@
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>

4
livox_ros_driver/launch/livox_template.launch
View File

@@ -10,6 +10,7 @@
<arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -19,8 +20,9 @@
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_lidar_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<node name="livox_lidar_publisher" pkg="livox_coat"
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>

5
livox_ros_driver/launch/lvx_to_rosbag.launch
View File

@@ -10,6 +10,7 @@
<arg name="rviz_enable" default="false"/>
<arg name="rosbag_enable" default="false"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -18,7 +19,9 @@
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>

5
livox_ros_driver/launch/lvx_to_rosbag_rviz.launch
View File

@@ -10,6 +10,7 @@
<arg name="rviz_enable" default="true"/>
<arg name="rosbag_enable" default="true"/>
<arg name="cmdline_arg" default="$(arg bd_list)"/>
<arg name="msg_frame_id" default="livox_frame"/>
<param name="xfer_format" value="$(arg xfer_format)"/>
<param name="multi_topic" value="$(arg multi_topic)"/>
@@ -18,7 +19,9 @@
<param name="output_data_type" value="$(arg output_type)"/>
<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
<param name="user_config_path" type="string" value="$(find livox_ros_driver)/config/livox_hub_config.json"/>
<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
<node name="livox_lidar_publisher" pkg="livox_ros_driver"
type="livox_ros_driver_node" required="true"
output="screen" args="$(arg cmdline_arg)"/>

4
livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h
View File

@@ -26,8 +26,8 @@
#define LIVOX_ROS_DRIVER_INClUDE_LIVOX_ROS_DRIVER_H_
#define LIVOX_ROS_DRIVER_VER_MAJOR 2
#define LIVOX_ROS_DRIVER_VER_MINOR 0
#define LIVOX_ROS_DRIVER_VER_PATCH 1
#define LIVOX_ROS_DRIVER_VER_MINOR 5
#define LIVOX_ROS_DRIVER_VER_PATCH 0
#define GET_STRING(n) GET_STRING_DIRECT(n)
#define GET_STRING_DIRECT(n) #n

184
livox_ros_driver/livox_ros_driver/lddc.cpp
View File

@@ -43,14 +43,13 @@ namespace livox_ros {
typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
/** Lidar Data Distribute Control
* ----------------------------------------------------------------*/
/** Lidar Data Distribute Control--------------------------------------------*/
Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
double frq)
double frq, std::string &frame_id)
: transfer_format_(format), use_multi_topic_(multi_topic),
data_src_(data_src), output_type_(output_type), publish_frq_(frq) {
publish_interval_ms_ = 1000 / publish_frq_;
data_src_(data_src), output_type_(output_type), publish_frq_(frq), \
frame_id_(frame_id) {
publish_period_ns_ = kNsPerSecond / publish_frq_;
lds_ = nullptr;
memset(private_pub_, 0, sizeof(private_pub_));
memset(private_imu_pub_, 0, sizeof(private_imu_pub_));
@@ -62,7 +61,6 @@ Lddc::Lddc(int format, int multi_topic, int data_src, int output_type,
Lddc::~Lddc() {
printf("lddc exit\n\n\n\n");
if (global_pub_) {
delete global_pub_;
}
@@ -88,17 +86,64 @@ Lddc::~Lddc() {
}
}
int32_t Lddc::GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue,
uint64_t *start_time,StoragePacket *storage_packet) {
QueueProPop(queue, storage_packet);
uint64_t timestamp = GetStoragePacketTimestamp(storage_packet,
lidar->data_src);
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)) {
//ROS_INFO("0 : %u", diff_time);
*start_time = timestamp - remaining_time;
return 0;
} else if (diff_time <= lidar->packet_interval_max) {
*start_time = timestamp;
return 0;
} else {
/** Skip some packets up to the period boundary*/
//ROS_INFO("2 : %u", diff_time);
do {
if (QueueIsEmpty(queue)) {
break;
}
QueuePopUpdate(queue); /* skip packet */
QueueProPop(queue, storage_packet);
uint32_t last_remaning_time = remaining_time;
timestamp = GetStoragePacketTimestamp(storage_packet,
lidar->data_src);
remaining_time = timestamp % publish_period_ns_;
/** Flip to another period */
if (last_remaning_time > remaining_time) {
//ROS_INFO("Flip to another period, exit");
break;
}
diff_time = publish_period_ns_ - remaining_time;
} while (diff_time > lidar->packet_interval);
/* the remaning packets in queue maybe not enough after skip */
return -1;
}
}
uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle) {
uint64_t timestamp = 0;
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
StoragePacket storage_packet;
LidarDevice *lidar = &lds_->lidars_[handle];
if (GetPublishStartTime(lidar, queue, &last_timestamp, &storage_packet)) {
/* the remaning packets in queue maybe not enough after skip */
return 0;
}
sensor_msgs::PointCloud2 cloud;
cloud.header.frame_id = "livox_frame";
cloud.header.frame_id.assign(frame_id_);
cloud.height = 1;
cloud.width = 0;
cloud.width = 0;
cloud.fields.resize(6);
cloud.fields[0].offset = 0;
cloud.fields[0].name = "x";
@@ -129,57 +174,48 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
sizeof(LivoxPointXyzrtl));
cloud.point_step = sizeof(LivoxPointXyzrtl);
uint8_t *point_base = cloud.data.data();
uint8_t data_source = lds_->lidars_[handle].data_src;
StoragePacket storage_packet;
while (published_packet < packet_num) {
uint8_t data_source = lidar->data_src;
while ((published_packet < packet_num) && !QueueIsEmpty(queue)) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket *raw_packet =
reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
uint32_t packet_interval = GetPacketInterval(raw_packet->data_type);
int64_t packet_loss_threshold_lower = packet_interval + packet_interval / 2;
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
int64_t packet_gap = timestamp - last_timestamp;
if (published_packet && (packet_gap > packet_loss_threshold_lower) &&
lds_->lidars_[handle].data_is_pubulished) {
ROS_INFO("Lidar[%d] packet loss, interval is %ldus", handle, packet_gap);
if ((packet_gap > lidar->packet_interval_max) &&
lidar->data_is_pubulished) {
//ROS_INFO("Lidar[%d] packet time interval is %ldns", handle, packet_gap);
if (kSourceLvxFile != data_source) {
// ROS_INFO("Lidar[%d] packet loss %ld %d %d", handle,
// packet_loss_threshold_lower, packet_interval, raw_packet->data_type);
int64_t packet_loss_threshold_upper = packet_interval * packet_num;
if (packet_gap >
packet_loss_threshold_upper) { // skip when gap is too large
break;
}
point_base = FillZeroPointXyzrtl(point_base, storage_packet.point_num);
cloud.width += storage_packet.point_num;
last_timestamp = last_timestamp + packet_interval;
last_timestamp = last_timestamp + lidar->packet_interval;
if (!published_packet) {
cloud.header.stamp = ros::Time(last_timestamp / 1000000000.0);
}
++published_packet;
continue;
}
}
if (!published_packet) { // use the first packet timestamp as pointcloud2
// msg timestamp
/** Use the first packet timestamp as pointcloud2 msg timestamp */
if (!published_packet) {
cloud.header.stamp = ros::Time(timestamp / 1000000000.0);
}
cloud.width += storage_packet.point_num;
if (kSourceLvxFile != data_source) {
PointConvertHandler pf_point_convert =
GetConvertHandler(raw_packet->data_type);
GetConvertHandler(lidar->raw_data_type);
if (pf_point_convert) {
point_base = pf_point_convert(
point_base, raw_packet, lds_->lidars_[handle].extrinsic_parameter);
point_base, raw_packet, lidar->extrinsic_parameter);
} else {
/* Skip the packet */
/** Skip the packet */
ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
raw_packet->data_type);
break;
}
} else {
point_base = LivoxPointToPxyzrtl(
point_base, raw_packet, lds_->lidars_[handle].extrinsic_parameter);
point_base, raw_packet, lidar->extrinsic_parameter);
}
QueuePopUpdate(queue);
@@ -190,7 +226,7 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
cloud.row_step = cloud.width * cloud.point_step;
cloud.is_bigendian = false;
cloud.is_dense = true;
cloud.data.resize(cloud.row_step); // adjust to the real size
cloud.data.resize(cloud.row_step); /** Adjust to the real size */
ros::Publisher *p_publisher = Lddc::GetCurrentPublisher(handle);
if (kOutputToRos == output_type_) {
@@ -202,8 +238,8 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
}
}
if (!lds_->lidars_[handle].data_is_pubulished) {
lds_->lidars_[handle].data_is_pubulished = true;
if (!lidar->data_is_pubulished) {
lidar->data_is_pubulished = true;
}
return published_packet;
@@ -216,37 +252,38 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
uint64_t last_timestamp = 0;
uint32_t published_packet = 0;
StoragePacket storage_packet;
LidarDevice *lidar = &lds_->lidars_[handle];
if (GetPublishStartTime(lidar, queue, &last_timestamp, &storage_packet)) {
/* the remaning packets in queue maybe not enough after skip */
return 0;
}
/* init point cloud data struct */
PointCloud::Ptr cloud(new PointCloud);
cloud->header.frame_id = "livox_frame";
// cloud->header.stamp = ros::Time::now();
cloud->header.frame_id.assign(frame_id_);
/* cloud->header.stamp = ros::Time::now(); */
cloud->height = 1;
cloud->width = 0;
cloud->width = 0;
uint8_t data_source = lds_->lidars_[handle].data_src;
StoragePacket storage_packet;
while (published_packet < packet_num) {
uint8_t data_source = lidar->data_src;
while ((published_packet < packet_num) && !QueueIsEmpty(queue)) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket *raw_packet =
reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
uint32_t packet_interval = GetPacketInterval(raw_packet->data_type);
int64_t packet_loss_threshold_lower = packet_interval + packet_interval / 2;
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
int64_t packet_gap = timestamp - last_timestamp;
if ((packet_gap > packet_loss_threshold_lower) && published_packet &&
lds_->lidars_[handle].data_is_pubulished) {
ROS_INFO("Lidar[%d] packet loss, interval is %ldus", handle, packet_gap);
int64_t packet_loss_threshold_upper = packet_interval * packet_num;
if (packet_gap >
packet_loss_threshold_upper) { // skip when gap is too large
break;
}
pcl::PointXYZI point = {0}; // fill zero points
if ((packet_gap > lidar->packet_interval_max) &&
lidar->data_is_pubulished) {
//ROS_INFO("Lidar[%d] packet time interval is %ldns", handle, packet_gap);
pcl::PointXYZI point = {0}; /* fill zero points */
for (uint32_t i = 0; i < storage_packet.point_num; i++) {
cloud->points.push_back(point);
}
last_timestamp = last_timestamp + packet_interval;
last_timestamp = last_timestamp + lidar->packet_interval;
if (!published_packet) {
cloud->header.stamp = last_timestamp / 1000.0; // to pcl ros time stamp
}
++published_packet;
continue;
}
@@ -258,10 +295,10 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
uint8_t point_buf[2048];
if (kSourceLvxFile != data_source) {
PointConvertHandler pf_point_convert =
GetConvertHandler(raw_packet->data_type);
GetConvertHandler(lidar->raw_data_type);
if (pf_point_convert) {
pf_point_convert(point_buf, raw_packet,
lds_->lidars_[handle].extrinsic_parameter);
lidar->extrinsic_parameter);
} else {
/* Skip the packet */
ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
@@ -270,7 +307,7 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
}
} else {
LivoxPointToPxyzrtl(point_buf, raw_packet,
lds_->lidars_[handle].extrinsic_parameter);
lidar->extrinsic_parameter);
}
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
@@ -299,8 +336,8 @@ uint32_t Lddc::PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
}
}
if (!lds_->lidars_[handle].data_is_pubulished) {
lds_->lidars_[handle].data_is_pubulished = true;
if (!lidar->data_is_pubulished) {
lidar->data_is_pubulished = true;
}
return published_packet;
@@ -316,7 +353,9 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
livox_ros_driver::CustomMsg livox_msg;
livox_msg.header.frame_id = "livox_frame";
//livox_msg.header.frame_id = "livox_frame";
livox_msg.header.frame_id.assign(frame_id_);
livox_msg.header.seq = msg_seq;
++msg_seq;
// livox_msg.header.stamp = ros::Time::now();
@@ -324,13 +363,14 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
livox_msg.point_num = 0;
livox_msg.lidar_id = handle;
LidarDevice *lidar = &lds_->lidars_[handle];
uint8_t data_source = lds_->lidars_[handle].data_src;
StoragePacket storage_packet;
while (published_packet < packet_num) {
QueueProPop(queue, &storage_packet);
LivoxEthPacket *raw_packet =
reinterpret_cast<LivoxEthPacket *>(storage_packet.raw_data);
uint32_t point_interval = GetPointInterval(raw_packet->data_type);
uint32_t point_interval = GetPointInterval(lidar->raw_data_type);
uint32_t dual_point = 0;
if ((raw_packet->data_type == kDualExtendCartesian) ||
(raw_packet->data_type == kDualExtendSpherical)) {
@@ -339,7 +379,7 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
timestamp = GetStoragePacketTimestamp(&storage_packet, data_source);
if (((timestamp - last_timestamp) > kDeviceDisconnectThreshold) &&
published_packet && lds_->lidars_[handle].data_is_pubulished) {
published_packet && lidar->data_is_pubulished) {
ROS_INFO("Lidar[%d] packet loss", handle);
break;
}
@@ -357,19 +397,19 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
uint8_t point_buf[2048];
if (kSourceLvxFile != data_source) {
PointConvertHandler pf_point_convert =
GetConvertHandler(raw_packet->data_type);
GetConvertHandler(lidar->raw_data_type);
if (pf_point_convert) {
pf_point_convert(point_buf, raw_packet,
lds_->lidars_[handle].extrinsic_parameter);
lidar->extrinsic_parameter);
} else {
/* Skip the packet */
ROS_INFO("Lidar[%d] unkown packet type[%d]", handle,
raw_packet->data_type);
lidar->raw_data_type);
break;
}
} else {
LivoxPointToPxyzrtl(point_buf, raw_packet,
lds_->lidars_[handle].extrinsic_parameter);
lidar->extrinsic_parameter);
}
LivoxPointXyzrtl *dst_point = (LivoxPointXyzrtl *)point_buf;
@@ -405,8 +445,8 @@ uint32_t Lddc::PublishCustomPointcloud(LidarDataQueue *queue,
}
}
if (!lds_->lidars_[handle].data_is_pubulished) {
lds_->lidars_[handle].data_is_pubulished = true;
if (!lidar->data_is_pubulished) {
lidar->data_is_pubulished = true;
}
return published_packet;
@@ -476,7 +516,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->pointcloud_data_type) / publish_frq_;
GetPacketNumPerSec(lidar->raw_data_type) / publish_frq_;
if (used_size < onetime_publish_packets) {
break;
}

8
livox_ros_driver/livox_ros_driver/lddc.h
View File

@@ -41,7 +41,8 @@ typedef enum {
class Lddc {
public:
Lddc(int format, int multi_topic, int data_src, int output_type, double frq);
Lddc(int format, int multi_topic, int data_src, int output_type, double frq, \
std::string &frame_id);
~Lddc();
int RegisterLds(Lds *lds);
@@ -59,6 +60,8 @@ public:
Lds *lds_;
private:
int32_t GetPublishStartTime(LidarDevice *lidar, LidarDataQueue *queue,
uint64_t *start_time, StoragePacket *storage_packet);
uint32_t PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num,
uint8_t handle);
uint32_t PublishPointcloudData(LidarDataQueue *queue, uint32_t packet_num,
@@ -78,7 +81,8 @@ private:
uint8_t data_src_;
uint8_t output_type_;
double publish_frq_;
int32_t publish_interval_ms_;
uint32_t publish_period_ns_;
std::string frame_id_;
ros::Publisher *private_pub_[kMaxSourceLidar];
ros::Publisher *global_pub_;
ros::Publisher *private_imu_pub_[kMaxSourceLidar];

1
livox_ros_driver/livox_ros_driver/lds.cpp
View File

@@ -441,6 +441,7 @@ void Lds::ResetLidar(LidarDevice *lidar, uint8_t data_src) {
lidar->data_src = data_src;
lidar->data_is_pubulished = false;
lidar->connect_state = kConnectStateOff;
lidar->raw_data_type = 0xFF;
}
void Lds::SetLidarDataSrc(LidarDevice *lidar, uint8_t data_src) {

21
livox_ros_driver/livox_ros_driver/lds.h
View File

@@ -95,11 +95,12 @@ typedef enum {
typedef enum { kCoordinateCartesian = 0, kCoordinateSpherical } CoordinateType;
typedef enum {
kConfigFan = 1,
kConfigReturnMode = 2,
kConfigCoordinate = 4,
kConfigImuRate = 8,
kConfigGetExtrinsicParameter = 16,
kConfigFan = 1 << 0,
kConfigReturnMode = 1 << 1,
kConfigCoordinate = 1 << 2,
kConfigImuRate = 1 << 3,
kConfigGetExtrinsicParameter = 1 << 4,
kConfigSetHighSensitivity = 1 << 5,
kConfigUndef
} LidarConfigCodeBit;
@@ -139,6 +140,7 @@ typedef struct {
uint32_t coordinate;
uint32_t imu_rate;
uint32_t extrinsic_parameter_source;
bool enable_high_sensitivity;
} UserRawConfig;
typedef struct {
@@ -147,6 +149,7 @@ typedef struct {
uint32_t coordinate;
uint32_t imu_rate;
uint32_t extrinsic_parameter_source;
bool enable_high_sensitivity;
volatile uint32_t set_bits;
volatile uint32_t get_bits;
} UserConfig;
@@ -166,10 +169,14 @@ typedef struct {
typedef struct {
uint8_t handle; /**< Lidar access handle. */
uint8_t data_src; /**< From raw lidar or livox file. */
uint8_t raw_data_type; /**< The data type in eth packaet */
bool data_is_pubulished; /**< Indicate the data of lidar whether is
pubulished. */
pubulished. */
volatile uint32_t packet_interval;/**< The time interval between packets
of current lidar, unit:ns */
volatile uint32_t packet_interval_max; /**< If more than it,
have packet loss */
volatile LidarConnectState connect_state;
uint8_t pointcloud_data_type;
DeviceInfo info;
LidarPacketStatistic statistic_info;
LidarDataQueue data;

7
livox_ros_driver/livox_ros_driver/lds_hub.cpp
View File

@@ -162,8 +162,11 @@ 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 (p_lidar->raw_data_type != eth_packet->data_type) {
p_lidar->raw_data_type = eth_packet->data_type;
p_lidar->packet_interval = GetPacketInterval(eth_packet->data_type);
p_lidar->packet_interval_max = p_lidar->packet_interval * 1.8f;
}
if (eth_packet->timestamp_type == kTimestampTypePps) {

85
livox_ros_driver/livox_ros_driver/lds_lidar.cpp
View File

@@ -24,15 +24,18 @@
#include "lds_lidar.h"
#include <memory>
#include <stdio.h>
#include <string.h>
#include <memory>
#include <thread>
#include <mutex>
#include "rapidjson/document.h"
#include "rapidjson/filereadstream.h"
#include "rapidjson/stringbuffer.h"
using namespace std;
namespace livox_ros {
/** Const varible
@@ -175,8 +178,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 (p_lidar->raw_data_type != eth_packet->data_type) {
p_lidar->raw_data_type = eth_packet->data_type;
p_lidar->packet_interval = GetPacketInterval(eth_packet->data_type);
p_lidar->packet_interval_max = p_lidar->packet_interval * 1.8f;
}
if (eth_packet->timestamp_type == kTimestampTypePps) {
@@ -278,6 +283,7 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
config.coordinate = kCoordinateCartesian;
config.imu_rate = kImuFreq200Hz;
config.extrinsic_parameter_source = kNoneExtrinsicParameter;
config.enable_high_sensitivity = false;
}
p_lidar->config.enable_fan = config.enable_fan;
@@ -286,6 +292,8 @@ void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info) {
p_lidar->config.imu_rate = config.imu_rate;
p_lidar->config.extrinsic_parameter_source =
config.extrinsic_parameter_source;
p_lidar->config.enable_high_sensitivity =
config.enable_high_sensitivity;
} else {
printf("Add lidar to connect is failed : %d %d \n", result, handle);
}
@@ -325,6 +333,9 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
/** Config lidar parameter */
if (p_lidar->info.state == kLidarStateNormal) {
/** Ensure the thread safety for set_bits and connect_state */
lock_guard<mutex> lock(g_lds_ldiar->config_mutex_);
if (p_lidar->config.coordinate != 0) {
SetSphericalCoordinate(handle, SetCoordinateCb, g_lds_ldiar);
} else {
@@ -349,6 +360,20 @@ void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type) {
kExtrinsicParameterFromLidar) {
LidarGetExtrinsicParameter(handle, GetLidarExtrinsicParameterCb,
g_lds_ldiar);
p_lidar->config.set_bits |= kConfigGetExtrinsicParameter;
}
if (kDeviceTypeLidarTele == info->type) {
if (p_lidar->config.enable_high_sensitivity) {
LidarEnableHighSensitivity(handle, SetHighSensitivityCb,
g_lds_ldiar);
printf("Enable high sensitivity\n");
} else {
LidarDisableHighSensitivity(handle, SetHighSensitivityCb,
g_lds_ldiar);
printf("Disable high sensitivity\n");
}
p_lidar->config.set_bits |= kConfigSetHighSensitivity;
}
p_lidar->connect_state = kConnectStateConfig;
@@ -407,14 +432,14 @@ void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle,
LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode));
printf("Set return mode success!\n");
lock_guard<mutex> lock(lds_lidar->config_mutex_);
p_lidar->config.set_bits &= ~((uint32_t)(kConfigReturnMode));
if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
}
} else {
LidarSetPointCloudReturnMode(
handle, (PointCloudReturnMode)(p_lidar->config.return_mode),
@@ -433,9 +458,10 @@ void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle,
LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
printf("Set coordinate success!\n");
lock_guard<mutex> lock(lds_lidar->config_mutex_);
p_lidar->config.set_bits &= ~((uint32_t)(kConfigCoordinate));
if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
@@ -461,14 +487,14 @@ void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle,
LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate));
printf("Set imu rate success!\n");
lock_guard<mutex> lock(lds_lidar->config_mutex_);
p_lidar->config.set_bits &= ~((uint32_t)(kConfigImuRate));
if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
}
} else {
LidarSetImuPushFrequency(handle, (ImuFreq)(p_lidar->config.imu_rate),
SetImuRatePushFrequencyCb, g_lds_ldiar);
@@ -501,13 +527,14 @@ void LdsLidar::GetLidarExtrinsicParameterCb(
if (p_lidar->config.extrinsic_parameter_source) {
p_extrinsic->enable = true;
}
p_lidar->config.set_bits &= ~((uint32_t)(kConfigGetExtrinsicParameter));
printf("Lidar[%d] get ExtrinsicParameter success!\n", handle);
lock_guard<mutex> lock(lds_lidar->config_mutex_);
p_lidar->config.set_bits &= ~((uint32_t)(kConfigGetExtrinsicParameter));
if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
}
}
} else {
printf("Lidar[%d] get ExtrinsicParameter fail!\n", handle);
}
@@ -516,6 +543,36 @@ void LdsLidar::GetLidarExtrinsicParameterCb(
}
}
void LdsLidar::SetHighSensitivityCb(livox_status status, uint8_t handle,
DeviceParameterResponse *response, void *clent_data) {
LdsLidar *lds_lidar = static_cast<LdsLidar *>(clent_data);
if (handle >= kMaxLidarCount) {
return;
}
LidarDevice *p_lidar = &(lds_lidar->lidars_[handle]);
if (status == kStatusSuccess) {
p_lidar->config.set_bits &= ~((uint32_t)(kConfigSetHighSensitivity));
printf("Set high sensitivity success!\n");
lock_guard<mutex> lock(lds_lidar->config_mutex_);
if (!p_lidar->config.set_bits) {
LidarStartSampling(handle, StartSampleCb, lds_lidar);
p_lidar->connect_state = kConnectStateSampling;
};
} else {
if (p_lidar->config.enable_high_sensitivity) {
LidarEnableHighSensitivity(handle, SetHighSensitivityCb,
g_lds_ldiar);
} else {
LidarDisableHighSensitivity(handle, SetHighSensitivityCb,
g_lds_ldiar);
}
printf("Set high sensitivity fail, try again!\n");
}
}
/** Callback function of starting sampling. */
void LdsLidar::StartSampleCb(livox_status status, uint8_t handle,
uint8_t response, void *clent_data) {
@@ -680,7 +737,7 @@ int LdsLidar::ParseConfigFile(const char *pathname) {
for (size_t i = 0; i < len; i++) {
const rapidjson::Value &object = array[i];
if (object.IsObject()) {
UserRawConfig config;
UserRawConfig config = {0};
memset(&config, 0, sizeof(config));
if (object.HasMember("broadcast_code") &&
object["broadcast_code"].IsString()) {
@@ -714,6 +771,11 @@ int LdsLidar::ParseConfigFile(const char *pathname) {
config.extrinsic_parameter_source =
object["extrinsic_parameter_source"].GetInt();
}
if (object.HasMember("enable_high_sensitivity") &&
object["enable_high_sensitivity"].GetBool()) {
config.enable_high_sensitivity =
object["enable_high_sensitivity"].GetBool();
}
printf("broadcast code[%s] : %d %d %d %d %d %d\n",
config.broadcast_code, config.enable_connect,
@@ -784,6 +846,7 @@ int LdsLidar::GetRawConfig(const char *broadcast_code, UserRawConfig &config) {
config.coordinate = ite_config.coordinate;
config.imu_rate = ite_config.imu_rate;
config.extrinsic_parameter_source = ite_config.extrinsic_parameter_source;
config.enable_high_sensitivity = ite_config.enable_high_sensitivity;
return 0;
}
}

4
livox_ros_driver/livox_ros_driver/lds_lidar.h
View File

@@ -29,6 +29,7 @@
#include <memory>
#include <vector>
#include <mutex>
#include "lds.h"
#include "livox_sdk.h"
@@ -87,6 +88,8 @@ private:
GetLidarExtrinsicParameterCb(livox_status status, uint8_t handle,
LidarGetExtrinsicParameterResponse *response,
void *clent_data);
static void SetHighSensitivityCb(livox_status status, uint8_t handle,
DeviceParameterResponse *response, void *clent_data);
void ResetLdsLidar(void);
int AddBroadcastCodeToWhitelist(const char *broadcast_code);
@@ -110,6 +113,7 @@ private:
bool enable_timesync_;
TimeSync *timesync_;
TimeSyncConfig timesync_config_;
std::mutex config_mutex_;
};
} // namespace livox_ros

11
livox_ros_driver/livox_ros_driver/lds_lvx.cpp
View File

@@ -161,11 +161,18 @@ void LdsLvx::ReadLvxFile() {
eth_packet = (LivoxEthPacket *)(&detail_packet->version);
handle = detail_packet->device_index;
}
data_type = eth_packet->data_type;
data_offset +=
(GetEthPacketLen(data_type) + 1); /* packet length + device index */
/** Packet length + device index */
data_offset += (GetEthPacketLen(data_type) + 1);
if (data_type != kImu) {
LidarDevice *p_lidar = &lidars_[handle];
LidarDataQueue *p_queue = &lidars_[handle].data;
if (p_lidar->raw_data_type != eth_packet->data_type) {
p_lidar->raw_data_type = eth_packet->data_type;
p_lidar->packet_interval = GetPacketInterval(eth_packet->data_type);
p_lidar->packet_interval_max = p_lidar->packet_interval * 1.8f;
}
if ((p_queue != nullptr) && (handle < lidar_count_)) {
while (QueueIsFull(p_queue)) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));

15
livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp
View File

@@ -63,18 +63,27 @@ int main(int argc, char **argv) {
int xfer_format = kPointCloud2Msg;
int multi_topic = 0;
int data_src = kSourceRawLidar;
double publish_freq = 20.0; /* Hz */
double publish_freq = 10.0; /* Hz */
int output_type = kOutputToRos;
std::string frame_id = "livox_frame";
livox_node.getParam("xfer_format", xfer_format);
livox_node.getParam("multi_topic", multi_topic);
livox_node.getParam("data_src", data_src);
livox_node.getParam("publish_freq", publish_freq);
livox_node.getParam("output_data_type", output_type);
livox_node.getParam("frame_id", frame_id);
if (publish_freq > 100.0) {
publish_freq = 100.0;
} else if (publish_freq < 1.0) {
publish_freq = 1.0;
} else {
publish_freq = publish_freq;
}
/** Lidar data distribute control and lidar data source set */
Lddc *lddc =
new Lddc(xfer_format, multi_topic, data_src, output_type, publish_freq);
Lddc *lddc = new Lddc(xfer_format, multi_topic, data_src, output_type, \
publish_freq, frame_id);
lddc->SetRosNode(&livox_node);
int ret = 0;