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

SW-5466: Support velodyne point type in the ros driver (foxy) #255

Merged
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
7 changes: 6 additions & 1 deletion CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,14 @@ Changelog
* introduced a new launch file parameter ``ptp_utc_tai_offset`` which represent offset in seconds
to be applied to all ROS messages the driver generates when ``TIME_FROM_PTP_1588`` timestamp mode
is used.
* [BREAKING]: the default value of ``ptp_utc_tai_offset`` is set to ``-37.0``. To retain the same
time offset for an existing system, users need to set ``ptp_utc_tai_offset`` to ``0.0``.
* 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
72 changes: 49 additions & 23 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 @@ -65,7 +67,7 @@ sudo apt install -y \
ros-$ROS_DISTRO-tf2-eigen \
ros-$ROS_DISTRO-rviz2
```
where `$ROS_DISTRO` is ``foxy``.
where `$ROS_DISTRO` is ``foxy`` or ``galactic``.

> **Note**
> Installing `ros-$ROS_DISTRO-rviz` package is optional in case you didn't need to visualize the
Expand Down Expand Up @@ -107,7 +109,7 @@ git clone -b ros2-foxy --recurse-submodules https://github.com/ouster-lidar/oust

Next to compile the driver you need to source the ROS environemt into the active termainl:
```bash
source /opt/ros/<ros-distro>/setup.bash # replace ros-distro with 'foxy'
source /opt/ros/<ros-distro>/setup.bash # replace ros-distro with 'foxy' or 'galactic'
```

Finally, invoke `colcon build` command from within the catkin workspace as shown below:
Expand All @@ -130,7 +132,7 @@ source ros2_ws/install/setup.bash
The package supports three modes of interaction, you can connect to a _live sensor_, _replay_ a recorded
bag or _record_ a new bag file using the corresponding launch files. Recently, we have added a new mode
that supports multicast. The commands are listed below, for convenience we do provide both launch file
formats (xml and python) but the python format is the preferred method.
formats (xml and python) but the python format is the preferred method:

#### Sensor Mode
To connect to a live sensor you use the following launch file
Expand Down Expand Up @@ -241,8 +243,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 @@ -67,3 +67,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