From c54b177267f4840f0a6ee0cffd5d7af3904112da Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Fri, 29 Sep 2023 09:22:09 -0700 Subject: [PATCH 01/10] Port the alignment solution to ROS1 --- include/ouster_ros/os_ros.h | 10 +++++----- src/laser_scan_processor.h | 4 +++- src/os_ros.cpp | 14 +++++++++----- 3 files changed, 17 insertions(+), 11 deletions(-) diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h index d053732b..c69bccd1 100644 --- a/include/ouster_ros/os_ros.h +++ b/include/ouster_ros/os_ros.h @@ -106,14 +106,14 @@ geometry_msgs::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::LaserScan lidar_scan_to_laser_scan_msg( - const ouster::LidarScan& ls, - const ros::Time& timestamp, - const std::string &frame, - const ouster::sensor::lidar_mode lidar_mode, - const uint16_t ring, + const ouster::LidarScan& ls, const ros::Time& timestamp, + const std::string& frame, const ouster::sensor::lidar_mode lidar_mode, + const uint16_t ring, const std::vector& pixel_shift_by_row, const int return_index); namespace impl { diff --git a/src/laser_scan_processor.h b/src/laser_scan_processor.h index ccd184e7..74960392 100644 --- a/src/laser_scan_processor.h +++ b/src/laser_scan_processor.h @@ -28,6 +28,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) {} @@ -37,7 +38,7 @@ class LaserScanProcessor { const ros::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); @@ -60,6 +61,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/src/os_ros.cpp b/src/os_ros.cpp index 0dd13f67..f2cc7ed2 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -150,7 +150,8 @@ geometry_msgs::TransformStamped transform_to_tf_msg( sensor_msgs::LaserScan lidar_scan_to_laser_scan_msg( const ouster::LidarScan& ls, const ros::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::LaserScan msg; msg.header.stamp = timestamp; msg.header.frame_id = frame; @@ -176,10 +177,13 @@ sensor_msgs::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 9c823b7a5ee6c7c0484469d479b16848433612af Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Fri, 29 Sep 2023 09:54:42 -0700 Subject: [PATCH 02/10] Add LaserScan to RVIZ --- config/viz.rviz | 33 ++++++++++++++++++++++++++++++++- 1 file changed, 32 insertions(+), 1 deletion(-) diff --git a/config/viz.rviz b/config/viz.rviz index 570ec59a..ce066842 100644 --- a/config/viz.rviz +++ b/config/viz.rviz @@ -6,8 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /LaserScan1 Splitter Ratio: 0.5654885768890381 - Tree Height: 756 + Tree Height: 629 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -220,6 +221,36 @@ Visualization Manager: {} Update Interval: 0 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/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 0; 255; 0 + Max Intensity: 1466 + Min Color: 0; 255; 0 + Min Intensity: 9 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /ouster/scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 From 5d59c3c10172896a44645e8bd0294e1e5b9fb55a Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 22 Nov 2023 14:07:10 -0800 Subject: [PATCH 03/10] Push fix + Add LaserScan definition to RIVZ --- config/viz.rviz | 33 ++++++++++++++++----------------- src/os_ros.cpp | 6 +++--- 2 files changed, 19 insertions(+), 20 deletions(-) diff --git a/config/viz.rviz b/config/viz.rviz index ce066842..26eee9b4 100644 --- a/config/viz.rviz +++ b/config/viz.rviz @@ -24,10 +24,9 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: PointCloud2 Preferences: PromptSaveOnExit: false Toolbars: @@ -199,6 +198,8 @@ Visualization Manager: Value: false - Class: rviz/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: false @@ -208,6 +209,7 @@ Visualization Manager: Value: false os_sensor: Value: true + Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true @@ -222,7 +224,7 @@ Visualization Manager: Update Interval: 0 Value: true - Alpha: 1 - Autocompute Intensity Bounds: false + Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 @@ -235,21 +237,19 @@ Visualization Manager: Decay Time: 0 Enabled: true Invert Rainbow: false - Max Color: 0; 255; 0 - Max Intensity: 1466 - Min Color: 0; 255; 0 - Min Intensity: 9 + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 Name: LaserScan Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.01 + Size (m): 0.009999999776482582 Style: Flat Squares Topic: /ouster/scan Unreliable: false Use Fixed Frame: true - Use rainbow: false + Use rainbow: true Value: true Enabled: true Global Options: @@ -279,7 +279,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 45.77256774902344 + Distance: 8.687203407287598 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -295,18 +295,17 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.2703977823257446 + Pitch: 1.5697963237762451 Target Frame: - Value: Orbit (rviz) - Yaw: 4.513584136962891 + Yaw: 4.6935882568359375 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1080 + Height: 2272 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -315,9 +314,9 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 + Width: 3840 X: 0 - Y: 1151 + Y: 54 near ir: collapsed: false range: diff --git a/src/os_ros.cpp b/src/os_ros.cpp index f2cc7ed2..36ac934c 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -178,10 +178,10 @@ sensor_msgs::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 sign = v < ls.w / 2 ? -1 : +1; + auto v_shift = (ls.w / 2 - 1 - v + 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 bff82c1bf7751f627dedaec8b77bf96c307b4062 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 22 Nov 2023 14:20:26 -0800 Subject: [PATCH 04/10] Update CHANGELOG --- CHANGELOG.rst | 1 + package.xml | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index b22bb927..d0ecacd1 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -9,6 +9,7 @@ Changelog other common pcl point types. * ouster_image_nodelet can operate independently from ouster_cloud_nodelet. * install ouster-ros and ouster_client include directories in separate folders. +* [BUGFIX]: LaserScan is not properly aligned with generated point cloud ouster_ros v0.10.0 ================== diff --git a/package.xml b/package.xml index ee3f8425..e0bc6977 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ ouster_ros - 0.12.0 + 0.12.1 Ouster ROS driver ouster developers BSD From 711487ff74aea4e224cd7aab8af6a180c152d4f8 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 22 Nov 2023 18:03:46 -0800 Subject: [PATCH 05/10] * [BUGFIX]: LaserScan does not work when using dual mode --- CHANGELOG.rst | 2 ++ src/laser_scan_processor.h | 7 ++++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index d0ecacd1..89cd6177 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -10,6 +10,8 @@ Changelog * ouster_image_nodelet can operate independently from ouster_cloud_nodelet. * install ouster-ros and ouster_client include directories in separate folders. * [BUGFIX]: LaserScan is not properly aligned with generated point cloud +* [BUGFIX]: LaserScan does not work when using dual mode + ouster_ros v0.10.0 ================== diff --git a/src/laser_scan_processor.h b/src/laser_scan_processor.h index 74960392..2d44a53c 100644 --- a/src/laser_scan_processor.h +++ b/src/laser_scan_processor.h @@ -29,9 +29,10 @@ class LaserScanProcessor { 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) {} + scan_msgs(get_n_returns(info)), post_processing_fn(func) { + for (size_t i = 0; i < scan_msgs.size(); ++i) + scan_msgs[i] = std::make_shared(); + } private: void process(const ouster::LidarScan& lidar_scan, uint64_t, From 39efe3fc9a3b81ba31eac7109742ee6cf6cd8198 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 23 Nov 2023 11:39:32 -0800 Subject: [PATCH 06/10] Use correct shift when assembling LaserScan msg towards on firmwares prior to 2.4 --- include/ouster_ros/os_ros.h | 2 ++ src/laser_scan_processor.h | 22 ++++++++++++++++------ src/os_ros.cpp | 34 ++++++++++++++++++++++++++-------- 3 files changed, 44 insertions(+), 14 deletions(-) diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h index c69bccd1..631aa6ad 100644 --- a/include/ouster_ros/os_ros.h +++ b/include/ouster_ros/os_ros.h @@ -158,6 +158,8 @@ inline bool check_token(const std::set& tokens, return tokens.find(token) != tokens.end(); } +ouster::util::version parse_version(const std::string& fw_rev); + } // namespace impl } // namespace ouster_ros diff --git a/src/laser_scan_processor.h b/src/laser_scan_processor.h index 2d44a53c..2c2a7c82 100644 --- a/src/laser_scan_processor.h +++ b/src/laser_scan_processor.h @@ -29,17 +29,27 @@ class LaserScanProcessor { ld_mode(info.mode), ring_(ring), pixel_shift_by_row(info.format.pixel_shift_by_row), - scan_msgs(get_n_returns(info)), post_processing_fn(func) { - for (size_t i = 0; i < scan_msgs.size(); ++i) - scan_msgs[i] = std::make_shared(); - } + scan_msgs(get_n_returns(info)), + post_processing_fn(func) { + for (size_t i = 0; i < scan_msgs.size(); ++i) + scan_msgs[i] = std::make_shared(); + + const auto fw = impl::parse_version(info.fw_rev); + if (fw.major == 2 && fw.minor < 4) { + std::transform(pixel_shift_by_row.begin(), + pixel_shift_by_row.end(), + pixel_shift_by_row.begin(), + [](auto c) { return c - 31; }); + } + } private: void process(const ouster::LidarScan& lidar_scan, uint64_t, const ros::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_, pixel_shift_by_row, i); + *scan_msgs[i] = + lidar_scan_to_laser_scan_msg(lidar_scan, msg_ts, frame, ld_mode, + ring_, pixel_shift_by_row, i); } if (post_processing_fn) post_processing_fn(scan_msgs); diff --git a/src/os_ros.cpp b/src/os_ros.cpp index 36ac934c..d18a2fb5 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -19,11 +19,14 @@ #include #include #include +#include -namespace sensor = ouster::sensor; namespace ouster_ros { +namespace sensor = ouster::sensor; +using namespace ouster::util; + bool is_legacy_lidar_profile(const sensor::sensor_info& info) { using sensor::UDPProfileLidar; return info.format.udp_profile_lidar == @@ -129,6 +132,23 @@ std::set parse_tokens(const std::string& input, char delim) { return tokens; } +version parse_version(const std::string& fw_rev) { + auto rgx = std::regex(R"(v(\d+).(\d+)\.(\d+))"); + std::smatch matches; + std::regex_search(fw_rev, matches, rgx); + + if (matches.size() < 4) return invalid_version; + + try { + return version{static_cast(stoul(matches[1])), + static_cast(stoul(matches[2])), + static_cast(stoul(matches[3]))}; + } catch (const std::exception&) { + return invalid_version; + } +} + + } // namespace impl geometry_msgs::TransformStamped transform_to_tf_msg( @@ -166,13 +186,11 @@ sensor_msgs::LaserScan lidar_scan_to_laser_scan_msg( msg.time_increment = 1.0f / (scan_width * scan_frequency); msg.angle_increment = 2 * M_PI / scan_width; - auto which_range = return_index == 0 ? sensor::ChanField::RANGE - : sensor::ChanField::RANGE2; - ouster::img_t range = ls.field(which_range); - auto which_signal = return_index == 0 ? sensor::ChanField::SIGNAL - : sensor::ChanField::SIGNAL2; - ouster::img_t signal = - impl::get_or_fill_zero(which_signal, ls); + ouster::img_t range = ls.field( + static_cast(sensor::ChanField::RANGE + return_index)); + ouster::img_t signal = impl::get_or_fill_zero( + static_cast(sensor::ChanField::SIGNAL + return_index), + ls); const auto rg = range.data(); const auto sg = signal.data(); msg.ranges.resize(ls.w); From 1939ffe2c2e89e17f745aaeb89964583c58fbc20 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 23 Nov 2023 11:52:22 -0800 Subject: [PATCH 07/10] minor changes --- CHANGELOG.rst | 1 + src/laser_scan_processor.h | 2 +- src/os_ros.cpp | 1 - 3 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 89cd6177..238038de 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -10,6 +10,7 @@ Changelog * ouster_image_nodelet can operate independently from ouster_cloud_nodelet. * install ouster-ros and ouster_client include directories in separate folders. * [BUGFIX]: LaserScan is not properly aligned with generated point cloud + * address an issue where LaserScan appeared different on FW prior to 2.4 * [BUGFIX]: LaserScan does not work when using dual mode diff --git a/src/laser_scan_processor.h b/src/laser_scan_processor.h index 2c2a7c82..e9222ae8 100644 --- a/src/laser_scan_processor.h +++ b/src/laser_scan_processor.h @@ -46,7 +46,7 @@ class LaserScanProcessor { private: void process(const ouster::LidarScan& lidar_scan, uint64_t, const ros::Time& msg_ts) { - for (int i = 0; i < static_cast(scan_msgs.size()); ++i) { + for (size_t i = 0; i < scan_msgs.size(); ++i) { *scan_msgs[i] = lidar_scan_to_laser_scan_msg(lidar_scan, msg_ts, frame, ld_mode, ring_, pixel_shift_by_row, i); diff --git a/src/os_ros.cpp b/src/os_ros.cpp index d18a2fb5..c90d9f3c 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -148,7 +148,6 @@ version parse_version(const std::string& fw_rev) { } } - } // namespace impl geometry_msgs::TransformStamped transform_to_tf_msg( From 7f96b75647fa9859e48ded7efcd049f4baf5314a Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Tue, 28 Nov 2023 12:56:05 -0800 Subject: [PATCH 08/10] Fix for odd numbers --- src/os_ros.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/os_ros.cpp b/src/os_ros.cpp index c90d9f3c..171171b9 100644 --- a/src/os_ros.cpp +++ b/src/os_ros.cpp @@ -195,12 +195,13 @@ sensor_msgs::LaserScan lidar_scan_to_laser_scan_msg( msg.ranges.resize(ls.w); msg.intensities.resize(ls.w); - for (auto v = 0; v < ls.w; ++v) { - auto sign = v < ls.w / 2 ? -1 : +1; - auto v_shift = (ls.w / 2 - 1 - v + 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]); + uint16_t u = ring; + for (auto v = 0; v < ls.w; ++v) { + auto v_shift = (v + ls.w - pixel_shift_by_row[u] + ls.w / 2) % ls.w; + auto src_idx = u * ls.w + v_shift; + auto tgt_idx = ls.w - 1 - v; + msg.ranges[tgt_idx] = rg[src_idx] * ouster::sensor::range_unit; + msg.intensities[tgt_idx] = static_cast(sg[src_idx]); } return msg; From f49cd5a1b5882750a948892243d25e04495ee67a Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 29 Nov 2023 09:17:35 -0800 Subject: [PATCH 09/10] Add a section that explicitly list supported devices with links to support material --- README.md | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/README.md b/README.md index a14f1e0d..3a4de566 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,7 @@ - [Official ROS1/ROS2 drivers for Ouster sensors](#official-ros1ros2-drivers-for-ouster-sensors) - [Overview](#overview) + - [Supported Devices](#supported-devices) - [Requirements](#requirements) - [Getting Started](#getting-started) - [Usage](#usage) @@ -39,6 +40,16 @@ on the topics of `/ouster/imu` and `/ouster/points`. In the case the used sensor return and it was configured to use this capability, then another topic will published named `/ouster/points2` which corresponds to the second point cloud. +## Supported Devices +The driver supports the following list of Ouster sensors: +- [OS0](https://ouster.com/products/hardware/os0-lidar-sensor) +- [OS1](https://ouster.com/products/hardware/os1-lidar-sensor) +- [OS2](https://ouster.com/products/hardware/os2-lidar-sensor) +- [OSDome](https://ouster.com/products/hardware/osdome-lidar-sensor) + +You can obtain detailed specs sheet about the sensors and obtain updated FW through the website +[downloads](https://ouster.com/downloads) section. + ## Requirements This package only supports **Melodic** and **Noetic** ROS distros. Please refer to ROS online documentation on how to setup ros on your machine before proceeding with the remainder of this guide. From a82430a8c8eaaa7ee4b9bc4091b05a3ef9dee419 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 29 Nov 2023 10:07:41 -0800 Subject: [PATCH 10/10] Update RVIZ to highlight the LaserScan --- config/viz.rviz | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/config/viz.rviz b/config/viz.rviz index 26eee9b4..521506dc 100644 --- a/config/viz.rviz +++ b/config/viz.rviz @@ -40,7 +40,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: false Line Style: - Line Width: 0.029999999329447746 + Line Width: 0.01 Value: Lines Name: Grid Normal Cell Count: 0 @@ -75,7 +75,7 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 2 - Size (m): 0.029999999329447746 + Size (m): 0.01 Style: Flat Squares Topic: /ouster/points Unreliable: false @@ -105,7 +105,7 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.029999999329447746 + Size (m): 0.01 Style: Flat Squares Topic: /ouster/points2 Unreliable: false