Skip to content

Commit

Permalink
Quick protoype of Velodyne point type
Browse files Browse the repository at this point in the history
  • Loading branch information
@Samahu
Samahu committed Sep 13, 2023
1 parent 24d9c7e commit ac6e8e8
Show file tree
Hide file tree
Showing 12 changed files with 317 additions and 382 deletions.
4 changes: 4 additions & 0 deletions ouster-ros/config/driver_params.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,3 +67,7 @@ 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: {default, xyzir}
# for more details about the fields of each point type and their data refer
# to ouster_ros/os_point.h header.
point_type: default
1 change: 1 addition & 0 deletions ouster-ros/config/os_sensor_cloud_image_params.yaml
View file
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: default # choose from: {default, xyzir}
ouster/os_image:
use_system_default_qos: False # needs to match the value defined for os_sensor node
1 change: 1 addition & 0 deletions ouster-ros/include/ouster_ros/os_point.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ struct EIGEN_ALIGN16 PointXYZIR {
} // namespace ouster_ros

// clang-format off

POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
(float, x, x)
(float, y, y)
Expand Down
46 changes: 7 additions & 39 deletions ouster-ros/include/ouster_ros/os_ros.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,22 +19,17 @@
#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;

template <class T>
using Cloud = pcl::PointCloud<T>;

/**
* Checks sensor_info if it currently represents a legacy udp lidar profile
* @param[in] info sensor_info
Expand Down Expand Up @@ -91,40 +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 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
*/
template <typename T>
void scan_to_cloud_f_destaggered(ouster_ros::Cloud<T>& 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
*/
template <class T>
sensor_msgs::msg::PointCloud2 cloud_to_cloud_msg(const Cloud<T>& 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 @@ -188,6 +149,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
22 changes: 0 additions & 22 deletions ouster-ros/src/lidar_packet_handler.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,28 +51,6 @@ uint64_t ulround(T value) {
return static_cast<uint64_t>(rounded_value);
}

// TODO: move to a separate file
std::set<std::string> parse_tokens(const std::string& input, char delim) {
std::set<std::string> tokens;
std::stringstream ss(input);
std::string token;

while (getline(ss, token, delim)) {
// Remove leading and trailing whitespaces from the token
size_t start = token.find_first_not_of(" ");
size_t end = token.find_last_not_of(" ");
token = token.substr(start, end - start + 1);
if (!token.empty()) tokens.insert(token);
}

return tokens;
}

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

} // namespace

namespace ouster_ros {
Expand Down
63 changes: 38 additions & 25 deletions ouster-ros/src/os_cloud_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
* @brief A node to publish point clouds and imu topics
*/

// 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 @@ -15,11 +15,6 @@
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

#include <algorithm>
#include <chrono>
#include <memory>
#include <cassert>

#include "ouster_msgs/msg/packet_msg.hpp"
#include "ouster_ros/os_processing_node_base.h"
#include "ouster_ros/visibility_control.h"
Expand All @@ -30,11 +25,11 @@
#include "point_cloud_processor.h"
#include "laser_scan_processor.h"

namespace ouster_ros {

namespace sensor = ouster::sensor;
using ouster_msgs::msg::PacketMsg;

namespace ouster_ros {

class OusterCloud : public OusterProcessingNodeBase {
public:
OUSTER_ROS_PUBLIC
Expand All @@ -58,11 +53,12 @@ class OusterCloud : public OusterProcessingNodeBase {

void declare_parameters() {
tf_bcast.declare_parameters();
declare_parameter<std::string>("timestamp_mode", "");
declare_parameter<double>("ptp_utc_tai_offset", -37.0);
declare_parameter<std::string>("proc_mask", "IMU|PCL|SCAN");
declare_parameter<bool>("use_system_default_qos", false);
declare_parameter<int>("scan_ring", 0);
declare_parameter("timestamp_mode", "");
declare_parameter("ptp_utc_tai_offset", -37.0);
declare_parameter("proc_mask", "IMU|PCL|SCAN");
declare_parameter("use_system_default_qos", false);
declare_parameter("scan_ring", 0);
declare_parameter("point_type", "default");
}

void metadata_handler(
Expand All @@ -87,9 +83,9 @@ class OusterCloud : public OusterProcessingNodeBase {
use_system_default_qos ? system_default_qos : sensor_data_qos;

auto proc_mask = get_parameter("proc_mask").as_string();
auto tokens = parse_tokens(proc_mask, '|');
auto tokens = impl::parse_tokens(proc_mask, '|');

if (check_token(tokens, "IMU")) {
if (impl::check_token(tokens, "IMU")) {
imu_pub =
create_publisher<sensor_msgs::msg::Imu>("imu", selected_qos);
imu_packet_handler = ImuPacketHandler::create_handler(
Expand All @@ -106,22 +102,39 @@ class OusterCloud : public OusterProcessingNodeBase {
int num_returns = get_n_returns(info);

std::vector<LidarScanProcessor> processors;
if (check_token(tokens, "PCL")) {
if (impl::check_token(tokens, "PCL")) {
lidar_pubs.resize(num_returns);
for (int i = 0; i < num_returns; ++i) {
lidar_pubs[i] = create_publisher<sensor_msgs::msg::PointCloud2>(
topic_for_return("points", i), selected_qos);
}
processors.push_back(PointCloudProcessor::create(
info, tf_bcast.point_cloud_frame_id(),
tf_bcast.apply_lidar_to_sensor_transform(),
[this](PointCloudProcessor::OutputType msgs) {
for (size_t i = 0; i < msgs.size(); ++i)
lidar_pubs[i]->publish(*msgs[i]);
}));

auto point_type = get_parameter("point_type").as_string();
if (point_type == "default") {
processors.push_back(
PointCloudProcessor<ouster_ros::Point>::create(
info, tf_bcast.point_cloud_frame_id(),
tf_bcast.apply_lidar_to_sensor_transform(),
[this](PointCloudProcessor<ouster_ros::Point>::OutputType msgs) {
for (size_t i = 0; i < msgs.size(); ++i)
lidar_pubs[i]->publish(*msgs[i]);
}));
} else if (point_type == "xyzir") {
processors.push_back(
PointCloudProcessor<ouster_ros::PointXYZIR>::create(
info, tf_bcast.point_cloud_frame_id(),
tf_bcast.apply_lidar_to_sensor_transform(),
[this](PointCloudProcessor<ouster_ros::PointXYZIR>::OutputType msgs) {
for (size_t i = 0; i < msgs.size(); ++i)
lidar_pubs[i]->publish(*msgs[i]);
}));
} else {
RCLCPP_WARN_STREAM(get_logger(),
"Un-supported point type used: " << point_type);
}
}

if (check_token(tokens, "SCAN")) {
if (impl::check_token(tokens, "SCAN")) {
scan_pubs.resize(num_returns);
for (int i = 0; i < num_returns; ++i) {
scan_pubs[i] = create_publisher<sensor_msgs::msg::LaserScan>(
Expand All @@ -146,7 +159,7 @@ class OusterCloud : public OusterProcessingNodeBase {
}));
}

if (check_token(tokens, "PCL") || check_token(tokens, "SCAN")) {
if (impl::check_token(tokens, "PCL") || impl::check_token(tokens, "SCAN")) {
lidar_packet_handler = LidarPacketHandler::create_handler(
info, processors, timestamp_mode,
static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
Expand Down
51 changes: 34 additions & 17 deletions ouster-ros/src/os_driver_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,9 +33,10 @@ class OusterDriver : public OusterSensor {
: OusterSensor("os_driver", options), tf_bcast(this) {
tf_bcast.declare_parameters();
tf_bcast.parse_parameters();
declare_parameter<std::string>("proc_mask", "IMU|IMG|PCL|SCAN");
declare_parameter<int>("scan_ring", 0);
declare_parameter<double>("ptp_utc_tai_offset", -37.0);
declare_parameter("proc_mask", "IMU|IMG|PCL|SCAN");
declare_parameter("scan_ring", 0);
declare_parameter("ptp_utc_tai_offset", -37.0);
declare_parameter("point_type", "default");
}

virtual void on_metadata_updated(const sensor::sensor_info& info) override {
Expand All @@ -45,7 +46,7 @@ class OusterDriver : public OusterSensor {

virtual void create_publishers() override {
auto proc_mask = get_parameter("proc_mask").as_string();
auto tokens = parse_tokens(proc_mask, '|');
auto tokens = impl::parse_tokens(proc_mask, '|');

bool use_system_default_qos =
get_parameter("use_system_default_qos").as_bool();
Expand All @@ -58,7 +59,7 @@ class OusterDriver : public OusterSensor {
auto ptp_utc_tai_offset =
get_parameter("ptp_utc_tai_offset").as_double();

if (check_token(tokens, "IMU")) {
if (impl::check_token(tokens, "IMU")) {
imu_pub =
create_publisher<sensor_msgs::msg::Imu>("imu", selected_qos);
imu_packet_handler = ImuPacketHandler::create_handler(
Expand All @@ -69,23 +70,39 @@ class OusterDriver : public OusterSensor {
int num_returns = get_n_returns(info);

std::vector<LidarScanProcessor> processors;
if (check_token(tokens, "PCL")) {
if (impl::check_token(tokens, "PCL")) {
lidar_pubs.resize(num_returns);
for (int i = 0; i < num_returns; ++i) {
lidar_pubs[i] = create_publisher<sensor_msgs::msg::PointCloud2>(
topic_for_return("points", i), selected_qos);
}

processors.push_back(PointCloudProcessor::create(
info, tf_bcast.point_cloud_frame_id(),
tf_bcast.apply_lidar_to_sensor_transform(),
[this](PointCloudProcessor::OutputType msgs) {
for (size_t i = 0; i < msgs.size(); ++i)
lidar_pubs[i]->publish(*msgs[i]);
}));
auto point_type = get_parameter("point_type").as_string();
if (point_type == "default") {
processors.push_back(
PointCloudProcessor<ouster_ros::Point>::create(
info, tf_bcast.point_cloud_frame_id(),
tf_bcast.apply_lidar_to_sensor_transform(),
[this](PointCloudProcessor<ouster_ros::Point>::OutputType msgs) {
for (size_t i = 0; i < msgs.size(); ++i)
lidar_pubs[i]->publish(*msgs[i]);
}));
} else if (point_type == "xyzir") {
processors.push_back(
PointCloudProcessor<ouster_ros::PointXYZIR>::create(
info, tf_bcast.point_cloud_frame_id(),
tf_bcast.apply_lidar_to_sensor_transform(),
[this](PointCloudProcessor<ouster_ros::PointXYZIR>::OutputType msgs) {
for (size_t i = 0; i < msgs.size(); ++i)
lidar_pubs[i]->publish(*msgs[i]);
}));
} else {
RCLCPP_WARN_STREAM(get_logger(),
"Un-supported point type used: " << point_type);
}
}

if (check_token(tokens, "SCAN")) {
if (impl::check_token(tokens, "SCAN")) {
scan_pubs.resize(num_returns);
for (int i = 0; i < num_returns; ++i) {
scan_pubs[i] = create_publisher<sensor_msgs::msg::LaserScan>(
Expand Down Expand Up @@ -115,7 +132,7 @@ class OusterDriver : public OusterSensor {
}));
}

if (check_token(tokens, "IMG")) {
if (impl::check_token(tokens, "IMG")) {
const std::map<sensor::ChanField, std::string>
channel_field_topic_map_1{
{sensor::ChanField::RANGE, "range_image"},
Expand Down Expand Up @@ -150,8 +167,8 @@ class OusterDriver : public OusterSensor {
}));
}

if (check_token(tokens, "PCL") || check_token(tokens, "SCAN") ||
check_token(tokens, "IMG"))
if (impl::check_token(tokens, "PCL") || impl::check_token(tokens, "SCAN") ||
impl::check_token(tokens, "IMG"))
lidar_packet_handler = LidarPacketHandler::create_handler(
info, processors, timestamp_mode,
static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
Expand Down
Loading

0 comments on commit ac6e8e8

Please sign in to comment.