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 and other point types in ouster-ros driver #216

Merged
merged 19 commits into from
Nov 13, 2023
Merged
Show file tree
Hide file tree
Changes from 7 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
15 changes: 13 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,22 @@ 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
)
ament_target_dependencies(${PROJECT_NAME}_test
rclcpp
ouster_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
18 changes: 18 additions & 0 deletions ouster-ros/config/driver_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -67,3 +67,21 @@ 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: ouster_ros::Point
# - 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 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
84 changes: 8 additions & 76 deletions ouster-ros/include/ouster_ros/os_ros.h
Original file line number Diff line number Diff line change
Expand Up @@ -19,20 +19,16 @@
#include <pcl/point_types.h>

#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>

#include <chrono>
#include <string>

#include "ouster_msgs/msg/packet_msg.hpp"
#include "ouster_ros/os_point.h"

namespace ouster_ros {

namespace sensor = ouster::sensor;
using Cloud = pcl::PointCloud<Point>;
using ns = std::chrono::nanoseconds;

/**
* Checks sensor_info if it currently represents a legacy udp lidar profile
Expand Down Expand Up @@ -90,74 +86,6 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_msgs::msg::PacketMsg& pm,
const std::string& frame,
const sensor::packet_format& pf);

/**
* Populate a PCL point cloud from a LidarScan.
* @param[in, out] points The points parameters is used to store the results of
* the cartesian product before it gets packed into the cloud object.
* @param[in] lut_direction the direction of the xyz lut (with single precision)
* @param[in] lut_offset the offset of the xyz lut (with single precision)
* @param[in] scan_ts scan start used to caluclate relative timestamps for
* points.
* @param[in] lidar_scan input lidar data
* @param[out] cloud output pcl pointcloud to populate
* @param[in] return_index index of return desired starting at 0
*/
[[deprecated("use the 2nd version of scan_to_cloud_f")]] void scan_to_cloud_f(
ouster::PointsF& points, const ouster::PointsF& lut_direction,
const ouster::PointsF& lut_offset, std::chrono::nanoseconds scan_ts,
const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud,
int return_index);

/**
* Populate a PCL point cloud from a LidarScan.
* @param[in, out] points The points parameters is used to store the results of
* the cartesian product before it gets packed into the cloud object.
* @param[in] lut_direction the direction of the xyz lut (with single precision)
* @param[in] lut_offset the offset of the xyz lut (with single precision)
* @param[in] scan_ts scan start used to caluclate relative timestamps for
* points
* @param[in] lidar_scan input lidar data
* @param[out] cloud output pcl pointcloud to populate
* @param[in] return_index index of return desired starting at 0
*/
void scan_to_cloud_f(ouster::PointsF& points,
const ouster::PointsF& lut_direction,
const ouster::PointsF& lut_offset, uint64_t scan_ts,
const ouster::LidarScan& lidar_scan,
ouster_ros::Cloud& cloud, int return_index);

/**
* Populate a destaggered PCL point cloud from a LidarScan
* @param[out] cloud output pcl pointcloud to populate
* @param[in, out] points The points parameters is used to store the results of
* the cartesian product before it gets packed into the cloud object.
* @param[in] lut_direction the direction of the xyz lut (with single precision)
* @param[in] lut_offset the offset of the xyz lut (with single precision)
* @param[in] scan_ts scan start used to caluclate relative timestamps for
* points
* @param[in] lidar_scan input lidar data
* @param[in] pixel_shift_by_row pixel shifts by row
* @param[in] return_index index of return desired starting at 0
*/
void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud,
ouster::PointsF& points,
const ouster::PointsF& lut_direction,
const ouster::PointsF& lut_offset, uint64_t scan_ts,
const ouster::LidarScan& ls,
const std::vector<int>& pixel_shift_by_row,
int return_index);

/**
* Serialize a PCL point cloud to a ROS message
* @param[in] cloud the PCL point cloud to convert
* @param[in] timestamp the timestamp to apply to the resulting ROS message
* @param[in] frame the frame to set in the resulting ROS message
* @return a ROS message containing the point cloud
*/
sensor_msgs::msg::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud,
const rclcpp::Time& timestamp,
const std::string& frame);

/**
* Convert transformation matrix return by sensor to ROS transform
* @param[in] mat transformation matrix return by sensor
Expand Down Expand Up @@ -203,10 +131,7 @@ struct read_and_cast {
template <typename T>
inline ouster::img_t<T> get_or_fill_zero(sensor::ChanField field,
const ouster::LidarScan& ls) {
if (!ls.field_type(field)) {
return ouster::img_t<T>::Zero(ls.h, ls.w);
}

if (!ls.field_type(field)) return ouster::img_t<T>::Zero(ls.h, ls.w);
ouster::img_t<T> result{ls.h, ls.w};
ouster::impl::visit_field(ls, field, read_and_cast(), result);
return result;
Expand All @@ -221,6 +146,13 @@ inline uint64_t ts_safe_offset_add(uint64_t ts, int64_t offset) {
return offset < 0 && ts < static_cast<uint64_t>(std::abs(offset)) ? 0 : ts + offset;
}

std::set<std::string> parse_tokens(const std::string& input, char delim);

inline bool check_token(const std::set<std::string>& tokens,
const std::string& token) {
return tokens.find(token) != tokens.end();
}


} // namespace impl

Expand Down
Loading
Loading