From d9fff6f23a63489f1c8953677ba8a817a44ae061 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 30 Aug 2023 08:54:51 -0700 Subject: [PATCH 1/3] Apply destagger to laser scan + Add laser to rviz --- ouster-ros/config/viz.rviz | 54 +++++++++++++++++++++----- ouster-ros/include/ouster_ros/os_ros.h | 4 +- ouster-ros/src/laser_scan_processor.h | 4 +- ouster-ros/src/os_ros.cpp | 14 ++++--- 4 files changed, 60 insertions(+), 16 deletions(-) diff --git a/ouster-ros/config/viz.rviz b/ouster-ros/config/viz.rviz index cbff6cf5..cc28e1dd 100644 --- a/ouster-ros/config/viz.rviz +++ b/ouster-ros/config/viz.rviz @@ -13,7 +13,9 @@ Panels: - /nearir1/Topic1 - /reflec1 - /signal1 - Splitter Ratio: 0.5 + - /LaserScan1 + - /LaserScan1/Topic1 + Splitter Ratio: 0.626074492931366 Tree Height: 1185 - Class: rviz_common/Selection Name: Selection @@ -87,23 +89,23 @@ Visualization Manager: Enabled: true Frame Timeout: 15 Frames: - " os_imu": + All Enabled: true + os_imu: Value: true - " os_lidar": + os_lidar: Value: true - " os_sensor": + os_sensor: Value: true - All Enabled: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: false Tree: - " os_sensor": - " os_imu": + os_sensor: + os_imu: {} - " os_lidar": + os_lidar: {} Update Interval: 0 Value: true @@ -163,10 +165,44 @@ Visualization Manager: Reliability Policy: Best Effort Value: /ouster/signal_image Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1000 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /ouster/scan + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: "os_sensor" + Fixed Frame: os_sensor Frame Rate: 30 Name: root Tools: diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h index 214d5b37..3a5c3a37 100644 --- a/ouster-ros/include/ouster_ros/os_ros.h +++ b/ouster-ros/include/ouster_ros/os_ros.h @@ -179,6 +179,8 @@ geometry_msgs::msg::TransformStamped transform_to_tf_msg( * @param[in] frame the parent frame of the generated laser scan message * @param[in] lidar_mode lidar mode (width x frequency) * @param[in] ring selected ring to be published + * @param[in] pixel_shift_by_row pixel shifts by row + * @param[in] return_index index of return desired starting at 0 * @return ROS message suitable for publishing as a LaserScan */ sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg( @@ -186,7 +188,7 @@ sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg( const rclcpp::Time& timestamp, const std::string &frame, const ouster::sensor::lidar_mode lidar_mode, - const uint16_t ring, + const uint16_t ring, const std::vector& pixel_shift_by_row, const int return_index); namespace impl { diff --git a/ouster-ros/src/laser_scan_processor.h b/ouster-ros/src/laser_scan_processor.h index 63dae838..7861e7a6 100644 --- a/ouster-ros/src/laser_scan_processor.h +++ b/ouster-ros/src/laser_scan_processor.h @@ -29,6 +29,7 @@ class LaserScanProcessor { : frame(frame_id), ld_mode(info.mode), ring_(ring), + pixel_shift_by_row(info.format.pixel_shift_by_row), scan_msgs(get_n_returns(info), std::make_shared()), post_processing_fn(func) {} @@ -38,7 +39,7 @@ class LaserScanProcessor { const rclcpp::Time& msg_ts) { for (int i = 0; i < static_cast(scan_msgs.size()); ++i) { *scan_msgs[i] = lidar_scan_to_laser_scan_msg( - lidar_scan, msg_ts, frame, ld_mode, ring_, i); + lidar_scan, msg_ts, frame, ld_mode, ring_, pixel_shift_by_row, i); } if (post_processing_fn) post_processing_fn(scan_msgs); @@ -61,6 +62,7 @@ class LaserScanProcessor { std::string frame; sensor::lidar_mode ld_mode; uint16_t ring_; + std::vector pixel_shift_by_row; OutputType scan_msgs; PostProcessingFn post_processing_fn; }; diff --git a/ouster-ros/src/os_ros.cpp b/ouster-ros/src/os_ros.cpp index 9e1b3745..0b3a01ef 100644 --- a/ouster-ros/src/os_ros.cpp +++ b/ouster-ros/src/os_ros.cpp @@ -354,7 +354,8 @@ geometry_msgs::msg::TransformStamped transform_to_tf_msg( sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg( const ouster::LidarScan& ls, const rclcpp::Time& timestamp, const std::string& frame, const ouster::sensor::lidar_mode ld_mode, - const uint16_t ring, const int return_index) { + const uint16_t ring, const std::vector& pixel_shift_by_row, + const int return_index) { sensor_msgs::msg::LaserScan msg; msg.header.stamp = timestamp; msg.header.frame_id = frame; @@ -380,10 +381,13 @@ sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg( const auto sg = signal.data(); msg.ranges.resize(ls.w); msg.intensities.resize(ls.w); - int idx = ls.w * ring + ls.w; - for (int i = 0; idx-- > ls.w * ring; ++i) { - msg.ranges[i] = rg[idx] * ouster::sensor::range_unit; - msg.intensities[i] = static_cast(sg[idx]); + + const auto u = ring; + for (auto v = 0; v < ls.w; ++v) { + const auto v_shift = (ls.w - 1 - (v + ls.w / 2 + pixel_shift_by_row[u])) % ls.w; + const auto src_idx = u * ls.w + v_shift; + msg.ranges[v] = rg[src_idx] * ouster::sensor::range_unit; + msg.intensities[v] = static_cast(sg[src_idx]); } return msg; From a534eae58be967d8a936d765b7b2141121cc4e45 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 4 Sep 2023 13:11:18 -0700 Subject: [PATCH 2/3] Align LaserScan with the PointCloud --- ouster-ros/src/os_ros.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ouster-ros/src/os_ros.cpp b/ouster-ros/src/os_ros.cpp index 0b3a01ef..e840c9e7 100644 --- a/ouster-ros/src/os_ros.cpp +++ b/ouster-ros/src/os_ros.cpp @@ -382,10 +382,11 @@ sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg( msg.ranges.resize(ls.w); msg.intensities.resize(ls.w); - const auto u = ring; for (auto v = 0; v < ls.w; ++v) { - const auto v_shift = (ls.w - 1 - (v + ls.w / 2 + pixel_shift_by_row[u])) % ls.w; - const auto src_idx = u * ls.w + v_shift; + auto idx = ls.w / 2 - 1 - v; + auto sign = v < ls.w / 2 ? +1 : -1; + auto v_shift = (idx - sign * pixel_shift_by_row[ring]) % ls.w; + auto src_idx = ring * ls.w + v_shift; msg.ranges[v] = rg[src_idx] * ouster::sensor::range_unit; msg.intensities[v] = static_cast(sg[src_idx]); } From 5130793d67c1cfe134dc41de9385494f22a34441 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Fri, 8 Sep 2023 12:57:32 -0700 Subject: [PATCH 3/3] Remove unnecessary sign swtiching --- ouster-ros/src/os_ros.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ouster-ros/src/os_ros.cpp b/ouster-ros/src/os_ros.cpp index e840c9e7..6872b7ec 100644 --- a/ouster-ros/src/os_ros.cpp +++ b/ouster-ros/src/os_ros.cpp @@ -383,9 +383,7 @@ sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg( msg.intensities.resize(ls.w); for (auto v = 0; v < ls.w; ++v) { - auto idx = ls.w / 2 - 1 - v; - auto sign = v < ls.w / 2 ? +1 : -1; - auto v_shift = (idx - sign * pixel_shift_by_row[ring]) % ls.w; + auto v_shift = (ls.w / 2 - 1 - v - pixel_shift_by_row[ring]) % ls.w; auto src_idx = ring * ls.w + v_shift; msg.ranges[v] = rg[src_idx] * ouster::sensor::range_unit; msg.intensities[v] = static_cast(sg[src_idx]);