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-5657: Back port SW-5466 to ROS-1 #253

Merged
merged 5 commits into from
Nov 16, 2023
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
13 changes: 11 additions & 2 deletions CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,14 @@
Changelog
=========

[unreleased]
============
* [BREAKING] ROS1 driver code now requires C++17 (required for point cloud customization feature).
* added the ability to customize the published point clouds(s) to velodyne point cloud format and
other common pcl point types.
* ouster_image_nodelet can operate independently from ouster_cloud_nodelet.


ouster_ros v0.10.0
==================

Expand All @@ -12,7 +20,9 @@ ouster_ros(1)
* [BREAKING]: publish PCL point clouds destaggered.
* 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.
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``.
* [BUGFIX]: destagger columns timestamp when generating destaggered point clouds.
* [BUGFIX]: gracefully stop the driver when shutdown is requested.

Expand Down Expand Up @@ -49,7 +59,6 @@ ouster_client
* [bugfix] Fixed dropped columns issue with 4096x5 and 2048x10



ouster_ros v0.9.0
==================

Expand Down
12 changes: 10 additions & 2 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@ find_package(
nodelet)

# ==== Options ====
add_compile_options(-std=c++17)
add_compile_options(-Wall -Wextra)
option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON)

Expand Down Expand Up @@ -89,8 +90,15 @@ add_dependencies(${PROJECT_NAME}_nodelets ${PROJECT_NAME}_gencpp)

# ==== Test ====
if(CATKIN_ENABLE_TESTING)
catkin_add_gtest(${PROJECT_NAME}_test tests/ring_buffer_test.cpp)
target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES})
catkin_add_gtest(${PROJECT_NAME}_test
tests/ring_buffer_test.cpp
src/os_ros.cpp
tests/point_accessor_test.cpp
tests/point_transform_test.cpp
tests/point_cloud_compose_test.cpp
)
target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES}
ouster_build pcl_common -Wl,--whole-archive ouster_client -Wl,--no-whole-archive)
endif()

# ==== Install ====
Expand Down
47 changes: 32 additions & 15 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -12,21 +12,22 @@
| 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)
- [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)
- [Launch Files Arguments](#launch-files-arguments)
- [Invoking Services](#invoking-services)
- [GetMetadata](#getmetadata)
- [GetConfig](#getconfig)
- [SetConfig (experimental)](#setconfig-experimental)
- [License](#license)
- [Official ROS1/ROS2 drivers for Ouster sensors](#official-ros1ros2-drivers-for-ouster-sensors)
- [Overview](#overview)
- [Requirements](#requirements)
- [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)
- [Launch Files Arguments](#launch-files-arguments)
- [Invoking Services](#invoking-services)
- [GetMetadata](#getmetadata)
- [GetConfig](#getconfig)
- [SetConfig (experimental)](#setconfig-experimental)
- [License](#license)


## Overview
Expand Down Expand Up @@ -175,6 +176,22 @@ roslaunch ouster_ros driver.launch --ros-args
The command should list all available arguments, whether they are optional or required and the
description and posible values of each argument.

New launch file parameter:
**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.

### Invoking Services
To execute any of the following service, first you need to open a new terminal
and source the castkin workspace again by running the command:
Expand Down
72 changes: 72 additions & 0 deletions 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 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