From 874c6bf167ee5c30237247f5c3c943a232ef1c14 Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Wed, 29 Nov 2023 12:20:54 -0800 Subject: [PATCH] ROS-196: laser scan from ros driver is not properly aligned with point cloud [foxy] (#204) * Apply destagger to laser scan + Add laser to rviz * Align LaserScan with the PointCloud * Apply proper pixel shift * Resolve the issue of zeroed laserscan on dual mode * Address an issue where LaserScan appeared different on FW prior to 2.4 * Fix the issue for odd numbers * List selected sensors on the main page + Update RVIZ config to highlight the 2D LaserScan. --- CHANGELOG.rst | 6 +++ README.md | 12 ++++++ ouster-ros/config/viz.rviz | 54 +++++++++++++++++++++----- ouster-ros/include/ouster_ros/os_ros.h | 5 ++- ouster-ros/package.xml | 2 +- ouster-ros/src/laser_scan_processor.h | 27 +++++++++---- ouster-ros/src/os_ros.cpp | 37 +++++++++++++++--- 7 files changed, 119 insertions(+), 24 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index cbb6c97c..0bae453c 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog ========= +[unreleased] +============ +* [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 + ouster_ros v0.12.0 ================== diff --git a/README.md b/README.md index 9b3b58a2..74230a77 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,7 @@ - [Official ROS driver for Ouster sensors](#official-ros-driver-for-ouster-sensors) - [Overview](#overview) + - [Supported Devices](#supported-devices) - [Requirements](#requirements) - [Linux](#linux) - [Windows](#windows) @@ -43,6 +44,17 @@ messages on the topics of `/ouster/imu` and `/ouster/points`. In the case the us dual return and it was configured to use this capability, then another topic will published under the name `/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 branch is only intended for use with the **Foxy** ROS 2 distro. Please refer to ROS 2 online documentation on how to setup ROS on your machine before proceeding with the remainder of this guide. diff --git a/ouster-ros/config/viz.rviz b/ouster-ros/config/viz.rviz index cbff6cf5..e020af30 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.02 + 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 c2589104..10fbf66b 100644 --- a/ouster-ros/include/ouster_ros/os_ros.h +++ b/ouster-ros/include/ouster_ros/os_ros.h @@ -108,6 +108,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( @@ -115,7 +117,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 { @@ -154,6 +156,7 @@ 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 diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index 6609bf52..9b4f760b 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.12.0 + 0.12.1 Ouster ROS2 driver ouster developers BSD diff --git a/ouster-ros/src/laser_scan_processor.h b/ouster-ros/src/laser_scan_processor.h index 63dae838..72096517 100644 --- a/ouster-ros/src/laser_scan_processor.h +++ b/ouster-ros/src/laser_scan_processor.h @@ -8,7 +8,7 @@ #pragma once -// prevent clang-format from altering the location of "ouster_ros/ros.h", the +// prevent clang-format from altering the location of "ouster_ros/os_ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off #include "ouster_ros/os_ros.h" @@ -29,16 +29,28 @@ class LaserScanProcessor { : frame(frame_id), ld_mode(info.mode), ring_(ring), - scan_msgs(get_n_returns(info), - std::make_shared()), - post_processing_fn(func) {} + 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(); + + 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 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); + 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); } if (post_processing_fn) post_processing_fn(scan_msgs); @@ -61,6 +73,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 a571ae80..df1d332f 100644 --- a/ouster-ros/src/os_ros.cpp +++ b/ouster-ros/src/os_ros.cpp @@ -19,11 +19,15 @@ #include #include #include +#include + + +namespace ouster_ros { namespace sensor = ouster::sensor; +using namespace ouster::util; using ouster_sensor_msgs::msg::PacketMsg; -namespace ouster_ros { bool is_legacy_lidar_profile(const sensor::sensor_info& info) { using sensor::UDPProfileLidar; @@ -130,6 +134,22 @@ 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::msg::TransformStamped transform_to_tf_msg( @@ -151,7 +171,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; @@ -177,10 +198,14 @@ 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]); + + 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;