diff --git a/livox_ros_driver/config/display_hub_points.rviz b/livox_ros_driver/config/display_hub_points.rviz index 32cc93d..dd13d0a 100644 --- a/livox_ros_driver/config/display_hub_points.rviz +++ b/livox_ros_driver/config/display_hub_points.rviz @@ -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: 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: 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 diff --git a/livox_ros_driver/config/livox_lidar_config.json b/livox_ros_driver/config/livox_lidar_config.json index 5063524..c22bf4d 100644 --- a/livox_ros_driver/config/livox_lidar_config.json +++ b/livox_ros_driver/config/livox_lidar_config.json @@ -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 } ], diff --git a/livox_ros_driver/launch/livox_hub.launch b/livox_ros_driver/launch/livox_hub.launch index 133bdef..7923895 100644 --- a/livox_ros_driver/launch/livox_hub.launch +++ b/livox_ros_driver/launch/livox_hub.launch @@ -10,6 +10,7 @@ + @@ -19,6 +20,7 @@ + + @@ -19,6 +20,7 @@ + + @@ -19,6 +20,7 @@ + + @@ -19,7 +20,8 @@ - + + diff --git a/livox_ros_driver/launch/livox_lidar_msg.launch b/livox_ros_driver/launch/livox_lidar_msg.launch index cafd502..6ca18f4 100644 --- a/livox_ros_driver/launch/livox_lidar_msg.launch +++ b/livox_ros_driver/launch/livox_lidar_msg.launch @@ -10,6 +10,7 @@ + @@ -19,7 +20,8 @@ - + + diff --git a/livox_ros_driver/launch/livox_lidar_rviz.launch b/livox_ros_driver/launch/livox_lidar_rviz.launch index c1b9ef3..067e2ad 100644 --- a/livox_ros_driver/launch/livox_lidar_rviz.launch +++ b/livox_ros_driver/launch/livox_lidar_rviz.launch @@ -10,6 +10,7 @@ + @@ -18,8 +19,9 @@ - - + + + diff --git a/livox_ros_driver/launch/livox_template.launch b/livox_ros_driver/launch/livox_template.launch index ce6af13..da0e354 100644 --- a/livox_ros_driver/launch/livox_template.launch +++ b/livox_ros_driver/launch/livox_template.launch @@ -10,6 +10,7 @@ + @@ -19,8 +20,9 @@ + - diff --git a/livox_ros_driver/launch/lvx_to_rosbag.launch b/livox_ros_driver/launch/lvx_to_rosbag.launch index 0bfb2e4..b839db9 100644 --- a/livox_ros_driver/launch/lvx_to_rosbag.launch +++ b/livox_ros_driver/launch/lvx_to_rosbag.launch @@ -10,6 +10,7 @@ + @@ -18,7 +19,9 @@ - + + + diff --git a/livox_ros_driver/launch/lvx_to_rosbag_rviz.launch b/livox_ros_driver/launch/lvx_to_rosbag_rviz.launch index a9fc730..61b52cf 100644 --- a/livox_ros_driver/launch/lvx_to_rosbag_rviz.launch +++ b/livox_ros_driver/launch/lvx_to_rosbag_rviz.launch @@ -10,6 +10,7 @@ + @@ -18,7 +19,9 @@ - + + + diff --git a/livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h b/livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h index 6091dd7..093ed39 100644 --- a/livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h +++ b/livox_ros_driver/livox_ros_driver/include/livox_ros_driver.h @@ -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 diff --git a/livox_ros_driver/livox_ros_driver/lddc.cpp b/livox_ros_driver/livox_ros_driver/lddc.cpp index 14971f8..70be467 100644 --- a/livox_ros_driver/livox_ros_driver/lddc.cpp +++ b/livox_ros_driver/livox_ros_driver/lddc.cpp @@ -43,14 +43,13 @@ namespace livox_ros { typedef pcl::PointCloud 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; - sensor_msgs::PointCloud2 cloud; - cloud.header.frame_id = "livox_frame"; + 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.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(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(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(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; } diff --git a/livox_ros_driver/livox_ros_driver/lddc.h b/livox_ros_driver/livox_ros_driver/lddc.h index 74e05cc..8d71d18 100644 --- a/livox_ros_driver/livox_ros_driver/lddc.h +++ b/livox_ros_driver/livox_ros_driver/lddc.h @@ -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 @@ class Lddc { 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 @@ class Lddc { 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]; diff --git a/livox_ros_driver/livox_ros_driver/lds.cpp b/livox_ros_driver/livox_ros_driver/lds.cpp index 3e47897..07c0685 100644 --- a/livox_ros_driver/livox_ros_driver/lds.cpp +++ b/livox_ros_driver/livox_ros_driver/lds.cpp @@ -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) { diff --git a/livox_ros_driver/livox_ros_driver/lds.h b/livox_ros_driver/livox_ros_driver/lds.h index 198bf3e..8363468 100644 --- a/livox_ros_driver/livox_ros_driver/lds.h +++ b/livox_ros_driver/livox_ros_driver/lds.h @@ -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; diff --git a/livox_ros_driver/livox_ros_driver/lds_hub.cpp b/livox_ros_driver/livox_ros_driver/lds_hub.cpp index 7bda0fc..3a8dea1 100644 --- a/livox_ros_driver/livox_ros_driver/lds_hub.cpp +++ b/livox_ros_driver/livox_ros_driver/lds_hub.cpp @@ -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) { diff --git a/livox_ros_driver/livox_ros_driver/lds_lidar.cpp b/livox_ros_driver/livox_ros_driver/lds_lidar.cpp index 2c35b8f..e76117b 100644 --- a/livox_ros_driver/livox_ros_driver/lds_lidar.cpp +++ b/livox_ros_driver/livox_ros_driver/lds_lidar.cpp @@ -24,15 +24,18 @@ #include "lds_lidar.h" -#include #include #include +#include #include +#include #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 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 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 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 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 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(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 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; } } diff --git a/livox_ros_driver/livox_ros_driver/lds_lidar.h b/livox_ros_driver/livox_ros_driver/lds_lidar.h index fb2c2b0..8f37857 100644 --- a/livox_ros_driver/livox_ros_driver/lds_lidar.h +++ b/livox_ros_driver/livox_ros_driver/lds_lidar.h @@ -29,6 +29,7 @@ #include #include +#include #include "lds.h" #include "livox_sdk.h" @@ -87,6 +88,8 @@ class LdsLidar : public Lds { 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 @@ class LdsLidar : public Lds { bool enable_timesync_; TimeSync *timesync_; TimeSyncConfig timesync_config_; + std::mutex config_mutex_; }; } // namespace livox_ros diff --git a/livox_ros_driver/livox_ros_driver/lds_lvx.cpp b/livox_ros_driver/livox_ros_driver/lds_lvx.cpp index c13b40e..902435e 100644 --- a/livox_ros_driver/livox_ros_driver/lds_lvx.cpp +++ b/livox_ros_driver/livox_ros_driver/lds_lvx.cpp @@ -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)); diff --git a/livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp b/livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp index 5e293c7..795bcfa 100644 --- a/livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp +++ b/livox_ros_driver/livox_ros_driver/livox_ros_driver.cpp @@ -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;