From 5a08f89e21dabfb78ae359f9bfc4ba869a275104 Mon Sep 17 00:00:00 2001 From: Ussama Naal <606033+Samahu@users.noreply.github.com> Date: Wed, 29 Nov 2023 11:17:43 -0800 Subject: [PATCH] ROS-196: laser scan from ros driver is not properly aligned with point cloud [ROS1] (#224) * Port the alignment solution to ROS1 * Add LaserScan to RVIZ * Use correct shift when assembling LaserScan msg towards on firmwares prior to 2.4 * Add a section that explicitly list supported devices with links to support material * Update RVIZ to highlight the LaserScan --- CHANGELOG.rst | 4 +++ README.md | 11 +++++++ config/viz.rviz | 58 ++++++++++++++++++++++++++++--------- include/ouster_ros/os_ros.h | 12 ++++---- package.xml | 2 +- src/laser_scan_processor.h | 25 ++++++++++++---- src/os_ros.cpp | 48 +++++++++++++++++++++--------- 7 files changed, 121 insertions(+), 39 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index b22bb927..238038de 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -9,6 +9,10 @@ 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 + * 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.10.0 ================== 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. diff --git a/config/viz.rviz b/config/viz.rviz index 570ec59a..521506dc 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 @@ -23,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: @@ -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 @@ -198,6 +198,8 @@ Visualization Manager: Value: false - Class: rviz/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: false @@ -207,6 +209,7 @@ Visualization Manager: Value: false os_sensor: Value: true + Marker Alpha: 1 Marker Scale: 1 Name: TF Show Arrows: true @@ -220,6 +223,34 @@ Visualization Manager: {} Update Interval: 0 Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + 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: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /ouster/scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -248,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 @@ -264,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: @@ -284,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/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h index d053732b..631aa6ad 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 { @@ -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/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 diff --git a/src/laser_scan_processor.h b/src/laser_scan_processor.h index ccd184e7..e9222ae8 100644 --- a/src/laser_scan_processor.h +++ b/src/laser_scan_processor.h @@ -28,16 +28,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 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); + 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); @@ -60,6 +72,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..171171b9 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,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::TransformStamped transform_to_tf_msg( @@ -150,7 +169,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; @@ -165,21 +185,23 @@ 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); 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;