Skip to content

Commit

Permalink
SW-5466: Support Velodyne and other point types in ouster-ros driver (o…
Browse files Browse the repository at this point in the history
…uster-lidar#216)

* Quick protoype of Velodyne point type
* Add PointXYZIR point type + other major pcl point types
* Include point meta functions and point transform by the ouster_ros namesapce
* Wrap point meta functions with a namespace and use shorter names for the functions +
* Add a seed test module for the point_cloud_compose methods +
 Add description for the point_cloud_compose methods + refactor code and add compile time checks.
* Propagate error state, warn about potential incompatible profile, propagate error state
* Add minimal documentation about the new `point_type` parameter.
  • Loading branch information
Samahu authored and AchmadFathoni committed Feb 16, 2024
1 parent 6d0561f commit 39996fd
Show file tree
Hide file tree
Showing 25 changed files with 1,912 additions and 574 deletions.
4 changes: 3 additions & 1 deletion CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,9 @@ Changelog
* fix: destagger columns timestamp when generating destaggered point clouds.
* shutdown the driver when unable to connect to the sensor on startup
* breaking: rename ouster_msgs to ouster_sensor_msgs

* added the ability to customize the published point clouds(s) to velodyne point cloud format and
other common pcl point types.
* ouster_image_compoenent can operate separately from ouster_cloud_component.

ouster_ros v0.10.0
==================
Expand Down
66 changes: 46 additions & 20 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -12,24 +12,26 @@
| ROS2 (rolling/humble/iron) | [![rolling/humble/iron](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
| ROS2 (galactic/foxy) | [![galactic/foxy](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml/badge.svg?branch=ros2-foxy)](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)

- [Overview](#overview)
- [Requirements](#requirements)
- [Linux](#linux)
- [Windows](#windows)
- [Mac](#mac)
- [Getting Started](#getting-started)
- [Usage](#usage)
- [Launching Nodes](#launching-nodes)
- [Sensor Mode](#sensor-mode)
- [Recording Mode](#recording-mode)
- [Replay Mode](#replay-mode)
- [Multicast Mode (experimental)](#multicast-mode-experimental)
- [Invoking Services](#invoking-services)
- [GetMetadata](#getmetadata)
- [GetConfig](#getconfig)
- [SetConfig](#setconfig)
- [Reset](#reset)
- [License](#license)
- [Official ROS driver for Ouster sensors](#official-ros-driver-for-ouster-sensors)
- [Overview](#overview)
- [Requirements](#requirements)
- [Linux](#linux)
- [Windows](#windows)
- [Mac](#mac)
- [Getting Started](#getting-started)
- [Usage](#usage)
- [Launching Nodes](#launching-nodes)
- [Sensor Mode](#sensor-mode)
- [Recording Mode](#recording-mode)
- [Replay Mode](#replay-mode)
- [Multicast Mode (experimental)](#multicast-mode-experimental)
- [Invoking Services](#invoking-services)
- [GetMetadata](#getmetadata)
- [GetConfig](#getconfig)
- [SetConfig](#setconfig)
- [Reset](#reset)
- [Driver Parameters](#driver-parameters)
- [License](#license)


## Overview
Expand Down Expand Up @@ -238,8 +240,32 @@ connection, reset the sensor and reconnect again.
> **Note**
> Changing settings is not yet fully support during a reset operation (more on this)
TBD: For further detailed instructions refer to the [main guide](./docs/index.rst)

### Driver Parameters
The driver has several parameters that allow you to customize its behavior, all of
these parameters are defined with the `driver_params.yaml` file found under `config`
folder. The only required parameter is `sensor_hostname` which sets the sensor
hostname or ip that you want to connect to through ouster-ros driver.

Other notable parameters include:
* **point_type**: This parameter allows to customize the point cloud that the
driver produces through its `/ouster/points` topics. Choose one of the following
values:
- `original`: This uses the original point representation `ouster_ros::Point`
of the ouster-ros driver.
- `native`: directly maps all fields as published by the sensor to an
equivalent point cloud representation with the additon of ring
and timestamp fields.
- `xyz`: the simplest point type, only has {x, y, z}
- `xyzi`: same as xyz point type but adds intensity (signal) field. this
type is not compatible with the low data profile.
- `xyzir`: same as xyzi type but adds ring (channel) field.
this type is same as Velodyne point cloud type
this type is not compatible with the low data profile.

This is not a comprehenisve list of all the parameters that the driver supports
for more detailed list please refer to the `config/driver_params.yaml` file.

For further detailed instructions about the driver refer to the [main guide](./docs/index.rst)

## License
[License File](./LICENSE)
16 changes: 14 additions & 2 deletions ouster-ros/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -154,7 +154,7 @@ rclcpp_components_register_node(os_image_component
EXECUTABLE os_image
)

# ==== os_sensor_component ====
# ==== os_driver_component ====
create_ros2_component(os_driver_component
"src/os_sensor_node_base.cpp;src/os_sensor_node.cpp;src/os_driver_node.cpp"
"std_srvs"
Expand All @@ -168,11 +168,23 @@ rclcpp_components_register_node(os_driver_component
# ==== Test ====
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(${PROJECT_NAME}_test test/ring_buffer_test.cpp)
ament_add_gtest(${PROJECT_NAME}_test
test/ring_buffer_test.cpp
src/os_ros.cpp
test/point_accessor_test.cpp
test/point_transform_test.cpp
test/point_cloud_compose_test.cpp
)
ament_target_dependencies(${PROJECT_NAME}_test
rclcpp
ouster_sensor_msgs
)
target_include_directories(${PROJECT_NAME}_test PUBLIC
${_ouster_ros_INCLUDE_DIRS}
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_link_libraries(${PROJECT_NAME}_test ouster_ros_library)
endif()


Expand Down
19 changes: 19 additions & 0 deletions ouster-ros/config/driver_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -64,3 +64,22 @@ ouster/os_driver:
# data QoS. This is preferrable for production but default QoS is needed for
# rosbag. See: https://github.com/ros2/rosbag2/issues/125
use_system_default_qos: false
# point_type[optional]: choose from: {original, native, xyz, xyzi, xyzir}
# Here is a breif description of each option:
# - original: This uses the original point representation ouster_ros::Point
# of the ouster-ros driver.
# - native: directly maps all fields as published by the sensor to an
# equivalent point cloud representation with the additon of ring
# and timestamp fields.
# - xyz: the simplest point type, only has {x, y, z}
# - xyzi: same as xyz point type but adds intensity (signal) field. this
# type is not compatible with the low data profile.
# - xyzir: same as xyzi type but adds ring (channel) field.
# this type is same as Velodyne point cloud type
# this type is not compatible with the low data profile.
# for more details about the fields of each point type and their data refer
# to the following header files:
# - ouster_ros/os_point.h
# - ouster_ros/sensor_point_types.h
# - ouster_ros/common_point_types.h.
point_type: original
1 change: 1 addition & 0 deletions ouster-ros/config/os_sensor_cloud_image_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -27,5 +27,6 @@ ouster/os_cloud:
use_system_default_qos: False # needs to match the value defined for os_sensor node
scan_ring: 0 # Use this parameter in conjunction with the SCAN flag and choose a
# value the range [0, sensor_beams_count)
point_type: original # choose from: {original, native, xyz, xyzi, xyzir}
ouster/os_image:
use_system_default_qos: False # needs to match the value defined for os_sensor node
72 changes: 72 additions & 0 deletions ouster-ros/include/ouster_ros/common_point_types.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
/**
* Copyright (c) 2018-2023, Ouster, Inc.
* All rights reserved.
*
* @file common_point_types.h
* @brief common PCL point datatype for use with ouster sensors
*/

#pragma once

#include <pcl/point_types.h>

namespace ouster_ros {

/* The following are pcl point representations that are common/standard point
representation that we readily support.
*/

/*
* Same as Velodyne point cloud type
* @remark XYZIR point type is not compatible with RNG15_RFL8_NIR8/LOW_DATA
* udp lidar profile.
*/
struct EIGEN_ALIGN16 _PointXYZIR {
PCL_ADD_POINT4D;
float intensity;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct PointXYZIR : public _PointXYZIR {

inline PointXYZIR(const _PointXYZIR& pt)
{
x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f;
intensity = pt.intensity; ring = pt.ring;
}

inline PointXYZIR()
{
x = y = z = 0.0f; data[3] = 1.0f;
intensity = 0.0f; ring = 0;
}

inline const auto as_tuple() const {
return std::tie(x, y, z, intensity, ring);
}

inline auto as_tuple() {
return std::tie(x, y, z, intensity, ring);
}

template<size_t I>
inline auto& get() {
return std::get<I>(as_tuple());
}
};

} // namespace ouster_ros

// clang-format off

/* common point types */
POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::PointXYZIR,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(std::uint16_t, ring, ring)
)

// clang-format on
65 changes: 54 additions & 11 deletions ouster-ros/include/ouster_ros/os_point.h
Original file line number Diff line number Diff line change
Expand Up @@ -10,36 +10,79 @@

#include <pcl/point_types.h>

#include <Eigen/Core>
#include <chrono>

#include <ouster/lidar_scan.h>

namespace ouster_ros {

struct EIGEN_ALIGN16 Point {
// The default/original represntation of the point cloud since the driver
// inception. This shouldn't be confused with Point_LEGACY which provides exact
// mapping of the fields of Ouster LidarScan of the Legacy Profile, copying the
// the same order and using the same bit representation. For example, this Point
// struct uses float data type to represent intensity (aka signal); however, the
// sensor sends the signal channel either as UINT16 or UINT32 depending on the
// active udp lidar profile.
struct EIGEN_ALIGN16 _Point {
PCL_ADD_POINT4D;
float intensity;
float intensity; // equivalent to signal
uint32_t t;
uint16_t reflectivity;
uint16_t ring;
uint16_t ambient;
uint16_t ring; // equivalent to channel
uint16_t ambient; // equivalent to near_ir
uint32_t range;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace ouster_ros

struct Point : public _Point {

inline Point(const _Point& pt)
{
x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f;
intensity = pt.intensity;
t = pt.t;
reflectivity = pt.reflectivity;
ring = pt.ring;
ambient = pt.ambient;
range = pt.range;
}

inline Point()
{
x = y = z = 0.0f; data[3] = 1.0f;
intensity = 0.0f;
t = 0;
reflectivity = 0;
ring = 0;
ambient = 0;
range = 0;
}

inline const auto as_tuple() const {
return std::tie(x, y, z, intensity, t, reflectivity, ring, ambient, range);
}

inline auto as_tuple() {
return std::tie(x, y, z, intensity, t, reflectivity, ring, ambient, range);
}

template<size_t I>
inline auto& get() {
return std::get<I>(as_tuple());
}
};

} // namespace ouster_ros

// clang-format off

// DEFAULT/ORIGINAL
POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
// use std::uint32_t to avoid conflicting with pcl::uint32_t
(std::uint32_t, t, t)
(std::uint16_t, reflectivity, reflectivity)
(std::uint16_t, ring, ring)
(std::uint16_t, ambient, ambient)
(std::uint32_t, range, range)
)

// clang-format on
Loading

0 comments on commit 39996fd

Please sign in to comment.