Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ROS-196: laser scan from ros driver is not properly aligned with point cloud [foxy] #204

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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
==================

Expand Down
12 changes: 12 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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.
Expand Down
54 changes: 45 additions & 9 deletions ouster-ros/config/viz.rviz
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand Down
5 changes: 4 additions & 1 deletion ouster-ros/include/ouster_ros/os_ros.h
Original file line number Diff line number Diff line change
Expand Up @@ -108,14 +108,16 @@ 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(
const ouster::LidarScan& ls,
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<int>& pixel_shift_by_row,
const int return_index);

namespace impl {
Expand Down Expand Up @@ -154,6 +156,7 @@ inline bool check_token(const std::set<std::string>& tokens,
return tokens.find(token) != tokens.end();
}

ouster::util::version parse_version(const std::string& fw_rev);

} // namespace impl

Expand Down
2 changes: 1 addition & 1 deletion ouster-ros/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>ouster_ros</name>
<version>0.12.0</version>
<version>0.12.1</version>
<description>Ouster ROS2 driver</description>
<maintainer email="[email protected]">ouster developers</maintainer>
<license file="LICENSE">BSD</license>
Expand Down
27 changes: 20 additions & 7 deletions ouster-ros/src/laser_scan_processor.h
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -29,16 +29,28 @@ class LaserScanProcessor {
: frame(frame_id),
ld_mode(info.mode),
ring_(ring),
scan_msgs(get_n_returns(info),
std::make_shared<sensor_msgs::msg::LaserScan>()),
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<sensor_msgs::msg::LaserScan>();

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<int>(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);
Expand All @@ -61,6 +73,7 @@ class LaserScanProcessor {
std::string frame;
sensor::lidar_mode ld_mode;
uint16_t ring_;
std::vector<int> pixel_shift_by_row;
OutputType scan_msgs;
PostProcessingFn post_processing_fn;
};
Expand Down
37 changes: 31 additions & 6 deletions ouster-ros/src/os_ros.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,11 +19,15 @@
#include <chrono>
#include <string>
#include <vector>
#include <regex>


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;
Expand Down Expand Up @@ -130,6 +134,22 @@ std::set<std::string> 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<uint16_t>(stoul(matches[1])),
static_cast<uint16_t>(stoul(matches[2])),
static_cast<uint16_t>(stoul(matches[3]))};
} catch (const std::exception&) {
return invalid_version;
}
}

} // namespace impl

geometry_msgs::msg::TransformStamped transform_to_tf_msg(
Expand All @@ -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<int>& pixel_shift_by_row,
const int return_index) {
sensor_msgs::msg::LaserScan msg;
msg.header.stamp = timestamp;
msg.header.frame_id = frame;
Expand All @@ -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<float>(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<float>(sg[src_idx]);
}

return msg;
Expand Down
Loading