From 75f013774d5369f34dd0e0cbc7de967853cdfd53 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Fri, 26 May 2023 11:24:50 -0700 Subject: [PATCH 01/38] Factoring out Imu and Lidar packets handling --- ouster-ros/include/ouster_ros/os_ros.h | 13 ++ ouster-ros/src/imu_packet_handler.h | 39 ++++ ouster-ros/src/lidar_packet_handler.h | 290 +++++++++++++++++++++++++ ouster-ros/src/os_cloud_node.cpp | 274 +++-------------------- ouster-ros/src/os_ros.cpp | 23 +- ouster-ros/src/os_sensor_node.cpp | 196 ++++++++++++----- 6 files changed, 518 insertions(+), 317 deletions(-) create mode 100644 ouster-ros/src/imu_packet_handler.h create mode 100644 ouster-ros/src/lidar_packet_handler.h diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h index 7f17cd0d..4b4c8ff1 100644 --- a/ouster-ros/include/ouster_ros/os_ros.h +++ b/ouster-ros/include/ouster_ros/os_ros.h @@ -56,6 +56,19 @@ bool read_lidar_packet(const sensor::client& cli, ouster_msgs::msg::PacketMsg& pm, const sensor::packet_format& pf); +/** + * Parse an imu packet message into a ROS imu message + * @param[in] pf the packet format + * @param[in] timestamp the timestamp to give the resulting ROS message + * @param[in] frame the frame to set in the resulting ROS message + * @param[in] buf the raw packet message populated by read_imu_packet + * @return ROS sensor message with fields populated from the packet + */ +sensor_msgs::msg::Imu packet_to_imu_msg(const ouster::sensor::packet_format& pf, + const rclcpp::Time& timestamp, + const std::string& frame, + const uint8_t* buf); + /** * Parse an imu packet message into a ROS imu message * @param[in] pm packet message populated by read_imu_packet diff --git a/ouster-ros/src/imu_packet_handler.h b/ouster-ros/src/imu_packet_handler.h new file mode 100644 index 00000000..c8189d62 --- /dev/null +++ b/ouster-ros/src/imu_packet_handler.h @@ -0,0 +1,39 @@ +#pragma once +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file imu_packet_handler.h + * @brief ... + */ + +// prevent clang-format from altering the location of "ouster_ros/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" +// clang-format on + +class ImuPacketHandler { + public: + using HandlerOutput = sensor_msgs::msg::Imu; + using HandlerType = std::function; + + public: + static HandlerType create_handler( + const ouster::sensor::sensor_info& info, const std::string& frame, + bool use_ros_time) { + const auto& pf = ouster::sensor::get_format(info); + using Timestamper = std::function; + // clang-format off + auto timestamper = use_ros_time ? + Timestamper{[](const uint8_t* /*imu_buf*/) { + return rclcpp::Clock(RCL_ROS_TIME).now(); }} : + Timestamper{[pf](const uint8_t* imu_buf) { + return rclcpp::Time(pf.imu_gyro_ts(imu_buf)); }}; + // clang-format on + return [&pf, &frame, timestamper](const uint8_t* imu_buf) { + return ouster_ros::packet_to_imu_msg(pf, timestamper(imu_buf), + frame, imu_buf); + }; + } +}; \ No newline at end of file diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h new file mode 100644 index 00000000..3326aab8 --- /dev/null +++ b/ouster-ros/src/lidar_packet_handler.h @@ -0,0 +1,290 @@ +#pragma once +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file lidar_packet_handler.h + * @brief ... + */ + +// prevent clang-format from altering the location of "ouster_ros/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" +// clang-format on + +namespace { + +template +int find_if_reverse(const Eigen::Array& array, + UnaryPredicate predicate) { + auto p = array.data() + array.size() - 1; + do { + if (predicate(*p)) return p - array.data(); + } while (p-- != array.data()); + return -1; +} + +uint64_t linear_interpolate(int x0, uint64_t y0, int x1, uint64_t y1, int x) { + uint64_t min_v, max_v; + double sign; + if (y1 > y0) { + min_v = y0; + max_v = y1; + sign = +1; + } else { + min_v = y1; + max_v = y0; + sign = -1; + } + return y0 + (x - x0) * sign * (max_v - min_v) / (x1 - x0); +} + +template +uint64_t ulround(T value) { + T rounded_value = std::round(value); + if (rounded_value < 0) return 0ULL; + if (rounded_value > ULLONG_MAX) return ULLONG_MAX; + return static_cast(rounded_value); +} + +} // namespace + +namespace sensor = ouster::sensor; + + +class LidarPacketHandler { + using LidarPacketAccumlator = std::function; + + + public: + using HandlerOutput = std::vector>; + using HandlerType = std::function; + + public: + explicit LidarPacketHandler(const ouster::sensor::sensor_info& info, const std::string& frame, bool use_ros_time) : + ref_frame(frame) { + create_lidarscan_objects(info); + compute_scan_ts = [this](const auto& ts_v) { + return compute_scan_ts_0(ts_v); + }; + + scan_col_ts_spacing_ns = compute_scan_col_ts_spacing_ns(info.mode); + const sensor::packet_format& pf = sensor::get_format(info); + + lidar_packet_accumlator = use_ros_time ? + LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) { + return lidar_handler_ros_time(pf, lidar_buf); }} : + LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) { + return lidar_handler_sensor_time(pf, lidar_buf); }}; + } + + LidarPacketHandler(const LidarPacketHandler&) = delete; + LidarPacketHandler& operator=(const LidarPacketHandler&) = delete; + ~LidarPacketHandler() = default; + + public: + static HandlerType create_handler( + const ouster::sensor::sensor_info& info, const std::string& frame, bool use_ros_time) { + auto handler = std::make_shared(info, frame, use_ros_time); + return [handler](const uint8_t* lidar_buf) { + return handler->lidar_packet_accumlator(lidar_buf) ? handler->pc_msgs : HandlerOutput{}; + }; + } + + static int num_returns(const ouster::sensor::sensor_info& info) { + using ouster::sensor::UDPProfileLidar; + return info.format.udp_profile_lidar == + UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL + ? 2 + : 1; + } + + private: + void create_lidarscan_objects(const ouster::sensor::sensor_info& info) { + // The ouster_ros drive currently only uses single precision when it + // produces the point cloud. So it isn't of a benefit to compute point + // cloud xyz coordinates using double precision (for the time being). + auto xyz_lut = ouster::make_xyz_lut(info); + lut_direction = xyz_lut.direction.cast(); + lut_offset = xyz_lut.offset.cast(); + points = ouster::PointsF(lut_direction.rows(), lut_offset.cols()); + scan_batcher = std::make_unique(info); + uint32_t H = info.format.pixels_per_column; + uint32_t W = info.format.columns_per_frame; + lidar_scan = std::make_unique( + W, H, info.format.udp_profile_lidar); + cloud = ouster_ros::Cloud{W, H}; + pc_msgs.resize(num_returns(info), std::make_shared()); + } + + void pcl_toROSMsg(const ouster_ros::Cloud& pcl_cloud, + sensor_msgs::msg::PointCloud2& cloud) { + // TODO: remove the staging step in the future + pcl::toPCLPointCloud2(pcl_cloud, staging_pcl_pc2); + pcl_conversions::moveFromPCL(staging_pcl_pc2, cloud); + } + + void convert_scan_to_pointcloud(uint64_t scan_ts, + const rclcpp::Time& msg_ts) { + for (int i = 0; i < static_cast(pc_msgs.size()); ++i) { + scan_to_cloud_f(points, lut_direction, lut_offset, scan_ts, + *lidar_scan, cloud, i); + pcl_toROSMsg(cloud, *pc_msgs[i]); + pc_msgs[i]->header.stamp = msg_ts; + pc_msgs[i]->header.frame_id = ref_frame; + } + } + + // time interpolation methods + uint64_t impute_value(int last_scan_last_nonzero_idx, + uint64_t last_scan_last_nonzero_value, + int curr_scan_first_nonzero_idx, + uint64_t curr_scan_first_nonzero_value, + int scan_width) { + assert(scan_width + curr_scan_first_nonzero_idx > + last_scan_last_nonzero_idx); + double interpolated_value = linear_interpolate( + last_scan_last_nonzero_idx, last_scan_last_nonzero_value, + scan_width + curr_scan_first_nonzero_idx, + curr_scan_first_nonzero_value, scan_width); + return ulround(interpolated_value); + } + + uint64_t extrapolate_value(int curr_scan_first_nonzero_idx, + uint64_t curr_scan_first_nonzero_value) { + double extrapolated_value = + curr_scan_first_nonzero_value - + scan_col_ts_spacing_ns * curr_scan_first_nonzero_idx; + return ulround(extrapolated_value); + } + + // compute_scan_ts_0 for first scan + uint64_t compute_scan_ts_0( + const ouster::LidarScan::Header& ts_v) { + auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), + [](uint64_t h) { return h != 0; }); + assert(idx != ts_v.data() + ts_v.size()); // should never happen + int curr_scan_first_nonzero_idx = idx - ts_v.data(); + uint64_t curr_scan_first_nonzero_value = *idx; + + uint64_t scan_ns = + curr_scan_first_nonzero_idx == 0 + ? curr_scan_first_nonzero_value + : extrapolate_value(curr_scan_first_nonzero_idx, + curr_scan_first_nonzero_value); + + last_scan_last_nonzero_idx = + find_if_reverse(ts_v, [](uint64_t h) { return h != 0; }); + assert(last_scan_last_nonzero_idx >= 0); // should never happen + last_scan_last_nonzero_value = ts_v(last_scan_last_nonzero_idx); + compute_scan_ts = [this](const auto& ts_v) { + return compute_scan_ts_n(ts_v); + }; + + return scan_ns; + } + + // compute_scan_ts_n applied to all subsequent scans except first one + uint64_t compute_scan_ts_n( + const ouster::LidarScan::Header& ts_v) { + auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), + [](uint64_t h) { return h != 0; }); + assert(idx != ts_v.data() + ts_v.size()); // should never happen + int curr_scan_first_nonzero_idx = idx - ts_v.data(); + uint64_t curr_scan_first_nonzero_value = *idx; + + uint64_t scan_ns = curr_scan_first_nonzero_idx == 0 + ? curr_scan_first_nonzero_value + : impute_value(last_scan_last_nonzero_idx, + last_scan_last_nonzero_value, + curr_scan_first_nonzero_idx, + curr_scan_first_nonzero_value, + static_cast(ts_v.size())); + + last_scan_last_nonzero_idx = + find_if_reverse(ts_v, [](uint64_t h) { return h != 0; }); + assert(last_scan_last_nonzero_idx >= 0); // should never happen + last_scan_last_nonzero_value = ts_v(last_scan_last_nonzero_idx); + return scan_ns; + } + + uint16_t packet_col_index(const sensor::packet_format& pf, const uint8_t* lidar_buf) { + return pf.col_measurement_id(pf.nth_col(0, lidar_buf)); + } + + rclcpp::Time extrapolate_frame_ts(const sensor::packet_format& pf, + const uint8_t* lidar_buf, + const rclcpp::Time current_time) { + auto curr_scan_first_arrived_idx = packet_col_index(pf, lidar_buf); + auto delta_time = rclcpp::Duration( + 0, + std::lround(scan_col_ts_spacing_ns * curr_scan_first_arrived_idx)); + return current_time - delta_time; + } + + static inline rclcpp::Time to_ros_time(uint64_t ts) { + return rclcpp::Time(ts); + } + + bool lidar_handler_sensor_time(const sensor::packet_format& pf, const uint8_t* lidar_buf) { + if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; + auto scan_ts = compute_scan_ts(lidar_scan->timestamp()); + convert_scan_to_pointcloud(scan_ts, to_ros_time(scan_ts)); + return true; + } + + bool lidar_handler_ros_time(const sensor::packet_format& pf, + const uint8_t* lidar_buf) { + auto packet_receive_time = rclcpp::Clock(RCL_ROS_TIME).now(); + if (!lidar_handler_ros_time_frame_ts_initialized) { + lidar_handler_ros_time_frame_ts = extrapolate_frame_ts( + pf, lidar_buf, packet_receive_time); // first point cloud time + lidar_handler_ros_time_frame_ts_initialized = true; + } + if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; + auto scan_ts = compute_scan_ts(lidar_scan->timestamp()); + convert_scan_to_pointcloud(scan_ts, lidar_handler_ros_time_frame_ts); + // set time for next point cloud msg + lidar_handler_ros_time_frame_ts = + extrapolate_frame_ts(pf, lidar_buf, packet_receive_time); + return true; + } + + static double compute_scan_col_ts_spacing_ns(sensor::lidar_mode ld_mode) { + const auto scan_width = sensor::n_cols_of_lidar_mode(ld_mode); + const auto scan_frequency = sensor::frequency_of_lidar_mode(ld_mode); + const double one_sec_in_ns = 1e+9; + return one_sec_in_ns / (scan_width * scan_frequency); + } + + private: + std::string ref_frame; + + ouster::PointsF lut_direction; + ouster::PointsF lut_offset; + ouster::PointsF points; + std::unique_ptr lidar_scan; + ouster_ros::Cloud cloud; + std::unique_ptr scan_batcher; + + // a buffer used for staging during the conversion + // from a PCL point cloud to a ros point cloud message + pcl::PCLPointCloud2 staging_pcl_pc2; + + bool lidar_handler_ros_time_frame_ts_initialized = false; + rclcpp::Time lidar_handler_ros_time_frame_ts; + + int last_scan_last_nonzero_idx = -1; + uint64_t last_scan_last_nonzero_value = 0; + + double scan_col_ts_spacing_ns; // interval or spacing between columns of a + // scan + + std::function&)> + compute_scan_ts; + + public: + HandlerOutput pc_msgs; + LidarPacketAccumlator lidar_packet_accumlator; +}; \ No newline at end of file diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 8d4af4e2..b08ac226 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -27,46 +27,12 @@ #include "ouster_ros/os_processing_node_base.h" #include "ouster_ros/visibility_control.h" +#include "imu_packet_handler.h" +#include "lidar_packet_handler.h" + namespace sensor = ouster::sensor; using ouster_msgs::msg::PacketMsg; -namespace { - -template -int find_if_reverse(const Eigen::Array& array, - UnaryPredicate predicate) { - auto p = array.data() + array.size() - 1; - do { - if (predicate(*p)) return p - array.data(); - } while (p-- != array.data()); - return -1; -} - -uint64_t linear_interpolate(int x0, uint64_t y0, int x1, uint64_t y1, int x) { - uint64_t min_v, max_v; - double sign; - if (y1 > y0) { - min_v = y0; - max_v = y1; - sign = +1; - } else { - min_v = y1; - max_v = y0; - sign = -1; - } - return y0 + (x - x0) * sign * (max_v - min_v) / (x1 - x0); -} - -template -uint64_t ulround(T value) { - T rounded_value = std::round(value); - if (rounded_value < 0) return 0ULL; - if (rounded_value > ULLONG_MAX) return ULLONG_MAX; - return static_cast(rounded_value); -} - -} // namespace - namespace ouster_ros { class OusterCloud : public OusterProcessingNodeBase { @@ -106,25 +72,13 @@ class OusterCloud : public OusterProcessingNodeBase { use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME"; } - static double compute_scan_col_ts_spacing_ns(sensor::lidar_mode ld_mode) { - const auto scan_width = sensor::n_cols_of_lidar_mode(ld_mode); - const auto scan_frequency = sensor::frequency_of_lidar_mode(ld_mode); - const double one_sec_in_ns = 1e+9; - return one_sec_in_ns / (scan_width * scan_frequency); - } - void metadata_handler( const std_msgs::msg::String::ConstSharedPtr& metadata_msg) { RCLCPP_INFO(get_logger(), "OusterCloud: retrieved new sensor metadata!"); info = sensor::parse_metadata(metadata_msg->data); send_static_transforms(); - n_returns = get_n_returns(); - create_lidarscan_objects(); - compute_scan_ts = [this](const auto& ts_v) { - return compute_scan_ts_0(ts_v); - }; - create_publishers(); + create_publishers(get_n_returns()); create_subscriptions(); } @@ -135,27 +89,11 @@ class OusterCloud : public OusterProcessingNodeBase { info.imu_to_sensor_transform, sensor_frame, imu_frame, now())); } - void create_lidarscan_objects() { - // The ouster_ros drive currently only uses single precision when it - // produces the point cloud. So it isn't of a benefit to compute point - // cloud xyz coordinates using double precision (for the time being). - auto xyz_lut = ouster::make_xyz_lut(info); - lut_direction = xyz_lut.direction.cast(); - lut_offset = xyz_lut.offset.cast(); - points = ouster::PointsF(lut_direction.rows(), lut_offset.cols()); - scan_batcher = std::make_unique(info); - uint32_t H = info.format.pixels_per_column; - uint32_t W = info.format.columns_per_frame; - lidar_scan = std::make_unique( - W, H, info.format.udp_profile_lidar); - cloud = ouster_ros::Cloud{W, H}; - } - - void create_publishers() { + void create_publishers(int num_returns) { rclcpp::SensorDataQoS qos; imu_pub = create_publisher("imu", qos); - lidar_pubs.resize(n_returns); - for (int i = 0; i < n_returns; i++) { + lidar_pubs.resize(num_returns); + for (int i = 0; i < num_returns; i++) { lidar_pubs[i] = create_publisher( topic_for_return("points", i), qos); } @@ -164,187 +102,35 @@ class OusterCloud : public OusterProcessingNodeBase { void create_subscriptions() { rclcpp::SensorDataQoS qos; - using LidarHandlerFunctionType = - std::function; - LidarHandlerFunctionType lidar_handler_ros_time_function = - [this](const PacketMsg::ConstSharedPtr msg) { - lidar_handler_ros_time(msg); - }; - LidarHandlerFunctionType lidar_handler_sensor_time_function = + imu_packet_handler = ImuPacketHandler::create_handler( + info, imu_frame, use_ros_time); + imu_packet_sub = create_subscription( + "imu_packets", qos, [this](const PacketMsg::ConstSharedPtr msg) { - lidar_handler_sensor_time(msg); - }; - auto lidar_handler = use_ros_time ? lidar_handler_ros_time_function - : lidar_handler_sensor_time_function; + auto imu_msg = imu_packet_handler(msg->buf.data()); + imu_pub->publish(imu_msg); + }); + + lidar_packet_handler = LidarPacketHandler::create_handler( + info, sensor_frame, use_ros_time); // TODO: add an option to select sensor_frame lidar_packet_sub = create_subscription( "lidar_packets", qos, - [this, lidar_handler](const PacketMsg::ConstSharedPtr msg) { - lidar_handler(msg); + [this](const PacketMsg::ConstSharedPtr msg) { + auto point_cloud_msgs = lidar_packet_handler(msg->buf.data()); + for (size_t i = 0; i < point_cloud_msgs.size(); ++i) { + lidar_pubs[i]->publish(*point_cloud_msgs[i]); + } }); - imu_packet_sub = create_subscription( - "imu_packets", qos, - [this](const PacketMsg::ConstSharedPtr msg) { imu_handler(msg); }); - } - - void pcl_toROSMsg(const ouster_ros::Cloud& pcl_cloud, - sensor_msgs::msg::PointCloud2& cloud) { - // TODO: remove the staging step in the future - pcl::toPCLPointCloud2(pcl_cloud, staging_pcl_pc2); - pcl_conversions::moveFromPCL(staging_pcl_pc2, cloud); - } - - void convert_scan_to_pointcloud_publish(uint64_t scan_ts, - const rclcpp::Time& msg_ts) { - for (int i = 0; i < n_returns; ++i) { - scan_to_cloud_f(points, lut_direction, lut_offset, scan_ts, - *lidar_scan, cloud, i); - pcl_toROSMsg(cloud, pc_msg); - pc_msg.header.stamp = msg_ts; - pc_msg.header.frame_id = sensor_frame; - lidar_pubs[i]->publish(pc_msg); - } - } - - uint64_t impute_value(int last_scan_last_nonzero_idx, - uint64_t last_scan_last_nonzero_value, - int curr_scan_first_nonzero_idx, - uint64_t curr_scan_first_nonzero_value, - int scan_width) { - assert(scan_width + curr_scan_first_nonzero_idx > - last_scan_last_nonzero_idx); - double interpolated_value = linear_interpolate( - last_scan_last_nonzero_idx, last_scan_last_nonzero_value, - scan_width + curr_scan_first_nonzero_idx, - curr_scan_first_nonzero_value, scan_width); - return ulround(interpolated_value); - } - - uint64_t extrapolate_value(int curr_scan_first_nonzero_idx, - uint64_t curr_scan_first_nonzero_value) { - double extrapolated_value = - curr_scan_first_nonzero_value - - scan_col_ts_spacing_ns * curr_scan_first_nonzero_idx; - return ulround(extrapolated_value); - } - - // compute_scan_ts_0 for first scan - uint64_t compute_scan_ts_0( - const ouster::LidarScan::Header& ts_v) { - auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), - [](uint64_t h) { return h != 0; }); - assert(idx != ts_v.data() + ts_v.size()); // should never happen - int curr_scan_first_nonzero_idx = idx - ts_v.data(); - uint64_t curr_scan_first_nonzero_value = *idx; - - uint64_t scan_ns = - curr_scan_first_nonzero_idx == 0 - ? curr_scan_first_nonzero_value - : extrapolate_value(curr_scan_first_nonzero_idx, - curr_scan_first_nonzero_value); - - last_scan_last_nonzero_idx = - find_if_reverse(ts_v, [](uint64_t h) { return h != 0; }); - assert(last_scan_last_nonzero_idx >= 0); // should never happen - last_scan_last_nonzero_value = ts_v(last_scan_last_nonzero_idx); - compute_scan_ts = [this](const auto& ts_v) { - return compute_scan_ts_n(ts_v); - }; - return scan_ns; - } - - // compute_scan_ts_n applied to all subsequent scans except first one - uint64_t compute_scan_ts_n( - const ouster::LidarScan::Header& ts_v) { - auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), - [](uint64_t h) { return h != 0; }); - assert(idx != ts_v.data() + ts_v.size()); // should never happen - int curr_scan_first_nonzero_idx = idx - ts_v.data(); - uint64_t curr_scan_first_nonzero_value = *idx; - - uint64_t scan_ns = curr_scan_first_nonzero_idx == 0 - ? curr_scan_first_nonzero_value - : impute_value(last_scan_last_nonzero_idx, - last_scan_last_nonzero_value, - curr_scan_first_nonzero_idx, - curr_scan_first_nonzero_value, - static_cast(ts_v.size())); - - last_scan_last_nonzero_idx = - find_if_reverse(ts_v, [](uint64_t h) { return h != 0; }); - assert(last_scan_last_nonzero_idx >= 0); // should never happen - last_scan_last_nonzero_value = ts_v(last_scan_last_nonzero_idx); - return scan_ns; - } - - void lidar_handler_sensor_time(const PacketMsg::ConstPtr& packet) { - if (!(*scan_batcher)(packet->buf.data(), *lidar_scan)) return; - auto scan_ts = compute_scan_ts(lidar_scan->timestamp()); - convert_scan_to_pointcloud_publish(scan_ts, to_ros_time(scan_ts)); - } - - uint16_t packet_col_index(const uint8_t* packet_buf) { - const auto& pf = sensor::get_format(info); - return pf.col_measurement_id(pf.nth_col(0, packet_buf)); - } - - rclcpp::Time extrapolate_frame_ts(const uint8_t* lidar_buf, - const rclcpp::Time current_time) { - auto curr_scan_first_arrived_idx = packet_col_index(lidar_buf); - auto delta_time = rclcpp::Duration( - 0, - std::lround(scan_col_ts_spacing_ns * curr_scan_first_arrived_idx)); - return current_time - delta_time; - } - - void lidar_handler_ros_time(const PacketMsg::ConstPtr& packet) { - auto packet_receive_time = rclcpp::Clock(RCL_ROS_TIME).now(); - const uint8_t* packet_buf = packet->buf.data(); - if (!lidar_handler_ros_time_frame_ts_initialized) { - lidar_handler_ros_time_frame_ts = extrapolate_frame_ts( - packet_buf, packet_receive_time); // first point cloud time - lidar_handler_ros_time_frame_ts_initialized = true; - } - if (!(*scan_batcher)(packet_buf, *lidar_scan)) return; - auto scan_ts = compute_scan_ts(lidar_scan->timestamp()); - convert_scan_to_pointcloud_publish(scan_ts, - lidar_handler_ros_time_frame_ts); - // set time for next point cloud msg - lidar_handler_ros_time_frame_ts = - extrapolate_frame_ts(packet_buf, packet_receive_time); - } - - void imu_handler(const PacketMsg::ConstSharedPtr packet) { - auto pf = sensor::get_format(info); - auto msg_ts = use_ros_time - ? rclcpp::Clock(RCL_ROS_TIME).now() - : to_ros_time(pf.imu_gyro_ts(packet->buf.data())); - auto imu_msg = - ouster_ros::packet_to_imu_msg(*packet, msg_ts, imu_frame, pf); - imu_pub->publish(imu_msg); - }; - - static inline rclcpp::Time to_ros_time(uint64_t ts) { - return rclcpp::Time(ts); } private: rclcpp::Subscription::SharedPtr lidar_packet_sub; std::vector::SharedPtr> lidar_pubs; + rclcpp::Subscription::SharedPtr imu_packet_sub; rclcpp::Publisher::SharedPtr imu_pub; - sensor_msgs::msg::PointCloud2 pc_msg; - - int n_returns = 0; - - ouster::PointsF lut_direction; - ouster::PointsF lut_offset; - ouster::PointsF points; - std::unique_ptr lidar_scan; - ouster_ros::Cloud cloud; - std::unique_ptr scan_batcher; - std::string sensor_frame; std::string imu_frame; std::string lidar_frame; @@ -353,19 +139,9 @@ class OusterCloud : public OusterProcessingNodeBase { bool use_ros_time; - int last_scan_last_nonzero_idx = -1; - uint64_t last_scan_last_nonzero_value = 0; - std::function&)> - compute_scan_ts; - double scan_col_ts_spacing_ns; // interval or spacing between columns of a - // scan - - pcl::PCLPointCloud2 - staging_pcl_pc2; // a buffer used for staging during the conversion - // from a PCL point cloud to a ros point cloud message + ImuPacketHandler::HandlerType imu_packet_handler; - bool lidar_handler_ros_time_frame_ts_initialized = false; - rclcpp::Time lidar_handler_ros_time_frame_ts; + LidarPacketHandler::HandlerType lidar_packet_handler; }; } // namespace ouster_ros diff --git a/ouster-ros/src/os_ros.cpp b/ouster-ros/src/os_ros.cpp index f13cf1a3..2ce74b16 100644 --- a/ouster-ros/src/os_ros.cpp +++ b/ouster-ros/src/os_ros.cpp @@ -38,22 +38,20 @@ bool read_lidar_packet(const sensor::client& cli, PacketMsg& pm, return read_lidar_packet(cli, pm.buf.data(), pf); } -sensor_msgs::msg::Imu packet_to_imu_msg(const PacketMsg& pm, - const rclcpp::Time& timestamp, - const std::string& frame, - const sensor::packet_format& pf) { - const double standard_g = 9.80665; +sensor_msgs::msg::Imu packet_to_imu_msg(const ouster::sensor::packet_format& pf, + const rclcpp::Time& timestamp, + const std::string& frame, + const uint8_t* buf) { sensor_msgs::msg::Imu m; - const uint8_t* buf = pm.buf.data(); - m.header.stamp = timestamp; m.header.frame_id = frame; m.orientation.x = 0; m.orientation.y = 0; m.orientation.z = 0; - m.orientation.w = 0; + m.orientation.w = 1; + const double standard_g = 9.80665; m.linear_acceleration.x = pf.imu_la_x(buf) * standard_g; m.linear_acceleration.y = pf.imu_la_y(buf) * standard_g; m.linear_acceleration.z = pf.imu_la_z(buf) * standard_g; @@ -67,6 +65,7 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const PacketMsg& pm, m.angular_velocity_covariance[i] = 0; m.linear_acceleration_covariance[i] = 0; } + for (int i = 0; i < 9; i += 4) { m.linear_acceleration_covariance[i] = 0.01; m.angular_velocity_covariance[i] = 6e-4; @@ -75,6 +74,14 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const PacketMsg& pm, return m; } +sensor_msgs::msg::Imu packet_to_imu_msg(const PacketMsg& pm, + const rclcpp::Time& timestamp, + const std::string& frame, + const sensor::packet_format& pf) { + + return packet_to_imu_msg(pf, timestamp, frame, pm.buf.data()); +} + struct read_and_cast { template void operator()(Eigen::Ref> field, diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index 0a6979fe..c2b143ec 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -89,6 +89,7 @@ class OusterSensor : public OusterSensorNodeBase { RCLCPP_DEBUG(get_logger(), "on_activate() is called."); LifecycleNode::on_activate(state); allocate_buffers(); + start_packet_processing_thread(); start_sensor_connection_thread(); return LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -100,29 +101,12 @@ class OusterSensor : public OusterSensorNodeBase { return LifecycleNodeInterface::CallbackReturn::FAILURE; } - void start_sensor_connection_thread() { - sensor_connection_active = true; - sensor_connection_thread = std::make_unique([this]() { - auto& pf = sensor::get_format(info); - while (sensor_connection_active) { - connection_loop(*sensor_client, pf); - } - RCLCPP_INFO(get_logger(), "connection_loop DONE."); - }); - } - - void stop_sensor_connection_thread() { - if (sensor_connection_thread->joinable()) { - sensor_connection_active = false; - sensor_connection_thread->join(); - } - } - LifecycleNodeInterface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State& state) { RCLCPP_DEBUG(get_logger(), "on_deactivate() is called."); LifecycleNode::on_deactivate(state); stop_sensor_connection_thread(); + stop_packet_processing_thread(); return LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -153,6 +137,7 @@ class OusterSensor : public OusterSensorNodeBase { if (state.label() == "active") { stop_sensor_connection_thread(); + stop_packet_processing_thread(); } // whether state was 'active' or 'inactive' do cleanup @@ -733,43 +718,70 @@ class OusterSensor : public OusterSensorNodeBase { void handle_lidar_packet(sensor::client& cli, const sensor::packet_format& pf) { - if (sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf)) { - read_lidar_packet_errors = 0; - if (is_non_legacy_lidar_profile(info) && - init_id_changed(pf, lidar_packet.buf.data())) { - // TODO: short circut reset if no breaking changes occured? - RCLCPP_WARN(get_logger(), - "sensor init_id has changed! reactivating.."); - reactivate_sensor(); - } else { - lidar_packet_pub->publish(lidar_packet); - } - } else { - if (++read_lidar_packet_errors > max_read_lidar_packet_errors) { - RCLCPP_ERROR_STREAM( - get_logger(), - "maximum number of allowed errors from " - "sensor::read_lidar_packet() reached, reactivating..."); - read_lidar_packet_errors = 0; - reactivate_sensor(true); - } - } + std::unique_lock lock(mtx); + cv.wait(lock, [this]{ return lidar_data_processed; }); + lidar_data_processed = false; + bool success = sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf); + lidar_data_available_for_processing = true; + lock.unlock(); + cv.notify_all(); + + // if (success) { + // read_lidar_packet_errors = 0; + // if (is_non_legacy_lidar_profile(info) && + // init_id_changed(pf, lidar_packet.buf.data())) { + // // TODO: short circut reset if no breaking changes occured? + // RCLCPP_WARN(get_logger(), + // "sensor init_id has changed! reactivating.."); + // reactivate_sensor(); + // } else { + // lidar_packet_pub->publish(lidar_packet); + // } + // } else { + // if (++read_lidar_packet_errors > max_read_lidar_packet_errors) { + // RCLCPP_ERROR_STREAM( + // get_logger(), + // "maximum number of allowed errors from " + // "sensor::read_lidar_packet() reached, reactivating..."); + // read_lidar_packet_errors = 0; + // reactivate_sensor(true); + // } + // } } void handle_imu_packet(sensor::client& cli, const sensor::packet_format& pf) { - if (sensor::read_imu_packet(cli, imu_packet.buf.data(), pf)) { - imu_packet_pub->publish(imu_packet); - } else { - if (++read_imu_packet_errors > max_read_imu_packet_errors) { - RCLCPP_ERROR_STREAM( - get_logger(), - "maximum number of allowed errors from " - "sensor::read_imu_packet() reached, reactivating..."); - read_imu_packet_errors = 0; - reactivate_sensor(true); - } - } + std::unique_lock lock(mtx); + cv.wait(lock, [this]{ return imu_data_processed; }); + imu_data_processed = false; + bool success = sensor::read_imu_packet(cli, imu_packet.buf.data(), pf); + imu_data_available_for_processing = true; + lock.unlock(); + cv.notify_all(); + + // if (success) { + // imu_packet_pub->publish(imu_packet); + // } else { + // if (++read_imu_packet_errors > max_read_imu_packet_errors) { + // RCLCPP_ERROR_STREAM( + // get_logger(), + // "maximum number of allowed errors from " + // "sensor::read_imu_packet() reached, reactivating..."); + // read_imu_packet_errors = 0; + // reactivate_sensor(true); + // } + // } + } + + void cleanup() { + sensor_client.reset(); + lidar_packet_pub.reset(); + imu_packet_pub.reset(); + get_metadata_srv.reset(); + get_config_srv.reset(); + set_config_srv.reset(); + sensor_connection_thread.reset(); + packet_processing_thread.reset(); } void connection_loop(sensor::client& cli, const sensor::packet_format& pf) { @@ -791,14 +803,66 @@ class OusterSensor : public OusterSensorNodeBase { } } - void cleanup() { - sensor_client.reset(); - lidar_packet_pub.reset(); - imu_packet_pub.reset(); - get_metadata_srv.reset(); - get_config_srv.reset(); - set_config_srv.reset(); - sensor_connection_thread.reset(); + void start_sensor_connection_thread() { + sensor_connection_active = true; + sensor_connection_thread = std::make_unique([this]() { + auto& pf = sensor::get_format(info); + while (sensor_connection_active) { + connection_loop(*sensor_client, pf); + } + RCLCPP_DEBUG(get_logger(), "sensor_connection_thread done."); + }); + } + + void stop_sensor_connection_thread() { + RCLCPP_DEBUG(get_logger(), "sensor_connection_thread stopping."); + if (sensor_connection_thread->joinable()) { + sensor_connection_active = false; + sensor_connection_thread->join(); + } + } + + void start_packet_processing_thread() { + packet_processing_active = true; + packet_processing_thread = std::make_unique([this]() { + while (packet_processing_active) { + process_packets(); + } + RCLCPP_DEBUG(get_logger(), "packet_processing_thread done."); + }); + } + + void stop_packet_processing_thread() { + RCLCPP_DEBUG(get_logger(), "packet_processing_thread stopping."); + if (packet_processing_thread->joinable()) { + packet_processing_active = false; + packet_processing_thread->join(); + } + } + + void process_packets() { + while (packet_processing_active) { + std::unique_lock lock(mtx); + + // Wait until there is a job in the queue + cv.wait(lock, [this]{ + return lidar_data_available_for_processing || imu_data_available_for_processing; }); + + if (lidar_data_available_for_processing) { + lidar_data_available_for_processing = false; + lidar_packet_pub->publish(lidar_packet); + lidar_data_processed = true; + } + + if (imu_data_available_for_processing) { + imu_data_available_for_processing = false; + imu_packet_pub->publish(imu_packet); + imu_data_processed = true; + } + + lock.unlock(); + cv.notify_all(); + } } private: @@ -816,11 +880,23 @@ class OusterSensor : public OusterSensorNodeBase { rclcpp::Service::SharedPtr get_config_srv; rclcpp::Service::SharedPtr set_config_srv; std::shared_ptr> change_state_client; + + std::mutex mtx; + std::condition_variable cv; + bool lidar_data_available_for_processing = false; + bool lidar_data_processed = true; + + bool imu_data_available_for_processing = false; + bool imu_data_processed = true; + + std::atomic sensor_connection_active = {false}; std::unique_ptr sensor_connection_thread; + std::atomic packet_processing_active = {false}; + std::unique_ptr packet_processing_thread; + bool force_sensor_reinit = false; bool reset_last_init_id = true; - std::atomic sensor_connection_active = {false}; bool last_init_id_initialized = false; uint32_t last_init_id; From 63484e381ba51547365be4c3c75eedc79d1b9872 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 31 May 2023 09:28:02 -0700 Subject: [PATCH 02/38] Added os_driver which combines os_sensor and os_cloud + Added required launch files + Better abstraction of classes + Simplified threading logic + Added thread safe implemention of ring buffer (not hooked yet) --- ouster-ros/CMakeLists.txt | 11 + .../ouster_ros/os_processing_node_base.h | 9 +- ouster-ros/include/ouster_ros/os_ros.h | 24 + .../include/ouster_ros/os_sensor_node_base.h | 4 + ouster-ros/launch/driver.launch.py | 112 ++ ouster-ros/launch/sensor.composite.launch.py | 5 +- .../launch/sensor.independent.launch.py | 6 +- ouster-ros/src/lidar_packet_handler.h | 10 +- ouster-ros/src/os_cloud_node.cpp | 3 +- ouster-ros/src/os_driver_node.cpp | 111 ++ ouster-ros/src/os_image_node.cpp | 2 +- ouster-ros/src/os_processing_node_base.cpp | 9 - ouster-ros/src/os_replay_node.cpp | 4 - ouster-ros/src/os_ros.cpp | 18 + ouster-ros/src/os_sensor_node.cpp | 1538 ++++++++--------- ouster-ros/src/os_sensor_node.h | 178 ++ ouster-ros/src/os_sensor_node_base.cpp | 24 +- ouster-ros/src/thread_safe_ring_buffer.h | 129 ++ 18 files changed, 1352 insertions(+), 845 deletions(-) create mode 100644 ouster-ros/launch/driver.launch.py create mode 100644 ouster-ros/src/os_driver_node.cpp create mode 100644 ouster-ros/src/os_sensor_node.h create mode 100644 ouster-ros/src/thread_safe_ring_buffer.h diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt index d7f70c4a..693ff1cc 100644 --- a/ouster-ros/CMakeLists.txt +++ b/ouster-ros/CMakeLists.txt @@ -156,6 +156,16 @@ rclcpp_components_register_node(os_image_component EXECUTABLE os_image ) +# ==== os_sensor_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" +) +rclcpp_components_register_node(os_driver_component + PLUGIN "ouster_ros::OusterDriver" + EXECUTABLE os_driver +) + # ==== Install ==== install( TARGETS @@ -164,6 +174,7 @@ install( os_replay_component os_cloud_component os_image_component + os_driver_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/ouster-ros/include/ouster_ros/os_processing_node_base.h b/ouster-ros/include/ouster_ros/os_processing_node_base.h index 66237edd..7b38b14c 100644 --- a/ouster-ros/include/ouster_ros/os_processing_node_base.h +++ b/ouster-ros/include/ouster_ros/os_processing_node_base.h @@ -17,7 +17,7 @@ namespace ouster_ros { class OusterProcessingNodeBase : public rclcpp::Node { protected: - explicit OusterProcessingNodeBase(const std::string& name, + OusterProcessingNodeBase(const std::string& name, const rclcpp::NodeOptions& options) : rclcpp::Node(name, options) {} @@ -25,13 +25,6 @@ class OusterProcessingNodeBase : public rclcpp::Node { std::function on_sensor_metadata); - int get_n_returns() const; - - static std::string topic_for_return(std::string base, int idx) { - if (idx == 0) return base; - return base + std::to_string(idx + 1); - } - protected: rclcpp::Subscription::SharedPtr metadata_sub; ouster::sensor::sensor_info info; diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h index 4b4c8ff1..617932fa 100644 --- a/ouster-ros/include/ouster_ros/os_ros.h +++ b/ouster-ros/include/ouster_ros/os_ros.h @@ -33,6 +33,28 @@ namespace sensor = ouster::sensor; using Cloud = pcl::PointCloud; using ns = std::chrono::nanoseconds; +/** + * Checks sensor_info if it currently represents a legacy udp lidar profile + * @param[in] info sensor_info + * @return whether sensor_info represents the legacy udp lidar profile + */ +bool is_legacy_lidar_profile(const sensor::sensor_info& info); + +/** + * Gets the number of point cloud returns that this sensor_info object represents + * @param[in] info sensor_info + * @return number of returns + */ +int get_n_returns(const sensor::sensor_info& info); + +/** + * Adds a suffix to the topic base name based on the return index + * @param[in] topic_base topic base name + * @param[in] return_idx return index {0, 1, ... n_returns } + * @return number of returns + */ +std::string topic_for_return(const std::string& topic_base, int return_idx); + /** * Read an imu packet into a ROS message. Blocks for up to a second if no data * is available. @@ -41,6 +63,7 @@ using ns = std::chrono::nanoseconds; * @param[in] pf the packet format * @return whether reading was successful */ +[[deprecated("no longer used internally and will be removed")]] bool read_imu_packet(const sensor::client& cli, ouster_msgs::msg::PacketMsg& pm, const sensor::packet_format& pf); @@ -52,6 +75,7 @@ bool read_imu_packet(const sensor::client& cli, ouster_msgs::msg::PacketMsg& pm, * @param[in] pf the packet format * @return whether reading was successful */ +[[deprecated("no longer used internally and will be removed")]] bool read_lidar_packet(const sensor::client& cli, ouster_msgs::msg::PacketMsg& pm, const sensor::packet_format& pf); diff --git a/ouster-ros/include/ouster_ros/os_sensor_node_base.h b/ouster-ros/include/ouster_ros/os_sensor_node_base.h index 8ec78e07..2d810e2a 100644 --- a/ouster-ros/include/ouster_ros/os_sensor_node_base.h +++ b/ouster-ros/include/ouster_ros/os_sensor_node_base.h @@ -36,6 +36,10 @@ class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode { void display_lidar_info(const ouster::sensor::sensor_info& info); + static std::string read_text_file(const std::string& text_file); + + static bool write_text_to_file(const std::string& file_path, const std::string& text); + protected: ouster::sensor::sensor_info info; rclcpp::Service::SharedPtr get_metadata_srv; diff --git a/ouster-ros/launch/driver.launch.py b/ouster-ros/launch/driver.launch.py new file mode 100644 index 00000000..af6ad6a4 --- /dev/null +++ b/ouster-ros/launch/driver.launch.py @@ -0,0 +1,112 @@ +# Copyright 2023 Ouster, Inc. +# + +"""Launch a sensor node along with os_cloud and os_""" + +from pathlib import Path +import launch +import lifecycle_msgs.msg +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node, LifecycleNode +from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, + RegisterEventHandler, EmitEvent, LogInfo) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch.events import matches_action +from launch_ros.events.lifecycle import ChangeState +from launch_ros.event_handlers import OnStateTransition + + +def generate_launch_description(): + """ + Generate launch description for running ouster_ros components separately each + component will run in a separate process). + """ + ouster_ros_pkg_dir = get_package_share_directory('ouster_ros') + default_params_file = \ + Path(ouster_ros_pkg_dir) / 'config' / 'parameters.yaml' + params_file = LaunchConfiguration('params_file') + params_file_arg = DeclareLaunchArgument('params_file', + default_value=str( + default_params_file), + description='name or path to the parameters file to use.') + + ouster_ns = LaunchConfiguration('ouster_ns') + ouster_ns_arg = DeclareLaunchArgument( + 'ouster_ns', default_value='ouster') + + rviz_enable = LaunchConfiguration('viz') + rviz_enable_arg = DeclareLaunchArgument('viz', default_value='True') + + os_driver = LifecycleNode( + package='ouster_ros', + executable='os_driver', + name='os_driver', + namespace=ouster_ns, + parameters=[params_file], + output='screen', + ) + + sensor_configure_event = EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action(os_driver), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + ) + ) + + sensor_activate_event = RegisterEventHandler( + OnStateTransition( + target_lifecycle_node=os_driver, goal_state='inactive', + entities=[ + LogInfo(msg="os_driver activating..."), + EmitEvent(event=ChangeState( + lifecycle_node_matcher=matches_action(os_driver), + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, + )), + ], + handle_once=True + ) + ) + + # TODO: figure out why registering for on_shutdown event causes an exception + # and error handling + # shutdown_event = RegisterEventHandler( + # OnShutdown( + # on_shutdown=[ + # EmitEvent(event=ChangeState( + # lifecycle_node_matcher=matches_node_name(node_name=F"/ouster/os_sensor"), + # transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN, + # )), + # LogInfo(msg="os_sensor node exiting..."), + # ], + # ) + # ) + + os_image = Node( + package='ouster_ros', + executable='os_image', + name='os_image', + namespace=ouster_ns, + parameters=[params_file], + output='screen', + ) + + rviz_launch_file_path = \ + Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py' + rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([str(rviz_launch_file_path)]), + condition=IfCondition(rviz_enable) + ) + + return launch.LaunchDescription([ + params_file_arg, + ouster_ns_arg, + rviz_enable_arg, + rviz_launch, + os_driver, + os_image, + sensor_configure_event, + sensor_activate_event, + # shutdown_event + ]) diff --git a/ouster-ros/launch/sensor.composite.launch.py b/ouster-ros/launch/sensor.composite.launch.py index 0db299ac..c1db8dcf 100644 --- a/ouster-ros/launch/sensor.composite.launch.py +++ b/ouster-ros/launch/sensor.composite.launch.py @@ -97,7 +97,8 @@ def invoke_lifecycle_cmd(node_name, verb): params_file_arg, ouster_ns_arg, rviz_enable_arg, + rviz_launch, os_container, sensor_configure_cmd, - TimerAction(period=1.0, actions=[sensor_activate_cmd]), - rviz_launch]) + TimerAction(period=1.0, actions=[sensor_activate_cmd]) + ]) diff --git a/ouster-ros/launch/sensor.independent.launch.py b/ouster-ros/launch/sensor.independent.launch.py index 8ef67d4f..5d190093 100644 --- a/ouster-ros/launch/sensor.independent.launch.py +++ b/ouster-ros/launch/sensor.independent.launch.py @@ -1,14 +1,14 @@ # Copyright 2023 Ouster, Inc. # -"""Launch a sensor node along with...""" +"""Launch a sensor node along with os_cloud and os_""" from pathlib import Path import launch import lifecycle_msgs.msg from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node, LifecycleNode -from launch.actions import (IncludeLaunchDescription, DeclareLaunchArgument, +from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler, EmitEvent, LogInfo) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -112,10 +112,10 @@ def generate_launch_description(): params_file_arg, ouster_ns_arg, rviz_enable_arg, + rviz_launch, os_sensor, os_cloud, os_image, - rviz_launch, sensor_configure_event, sensor_activate_event, # shutdown_event diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h index 3326aab8..9dcf0c59 100644 --- a/ouster-ros/src/lidar_packet_handler.h +++ b/ouster-ros/src/lidar_packet_handler.h @@ -13,6 +13,8 @@ #include "ouster_ros/os_ros.h" // clang-format on +#include + namespace { template @@ -62,7 +64,7 @@ class LidarPacketHandler { using HandlerType = std::function; public: - explicit LidarPacketHandler(const ouster::sensor::sensor_info& info, const std::string& frame, bool use_ros_time) : + LidarPacketHandler(const ouster::sensor::sensor_info& info, const std::string& frame, bool use_ros_time) : ref_frame(frame) { create_lidarscan_objects(info); compute_scan_ts = [this](const auto& ts_v) { @@ -223,14 +225,10 @@ class LidarPacketHandler { return current_time - delta_time; } - static inline rclcpp::Time to_ros_time(uint64_t ts) { - return rclcpp::Time(ts); - } - bool lidar_handler_sensor_time(const sensor::packet_format& pf, const uint8_t* lidar_buf) { if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; auto scan_ts = compute_scan_ts(lidar_scan->timestamp()); - convert_scan_to_pointcloud(scan_ts, to_ros_time(scan_ts)); + convert_scan_to_pointcloud(scan_ts, rclcpp::Time(scan_ts)); return true; } diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index b08ac226..36afe33c 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -15,7 +15,6 @@ #include #include #include -#include #include #include @@ -78,7 +77,7 @@ class OusterCloud : public OusterProcessingNodeBase { "OusterCloud: retrieved new sensor metadata!"); info = sensor::parse_metadata(metadata_msg->data); send_static_transforms(); - create_publishers(get_n_returns()); + create_publishers(get_n_returns(info)); create_subscriptions(); } diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp new file mode 100644 index 00000000..ed78b6c7 --- /dev/null +++ b/ouster-ros/src/os_driver_node.cpp @@ -0,0 +1,111 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file os_driver.cpp + * @brief This node combines the capabilities of os_sensor and os_cloud into + * one ROS node/component + */ + +// prevent clang-format from altering the location of "ouster_ros/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" +// clang-format on + +#include + +#include "os_sensor_node.h" + +#include "imu_packet_handler.h" +#include "lidar_packet_handler.h" + +namespace ouster_ros { + +using ouster_msgs::msg::PacketMsg; + +class OusterDriver : public OusterSensor { + public: + OUSTER_ROS_PUBLIC + explicit OusterDriver(const rclcpp::NodeOptions& options) + : OusterSensor("os_driver", options), tf_bcast(this) { + declare_parameters(); + parse_parameters(); + } + +private: + void declare_parameters() { + declare_parameter("sensor_frame", "os_sensor"); + declare_parameter("lidar_frame", "os_lidar"); + declare_parameter("imu_frame", "os_imu"); + } + + void parse_parameters() { + sensor_frame = get_parameter("sensor_frame").as_string(); + lidar_frame = get_parameter("lidar_frame").as_string(); + imu_frame = get_parameter("imu_frame").as_string(); + } + + virtual void on_metadata_updated(const sensor::sensor_info& info) override { + OusterSensor::on_metadata_updated(info); + send_static_transforms(info); + } + + void send_static_transforms(const sensor::sensor_info& info) { + tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + info.lidar_to_sensor_transform, sensor_frame, lidar_frame, now())); + tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + info.imu_to_sensor_transform, sensor_frame, imu_frame, now())); + } + + virtual void create_publishers() override { + int num_returns = get_n_returns(info); + + rclcpp::SensorDataQoS qos; + imu_pub = create_publisher("imu", qos); + lidar_pubs.resize(num_returns); + for (int i = 0; i < num_returns; i++) { + lidar_pubs[i] = create_publisher( + topic_for_return("points", i), qos); + } + + auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); + bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME"; + + imu_packet_handler = ImuPacketHandler::create_handler( + info, imu_frame, use_ros_time); + lidar_packet_handler = LidarPacketHandler::create_handler( + info, sensor_frame, use_ros_time); // TODO: add an option to select sensor_frame + } + + virtual void on_lidar_packet_msg(const PacketMsg& lidar_packet) override { + auto point_cloud_msgs = lidar_packet_handler(lidar_packet.buf.data()); + for (size_t i = 0; i < point_cloud_msgs.size(); ++i) { + lidar_pubs[i]->publish(*point_cloud_msgs[i]); + } + } + + virtual void on_imu_packet_msg(const PacketMsg& imu_packet) override { + auto imu_msg = imu_packet_handler(imu_packet.buf.data()); + imu_pub->publish(imu_msg); + } + +private: + std::string sensor_frame; + std::string imu_frame; + std::string lidar_frame; + + tf2_ros::StaticTransformBroadcaster tf_bcast; + + std::vector::SharedPtr> + lidar_pubs; + rclcpp::Publisher::SharedPtr imu_pub; + ImuPacketHandler::HandlerType imu_packet_handler; + LidarPacketHandler::HandlerType lidar_packet_handler; +}; + +} // namespace ouster_ros + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(ouster_ros::OusterDriver) diff --git a/ouster-ros/src/os_image_node.cpp b/ouster-ros/src/os_image_node.cpp index 4289adf8..a874af8f 100644 --- a/ouster-ros/src/os_image_node.cpp +++ b/ouster-ros/src/os_image_node.cpp @@ -62,7 +62,7 @@ class OusterImage : public OusterProcessingNodeBase { "OusterImage: retrieved new sensor metadata!"); info = sensor::parse_metadata(metadata_msg->data); create_cloud_object(); - const int n_returns = get_n_returns(); + const int n_returns = get_n_returns(info); create_publishers(n_returns); create_subscriptions(n_returns); } diff --git a/ouster-ros/src/os_processing_node_base.cpp b/ouster-ros/src/os_processing_node_base.cpp index a066980d..2875ec4c 100644 --- a/ouster-ros/src/os_processing_node_base.cpp +++ b/ouster-ros/src/os_processing_node_base.cpp @@ -8,8 +8,6 @@ #include "ouster_ros/os_processing_node_base.h" -using ouster::sensor::UDPProfileLidar; - namespace ouster_ros { void OusterProcessingNodeBase::create_metadata_subscriber( @@ -22,11 +20,4 @@ void OusterProcessingNodeBase::create_metadata_subscriber( "metadata", latching_qos, on_sensor_metadata); } -int OusterProcessingNodeBase::get_n_returns() const { - return info.format.udp_profile_lidar == - UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL - ? 2 - : 1; -} - } // namespace ouster_ros diff --git a/ouster-ros/src/os_replay_node.cpp b/ouster-ros/src/os_replay_node.cpp index c13b1b24..a7101ced 100644 --- a/ouster-ros/src/os_replay_node.cpp +++ b/ouster-ros/src/os_replay_node.cpp @@ -114,10 +114,6 @@ class OusterReplay : public OusterSensorNodeBase { void load_metadata_from_file(const std::string& meta_file) { try { - std::ifstream in_file(meta_file); - std::stringstream buffer; - buffer << in_file.rdbuf(); - cached_metadata = buffer.str(); info = sensor::parse_metadata(cached_metadata); display_lidar_info(info); } catch (const std::runtime_error& e) { diff --git a/ouster-ros/src/os_ros.cpp b/ouster-ros/src/os_ros.cpp index 2ce74b16..f756aa01 100644 --- a/ouster-ros/src/os_ros.cpp +++ b/ouster-ros/src/os_ros.cpp @@ -26,6 +26,24 @@ using ouster_msgs::msg::PacketMsg; namespace ouster_ros { +bool is_legacy_lidar_profile(const sensor::sensor_info& info) { + using sensor::UDPProfileLidar; + return info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY; +} + +int get_n_returns(const sensor::sensor_info& info) { + using sensor::UDPProfileLidar; + return info.format.udp_profile_lidar == + UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL + ? 2 + : 1; +} + +std::string topic_for_return(const std::string& base, int idx) { + return idx == 0 ? base : base + std::to_string(idx + 1); +} + + bool read_imu_packet(const sensor::client& cli, PacketMsg& pm, const sensor::packet_format& pf) { pm.buf.resize(pf.imu_packet_size + 1); diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index c2b143ec..647ed4bd 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -12,905 +12,827 @@ #include "ouster_ros/os_ros.h" // clang-format on -#include -#include #include #include -#include - -#include -#include -#include -#include "ouster_msgs/msg/packet_msg.hpp" -#include "ouster_srvs/srv/get_config.hpp" -#include "ouster_srvs/srv/set_config.hpp" -#include "ouster_ros/visibility_control.h" -#include "ouster_ros/os_sensor_node_base.h" - -namespace sensor = ouster::sensor; -using lifecycle_msgs::srv::ChangeState; + +#include "os_sensor_node.h" + using ouster_msgs::msg::PacketMsg; -using ouster_srvs::srv::GetConfig; -using ouster_srvs::srv::SetConfig; -using rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface; -using sensor::UDPProfileLidar; using namespace std::chrono_literals; using namespace std::string_literals; namespace ouster_ros { -class OusterSensor : public OusterSensorNodeBase { - public: - OUSTER_ROS_PUBLIC - explicit OusterSensor(const rclcpp::NodeOptions& options) - : OusterSensorNodeBase("os_sensor", options) { - declare_parameters(); - change_state_client = - create_client(get_name() + "/change_state"s); - } - - LifecycleNodeInterface::CallbackReturn on_configure( - const rclcpp_lifecycle::State&) { - RCLCPP_DEBUG(get_logger(), "on_configure() is called."); - - try { - sensor_hostname = get_sensor_hostname(); - auto config = staged_config.empty() - ? parse_config_from_ros_parameters() - : parse_config_from_staged_config_string(); - configure_sensor(sensor_hostname, config); - sensor_client = create_sensor_client(sensor_hostname, config); - if (!sensor_client) - return LifecycleNodeInterface::CallbackReturn::FAILURE; - create_metadata_publisher(); - update_config_and_metadata(*sensor_client); - publish_metadata(); - save_metadata(); - create_reset_service(); - create_get_metadata_service(); - create_get_config_service(); - create_set_config_service(); - create_publishers(); - } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM( - get_logger(), - "exception thrown while configuring the sensor, details: " - << ex.what()); - // TODO: return ERROR on fatal errors, FAILURE otherwise - return LifecycleNodeInterface::CallbackReturn::ERROR; - } - - return LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - LifecycleNodeInterface::CallbackReturn on_activate( - const rclcpp_lifecycle::State& state) { - RCLCPP_DEBUG(get_logger(), "on_activate() is called."); - LifecycleNode::on_activate(state); - allocate_buffers(); - start_packet_processing_thread(); - start_sensor_connection_thread(); +OusterSensor::OusterSensor(const std::string& name, const rclcpp::NodeOptions& options) + : OusterSensorNodeBase(name, options), + change_state_client{create_client(get_name() + "/change_state"s)} { + declare_parameters(); +} + +OusterSensor::OusterSensor(const rclcpp::NodeOptions& options) + : OusterSensor("os_sensor", options) { + declare_parameters(); +} + +void OusterSensor::declare_parameters() { + declare_parameter("sensor_hostname"); + declare_parameter("metadata", ""); + declare_parameter("udp_dest", ""); + declare_parameter("mtp_dest", ""); + declare_parameter("mtp_main", false); + declare_parameter("lidar_port", 0); + declare_parameter("imu_port", 0); + declare_parameter("lidar_mode", ""); + declare_parameter("timestamp_mode", ""); + declare_parameter("udp_profile_lidar", ""); +} + +LifecycleNodeInterface::CallbackReturn OusterSensor::on_configure( + const rclcpp_lifecycle::State&) { + RCLCPP_DEBUG(get_logger(), "on_configure() is called."); + + try { + sensor_hostname = get_sensor_hostname(); + auto config = staged_config.empty() + ? parse_config_from_ros_parameters() + : parse_config_from_staged_config_string(); + configure_sensor(sensor_hostname, config); + sensor_client = create_sensor_client(sensor_hostname, config); + if (!sensor_client) + return LifecycleNodeInterface::CallbackReturn::FAILURE; + create_metadata_publisher(); + update_config_and_metadata(*sensor_client); + create_services(); + create_publishers(); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM( + get_logger(), + "exception thrown while configuring the sensor, details: " + << ex.what()); + // TODO: return ERROR on fatal errors, FAILURE otherwise + return LifecycleNodeInterface::CallbackReturn::ERROR; + } + + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +LifecycleNodeInterface::CallbackReturn OusterSensor::on_activate( + const rclcpp_lifecycle::State& state) { + RCLCPP_DEBUG(get_logger(), "on_activate() is called."); + LifecycleNode::on_activate(state); + allocate_buffers(); + start_packet_processing_thread(); + start_sensor_connection_thread(); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +LifecycleNodeInterface::CallbackReturn OusterSensor::on_error( + const rclcpp_lifecycle::State&) { + RCLCPP_DEBUG(get_logger(), "on_error() is called."); + // Always return failure for now + return LifecycleNodeInterface::CallbackReturn::FAILURE; +} + +LifecycleNodeInterface::CallbackReturn OusterSensor::on_deactivate( + const rclcpp_lifecycle::State& state) { + RCLCPP_DEBUG(get_logger(), "on_deactivate() is called."); + LifecycleNode::on_deactivate(state); + stop_sensor_connection_thread(); + stop_packet_processing_thread(); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +LifecycleNodeInterface::CallbackReturn OusterSensor::on_cleanup( + const rclcpp_lifecycle::State&) { + RCLCPP_DEBUG(get_logger(), "on_cleanup() is called."); + + try { + cleanup(); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM( + get_logger(), + "exception thrown durng cleanup, details: " << ex.what()); + return LifecycleNodeInterface::CallbackReturn::ERROR; + } + + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +LifecycleNodeInterface::CallbackReturn OusterSensor::on_shutdown( + const rclcpp_lifecycle::State& state) { + RCLCPP_DEBUG(get_logger(), "on_shutdown() is called."); + + if (state.label() == "unconfigured") { + // nothing to do, return success return LifecycleNodeInterface::CallbackReturn::SUCCESS; } - LifecycleNodeInterface::CallbackReturn on_error( - const rclcpp_lifecycle::State&) { - RCLCPP_DEBUG(get_logger(), "on_error() is called."); - // Always return failure for now - return LifecycleNodeInterface::CallbackReturn::FAILURE; - } - - LifecycleNodeInterface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State& state) { - RCLCPP_DEBUG(get_logger(), "on_deactivate() is called."); - LifecycleNode::on_deactivate(state); + if (state.label() == "active") { stop_sensor_connection_thread(); stop_packet_processing_thread(); - return LifecycleNodeInterface::CallbackReturn::SUCCESS; } - LifecycleNodeInterface::CallbackReturn on_cleanup( - const rclcpp_lifecycle::State&) { - RCLCPP_DEBUG(get_logger(), "on_cleanup() is called."); - - try { - cleanup(); - } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM( - get_logger(), - "exception thrown durng cleanup, details: " << ex.what()); - return LifecycleNodeInterface::CallbackReturn::ERROR; - } - - return LifecycleNodeInterface::CallbackReturn::SUCCESS; + // whether state was 'active' or 'inactive' do cleanup + try { + cleanup(); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM( + get_logger(), + "exception thrown durng cleanup, details: " << ex.what()); + return LifecycleNodeInterface::CallbackReturn::ERROR; } - LifecycleNodeInterface::CallbackReturn on_shutdown( - const rclcpp_lifecycle::State& state) { - RCLCPP_DEBUG(get_logger(), "on_shutdown() is called."); + change_state_client.reset(); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} - if (state.label() == "unconfigured") { - // nothing to do, return success - return LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - - if (state.label() == "active") { - stop_sensor_connection_thread(); - stop_packet_processing_thread(); - } - - // whether state was 'active' or 'inactive' do cleanup - try { - cleanup(); - } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM( - get_logger(), - "exception thrown durng cleanup, details: " << ex.what()); - return LifecycleNodeInterface::CallbackReturn::ERROR; - } - - change_state_client.reset(); - return LifecycleNodeInterface::CallbackReturn::SUCCESS; +std::string OusterSensor::get_sensor_hostname() { + auto hostname = get_parameter("sensor_hostname").as_string(); + if (!is_arg_set(hostname)) { + auto error_msg = "Must specify a sensor hostname"; + RCLCPP_ERROR_STREAM(get_logger(), error_msg); + throw std::runtime_error(error_msg); } - private: - void declare_parameters() { - declare_parameter("sensor_hostname"); - declare_parameter("metadata", ""); - declare_parameter("udp_dest", ""); - declare_parameter("mtp_dest", ""); - declare_parameter("mtp_main", false); - declare_parameter("lidar_port", 0); - declare_parameter("imu_port", 0); - declare_parameter("lidar_mode", ""); - declare_parameter("timestamp_mode", ""); - declare_parameter("udp_profile_lidar", ""); - } - - std::string get_sensor_hostname() { - auto hostname = get_parameter("sensor_hostname").as_string(); - if (!is_arg_set(hostname)) { - auto error_msg = "Must specify a sensor hostname"; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); - throw std::runtime_error(error_msg); - } + return hostname; +} - return hostname; +void OusterSensor::update_config_and_metadata(sensor::client& cli) { + sensor::sensor_config config; + auto success = get_config(sensor_hostname, config); + if (!success) { + active_config.clear(); + cached_metadata.clear(); + auto error_msg = "Failed to collect sensor config"; + RCLCPP_ERROR_STREAM(get_logger(), error_msg); + throw std::runtime_error(error_msg); } - bool update_config_and_metadata(sensor::client& cli) { - sensor::sensor_config config; - auto success = get_config(sensor_hostname, config); - if (!success) { - RCLCPP_ERROR(get_logger(), "Failed to collect sensor config"); - active_config.clear(); - cached_metadata.clear(); - return false; - } - - active_config = sensor::to_string(config); + active_config = sensor::to_string(config); - try { - cached_metadata = sensor::get_metadata(cli, 60, false); - } catch (const std::exception& e) { - RCLCPP_ERROR_STREAM(get_logger(), - "sensor::get_metadata exception: " << e.what()); - cached_metadata.clear(); - } - - if (cached_metadata.empty()) { - RCLCPP_ERROR(get_logger(), "Failed to collect sensor metadata"); - return false; - } - - info = sensor::parse_metadata(cached_metadata); - // TODO: revist when *min_version* is changed - populate_metadata_defaults(info, sensor::MODE_UNSPEC); - display_lidar_info(info); - - return active_config.size() > 0 && cached_metadata.size() > 0; + try { + cached_metadata = sensor::get_metadata(cli, 60, false); + } catch (const std::exception& e) { + RCLCPP_ERROR_STREAM(get_logger(), + "sensor::get_metadata exception: " << e.what()); + cached_metadata.clear(); } - void save_metadata() { - auto meta_file = get_parameter("metadata").as_string(); - if (!is_arg_set(meta_file)) { - meta_file = sensor_hostname.substr(0, sensor_hostname.rfind('.')) + - "-metadata.json"; - RCLCPP_INFO_STREAM( - get_logger(), - "No metadata file was specified, using: " << meta_file); - } - - // write metadata file. If metadata_path is relative, will use cwd - // (usually ~/.ros) - if (!write_metadata(meta_file, cached_metadata)) { - RCLCPP_ERROR(get_logger(), - "Exiting because of failure to write metadata path"); - throw std::runtime_error("Failure to write metadata path"); - } + if (cached_metadata.empty()) { + RCLCPP_ERROR(get_logger(), "Failed to collect sensor metadata"); + auto error_msg = "Failed to collect sensor metadata"; + RCLCPP_ERROR_STREAM(get_logger(), error_msg); + throw std::runtime_error(error_msg); } - template - bool change_state(std::uint8_t transition_id, CallbackT callback, - CallbackT_Args... callback_args, - std::chrono::seconds time_out = 3s) { - if (!change_state_client->wait_for_service(time_out)) { - RCLCPP_ERROR_STREAM(get_logger(), - "Service " - << change_state_client->get_service_name() - << "is not available."); - return false; - } + info = sensor::parse_metadata(cached_metadata); + // TODO: revist when *min_version* is changed + populate_metadata_defaults(info, sensor::MODE_UNSPEC); - auto request = std::make_shared(); - request->transition.id = transition_id; - // send an async request to perform the transition - change_state_client->async_send_request( - request, [callback, callback_args...]( - rclcpp::Client::SharedFuture) { - callback(callback_args...); - }); - return true; - } - - std::string transition_id_to_string(uint8_t transition_id) { - switch (transition_id) { - case lifecycle_msgs::msg::Transition::TRANSITION_CREATE: - return "create"s; - case lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE: - return "configure"s; - case lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP: - return "cleanup"s; - case lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE: - return "activate"; - case lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE: - return "deactivate"s; - case lifecycle_msgs::msg::Transition::TRANSITION_DESTROY: - return "destroy"s; - default: - return "unknown"s; - } + publish_metadata(); + save_metadata(); + on_metadata_updated(info); +} + +void OusterSensor::save_metadata() { + auto meta_file = get_parameter("metadata").as_string(); + if (!is_arg_set(meta_file)) { + meta_file = sensor_hostname.substr(0, sensor_hostname.rfind('.')) + + "-metadata.json"; + RCLCPP_INFO_STREAM( + get_logger(), + "No metadata file was specified, using: " << meta_file); } - void execute_transitions_sequence(std::vector transitions_sequence, - size_t at) { - assert(at < transitions_sequence.size() && - "at index exceeds the number of transitions"); - auto transition_id = transitions_sequence[at]; + // write metadata file. If metadata_path is relative, will use cwd + // (usually ~/.ros) + + if (!write_text_to_file(meta_file, cached_metadata)) { + RCLCPP_INFO_STREAM(get_logger(), + "Wrote sensor metadata to " << meta_file); + } else { + RCLCPP_WARN_STREAM( + get_logger(), + "Failed to write metadata to " << meta_file << + "; check that the path is valid. " + "If you provided a relative path, please note that the working " + "directory of all ROS nodes is set by default to $ROS_HOME"); + } +} + +std::string OusterSensor::transition_id_to_string(uint8_t transition_id) { + switch (transition_id) { + case lifecycle_msgs::msg::Transition::TRANSITION_CREATE: + return "create"s; + case lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE: + return "configure"s; + case lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP: + return "cleanup"s; + case lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE: + return "activate"; + case lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE: + return "deactivate"s; + case lifecycle_msgs::msg::Transition::TRANSITION_DESTROY: + return "destroy"s; + default: + return "unknown"s; + } +} + +template +bool OusterSensor::change_state(std::uint8_t transition_id, CallbackT callback, + CallbackT_Args... callback_args, + std::chrono::seconds time_out) { + if (!change_state_client->wait_for_service(time_out)) { + RCLCPP_ERROR_STREAM(get_logger(), + "Service " + << change_state_client->get_service_name() + << "is not available."); + return false; + } + + auto request = std::make_shared(); + request->transition.id = transition_id; + // send an async request to perform the transition + change_state_client->async_send_request( + request, [callback, callback_args...]( + rclcpp::Client::SharedFuture) { + callback(callback_args...); + }); + return true; +} + +void OusterSensor::execute_transitions_sequence( + std::vector transitions_sequence, size_t at) { + assert(at < transitions_sequence.size() && + "at index exceeds the number of transitions"); + auto transition_id = transitions_sequence[at]; + RCLCPP_DEBUG_STREAM(get_logger(), + "transition: [" + << transition_id_to_string(transition_id) + << "] started"); + change_state(transition_id, [this, transitions_sequence, at]() { RCLCPP_DEBUG_STREAM(get_logger(), - "transition: [" - << transition_id_to_string(transition_id) - << "] started"); - change_state(transition_id, [this, transitions_sequence, at]() { + "transition: [" << transition_id_to_string( + transitions_sequence[at]) + << "] completed"); + if (at < transitions_sequence.size() - 1) { + execute_transitions_sequence(transitions_sequence, at + 1); + } else { RCLCPP_DEBUG_STREAM(get_logger(), - "transition: [" << transition_id_to_string( - transitions_sequence[at]) - << "] completed"); - if (at < transitions_sequence.size() - 1) { - execute_transitions_sequence(transitions_sequence, at + 1); - } else { - RCLCPP_DEBUG_STREAM(get_logger(), - "transitions sequence completed"); - } + "transitions sequence completed"); + } + }); +} + +// param init_id_reset is overriden to true when force_reinit is true +void OusterSensor::reset_sensor(bool force_reinit, bool init_id_reset) { + if (!sensor_connection_active) { + RCLCPP_WARN(get_logger(), + "sensor reset is invoked but sensor connection is not " + "active, ignoring call!"); + return; + } + + force_sensor_reinit = force_reinit; + reset_last_init_id = force_reinit ? true : init_id_reset; + auto request_transitions = std::vector{ + lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, + lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE}; + execute_transitions_sequence(request_transitions, 0); +} + +// TODO: need to notify dependent node(s) of the update +void OusterSensor::reactivate_sensor(bool init_id_reset) { + if (!sensor_connection_active) { + // This may indicate that we are in the process of re-activation + RCLCPP_WARN(get_logger(), + "sensor reactivate is invoked but sensor connection is " + "not active, ignoring call!"); + return; + } + + reset_last_init_id = init_id_reset; + update_config_and_metadata(*sensor_client); + auto request_transitions = std::vector{ + lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE}; + execute_transitions_sequence(request_transitions, 0); +} + +void OusterSensor::create_reset_service() { + reset_srv = create_service( + "reset", + [this](const std::shared_ptr, + std::shared_ptr) { + RCLCPP_INFO(get_logger(), "reset service invoked"); + reset_sensor(true); }); - } - // param init_id_reset is overriden to true when force_reinit is true - void reset_sensor(bool force_reinit, bool init_id_reset = false) { - if (!sensor_connection_active) { - RCLCPP_WARN(get_logger(), - "sensor reset is invoked but sensor connection is not " - "active, ignoring call!"); - return; - } + RCLCPP_INFO(get_logger(), "reset service created"); +} - force_sensor_reinit = force_reinit; - reset_last_init_id = force_reinit ? true : init_id_reset; - auto request_transitions = std::vector{ - lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, - lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, - lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, - lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE}; - execute_transitions_sequence(request_transitions, 0); - } - - // TODO: need to notify dependent node(s) of the update - void reactivate_sensor(bool init_id_reset = false) { - if (!sensor_connection_active) { - // This may indicate that we are in the process of re-activation - RCLCPP_WARN(get_logger(), - "sensor reactivate is invoked but sensor connection is " - "not active, ignoring call!"); - return; - } +void OusterSensor::create_get_config_service() { + get_config_srv = create_service( + "get_config", + [this](const std::shared_ptr, + std::shared_ptr response) { + response->config = active_config; + return active_config.size() > 0; + }); - reset_last_init_id = init_id_reset; - update_config_and_metadata(*sensor_client); - publish_metadata(); - save_metadata(); - auto request_transitions = std::vector{ - lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, - lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE}; - execute_transitions_sequence(request_transitions, 0); - } - - void create_reset_service() { - reset_srv = create_service( - "reset", - [this](const std::shared_ptr, - std::shared_ptr) { - RCLCPP_INFO(get_logger(), "reset service invoked"); - reset_sensor(true); - }); - - RCLCPP_INFO(get_logger(), "reset service created"); - } - - void create_get_config_service() { - get_config_srv = create_service( - "get_config", - [this](const std::shared_ptr, - std::shared_ptr response) { - response->config = active_config; - return active_config.size() > 0; - }); - - RCLCPP_INFO(get_logger(), "get_config service created"); - } - - void create_set_config_service() { - set_config_srv = create_service( - "set_config", - [this](const std::shared_ptr request, - std::shared_ptr response) { - response->config = ""; - - try { - staged_config = load_config_file(request->config_file); - if (staged_config.empty()) { - RCLCPP_ERROR_STREAM( - get_logger(), - "provided config file: " - << request->config_file - << " turned to be empty. set_config ignored!"); - return false; - } - } catch (const std::exception& e) { + RCLCPP_INFO(get_logger(), "get_config service created"); +} + +void OusterSensor::create_set_config_service() { + set_config_srv = create_service( + "set_config", + [this](const std::shared_ptr request, + std::shared_ptr response) { + response->config = ""; + + try { + staged_config = read_text_file(request->config_file); + if (staged_config.empty()) { RCLCPP_ERROR_STREAM( get_logger(), - "exception thrown while loading config file: " + "provided config file: " << request->config_file - << ", exception details: " << e.what()); + << " turned to be empty. set_config ignored!"); return false; } + } catch (const std::exception& e) { + RCLCPP_ERROR_STREAM( + get_logger(), + "exception thrown while loading config file: " + << request->config_file + << ", exception details: " << e.what()); + return false; + } - response->config = staged_config; - // TODO: this is currently set to force_reinit but it doesn't - // need to be the case if it was possible to know that the new - // config would result in a reinit when a reinit is not forced - reset_sensor(true); - return true; - }); - - RCLCPP_INFO(get_logger(), "set_config service created"); - } - - std::shared_ptr create_sensor_client( - const std::string& hostname, const sensor::sensor_config& config) { - RCLCPP_INFO_STREAM(get_logger(), "Starting sensor " - << hostname - << " initialization..."); + response->config = staged_config; + // TODO: this is currently set to force_reinit but it doesn't + // need to be the case if it was possible to know that the new + // config would result in a reinit when a reinit is not forced + reset_sensor(true); + return true; + }); - int lidar_port = - config.udp_port_lidar ? config.udp_port_lidar.value() : 0; - int imu_port = config.udp_port_imu ? config.udp_port_imu.value() : 0; + RCLCPP_INFO(get_logger(), "set_config service created"); +} + +std::shared_ptr OusterSensor::create_sensor_client( + const std::string& hostname, const sensor::sensor_config& config) { + RCLCPP_INFO_STREAM(get_logger(), "Starting sensor " + << hostname + << " initialization..."); + + int lidar_port = + config.udp_port_lidar ? config.udp_port_lidar.value() : 0; + int imu_port = config.udp_port_imu ? config.udp_port_imu.value() : 0; + auto udp_dest = config.udp_dest ? config.udp_dest.value() : ""; + + std::shared_ptr cli; + if (sensor::in_multicast(udp_dest)) { + // use the mtp_init_client to recieve data via multicast + // if mtp_main is true when sensor will be configured + cli = sensor::mtp_init_client(hostname, config, mtp_dest, mtp_main); + } else if (lidar_port != 0 && imu_port != 0) { + // use no-config version of init_client to bind to pre-configured + // ports + cli = sensor::init_client(hostname, lidar_port, imu_port); + } else { + // use the full init_client to generate and assign random ports to + // sensor auto udp_dest = config.udp_dest ? config.udp_dest.value() : ""; - - std::shared_ptr cli; - if (sensor::in_multicast(udp_dest)) { - // use the mtp_init_client to recieve data via multicast - // if mtp_main is true when sensor will be configured - cli = sensor::mtp_init_client(hostname, config, mtp_dest, mtp_main); - } else if (lidar_port != 0 && imu_port != 0) { - // use no-config version of init_client to bind to pre-configured - // ports - cli = sensor::init_client(hostname, lidar_port, imu_port); - } else { - // use the full init_client to generate and assign random ports to - // sensor - auto udp_dest = config.udp_dest ? config.udp_dest.value() : ""; - cli = sensor::init_client(hostname, udp_dest, sensor::MODE_UNSPEC, - sensor::TIME_FROM_UNSPEC, lidar_port, - imu_port); - } - - if (!cli) { - auto error_msg = "Failed to initialize client"; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); - throw std::runtime_error(error_msg); - } - - return cli; - } - - sensor::sensor_config parse_config_from_ros_parameters() { - auto udp_dest = get_parameter("udp_dest").as_string(); - auto mtp_dest_arg = get_parameter("mtp_dest").as_string(); - auto mtp_main_arg = get_parameter("mtp_main").as_bool(); - auto lidar_port = get_parameter("lidar_port").as_int(); - auto imu_port = get_parameter("imu_port").as_int(); - auto lidar_mode_arg = get_parameter("lidar_mode").as_string(); - auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); - auto udp_profile_lidar_arg = - get_parameter("udp_profile_lidar").as_string(); - - if (lidar_port < 0 || lidar_port > 65535) { + cli = sensor::init_client(hostname, udp_dest, sensor::MODE_UNSPEC, + sensor::TIME_FROM_UNSPEC, lidar_port, + imu_port); + } + + if (!cli) { + auto error_msg = "Failed to initialize client"; + RCLCPP_ERROR_STREAM(get_logger(), error_msg); + throw std::runtime_error(error_msg); + } + + return cli; +} + +sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { + auto udp_dest = get_parameter("udp_dest").as_string(); + auto mtp_dest_arg = get_parameter("mtp_dest").as_string(); + auto mtp_main_arg = get_parameter("mtp_main").as_bool(); + auto lidar_port = get_parameter("lidar_port").as_int(); + auto imu_port = get_parameter("imu_port").as_int(); + auto lidar_mode_arg = get_parameter("lidar_mode").as_string(); + auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); + auto udp_profile_lidar_arg = + get_parameter("udp_profile_lidar").as_string(); + + if (lidar_port < 0 || lidar_port > 65535) { + auto error_msg = + "Invalid lidar port number! port value should be in the range " + "[0, 65535]."; + RCLCPP_ERROR_STREAM(get_logger(), error_msg); + throw std::runtime_error(error_msg); + } + + if (imu_port < 0 || imu_port > 65535) { + auto error_msg = + "Invalid imu port number! port value should be in the range " + "[0, 65535]."; + RCLCPP_ERROR_STREAM(get_logger(), error_msg); + throw std::runtime_error(error_msg); + } + + nonstd::optional udp_profile_lidar; + if (is_arg_set(udp_profile_lidar_arg)) { + // set lidar profile from param + udp_profile_lidar = + sensor::udp_profile_lidar_of_string(udp_profile_lidar_arg); + if (!udp_profile_lidar) { auto error_msg = - "Invalid lidar port number! port value should be in the range " - "[0, 65535]."; + "Invalid udp profile lidar: " + udp_profile_lidar_arg; RCLCPP_ERROR_STREAM(get_logger(), error_msg); throw std::runtime_error(error_msg); } + } - if (imu_port < 0 || imu_port > 65535) { - auto error_msg = - "Invalid imu port number! port value should be in the range " - "[0, 65535]."; + // set lidar mode from param + sensor::lidar_mode lidar_mode = sensor::MODE_UNSPEC; + if (is_arg_set(lidar_mode_arg)) { + lidar_mode = sensor::lidar_mode_of_string(lidar_mode_arg); + if (!lidar_mode) { + auto error_msg = "Invalid lidar mode: " + lidar_mode_arg; RCLCPP_ERROR_STREAM(get_logger(), error_msg); throw std::runtime_error(error_msg); } + } - nonstd::optional udp_profile_lidar; - if (is_arg_set(udp_profile_lidar_arg)) { - // set lidar profile from param - udp_profile_lidar = - sensor::udp_profile_lidar_of_string(udp_profile_lidar_arg); - if (!udp_profile_lidar) { + // set timestamp mode from param + sensor::timestamp_mode timestamp_mode = sensor::TIME_FROM_UNSPEC; + if (is_arg_set(timestamp_mode_arg)) { + // In case the option TIME_FROM_ROS_TIME is set then leave the + // sensor timestamp_mode unmodified + if (timestamp_mode_arg == "TIME_FROM_ROS_TIME") { + RCLCPP_INFO(get_logger(), + "TIME_FROM_ROS_TIME timestamp mode specified." + " IMU and pointcloud messages will use ros time"); + } else { + timestamp_mode = + sensor::timestamp_mode_of_string(timestamp_mode_arg); + if (!timestamp_mode) { auto error_msg = - "Invalid udp profile lidar: " + udp_profile_lidar_arg; + "Invalid timestamp mode: " + timestamp_mode_arg; RCLCPP_ERROR_STREAM(get_logger(), error_msg); throw std::runtime_error(error_msg); } } - - // set lidar mode from param - sensor::lidar_mode lidar_mode = sensor::MODE_UNSPEC; - if (is_arg_set(lidar_mode_arg)) { - lidar_mode = sensor::lidar_mode_of_string(lidar_mode_arg); - if (!lidar_mode) { - auto error_msg = "Invalid lidar mode: " + lidar_mode_arg; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); - throw std::runtime_error(error_msg); - } - } - - // set timestamp mode from param - sensor::timestamp_mode timestamp_mode = sensor::TIME_FROM_UNSPEC; - if (is_arg_set(timestamp_mode_arg)) { - // In case the option TIME_FROM_ROS_TIME is set then leave the - // sensor timestamp_mode unmodified - if (timestamp_mode_arg == "TIME_FROM_ROS_TIME") { - RCLCPP_INFO(get_logger(), - "TIME_FROM_ROS_TIME timestamp mode specified." - " IMU and pointcloud messages will use ros time"); - } else { - timestamp_mode = - sensor::timestamp_mode_of_string(timestamp_mode_arg); - if (!timestamp_mode) { - auto error_msg = - "Invalid timestamp mode: " + timestamp_mode_arg; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); - throw std::runtime_error(error_msg); - } - } - } - - sensor::sensor_config config; - if (lidar_port == 0) { - RCLCPP_WARN_EXPRESSION( - get_logger(), !is_arg_set(mtp_dest_arg), - "lidar port set to zero, the client will assign a random port " - "number!"); - } else { - config.udp_port_lidar = lidar_port; - } - - if (imu_port == 0) { - RCLCPP_WARN_EXPRESSION( - get_logger(), !is_arg_set(mtp_dest_arg), - "imu port set to zero, the client will assign a random port " - "number!"); - } else { - config.udp_port_imu = imu_port; - } - - config.udp_profile_lidar = udp_profile_lidar; - config.operating_mode = sensor::OPERATING_NORMAL; - if (lidar_mode) config.ld_mode = lidar_mode; - if (timestamp_mode) config.ts_mode = timestamp_mode; - if (is_arg_set(udp_dest)) { - config.udp_dest = udp_dest; - if (sensor::in_multicast(udp_dest)) { - mtp_dest = - is_arg_set(mtp_dest_arg) ? mtp_dest_arg : std::string{}; - mtp_main = mtp_main_arg; - } - } - - return config; } - sensor::sensor_config parse_config_from_staged_config_string() { - sensor::sensor_config config = sensor::parse_config(staged_config); - staged_config.clear(); - return config; + sensor::sensor_config config; + if (lidar_port == 0) { + RCLCPP_WARN_EXPRESSION( + get_logger(), !is_arg_set(mtp_dest_arg), + "lidar port set to zero, the client will assign a random port " + "number!"); + } else { + config.udp_port_lidar = lidar_port; } - uint8_t compose_config_flags(const sensor::sensor_config& config) { - uint8_t config_flags = 0; - if (config.udp_dest) { - RCLCPP_INFO_STREAM(get_logger(), "Will send UDP data to " - << config.udp_dest.value()); - // TODO: revise multicast setup inference - if (sensor::in_multicast(*config.udp_dest)) { - if (is_arg_set(mtp_dest)) { - RCLCPP_INFO_STREAM( - get_logger(), - "Will recieve data via multicast on " << mtp_dest); - } else { - RCLCPP_INFO( - get_logger(), - "mtp_dest was not set, will recieve data via multicast " - "on first available interface"); - } - } - } else { - RCLCPP_INFO(get_logger(), "Will use automatic UDP destination"); - config_flags |= ouster::sensor::CONFIG_UDP_DEST_AUTO; - } - - if (force_sensor_reinit) { - force_sensor_reinit = false; - RCLCPP_INFO(get_logger(), "Forcing sensor to reinitialize"); - config_flags |= ouster::sensor::CONFIG_FORCE_REINIT; - } - - return config_flags; + if (imu_port == 0) { + RCLCPP_WARN_EXPRESSION( + get_logger(), !is_arg_set(mtp_dest_arg), + "imu port set to zero, the client will assign a random port " + "number!"); + } else { + config.udp_port_imu = imu_port; } - void configure_sensor(const std::string& hostname, - sensor::sensor_config& config) { - if (config.udp_dest && sensor::in_multicast(config.udp_dest.value()) && - !mtp_main) { - if (!get_config(hostname, config, true)) { - RCLCPP_ERROR(get_logger(), "Error getting active config"); + config.udp_profile_lidar = udp_profile_lidar; + config.operating_mode = sensor::OPERATING_NORMAL; + if (lidar_mode) config.ld_mode = lidar_mode; + if (timestamp_mode) config.ts_mode = timestamp_mode; + if (is_arg_set(udp_dest)) { + config.udp_dest = udp_dest; + if (sensor::in_multicast(udp_dest)) { + mtp_dest = + is_arg_set(mtp_dest_arg) ? mtp_dest_arg : std::string{}; + mtp_main = mtp_main_arg; + } + } + + return config; +} + +sensor::sensor_config OusterSensor::parse_config_from_staged_config_string() { + sensor::sensor_config config = sensor::parse_config(staged_config); + staged_config.clear(); + return config; +} + +uint8_t OusterSensor::compose_config_flags(const sensor::sensor_config& config) { + uint8_t config_flags = 0; + if (config.udp_dest) { + RCLCPP_INFO_STREAM(get_logger(), "Will send UDP data to " + << config.udp_dest.value()); + // TODO: revise multicast setup inference + if (sensor::in_multicast(*config.udp_dest)) { + if (is_arg_set(mtp_dest)) { + RCLCPP_INFO_STREAM( + get_logger(), + "Will recieve data via multicast on " << mtp_dest); } else { - RCLCPP_INFO(get_logger(), "Retrived active config of sensor"); + RCLCPP_INFO( + get_logger(), + "mtp_dest was not set, will recieve data via multicast " + "on first available interface"); } - return; - } - - uint8_t config_flags = compose_config_flags(config); - if (!set_config(hostname, config, config_flags)) { - throw std::runtime_error("Error connecting to sensor " + hostname); - } - - RCLCPP_INFO_STREAM(get_logger(), - "Sensor " << hostname << " configured successfully"); - } - - std::string load_config_file(const std::string& config_file) { - std::ifstream ifs{}; - ifs.open(config_file); - if (ifs.fail()) return {}; - std::stringstream buf; - buf << ifs.rdbuf(); - return buf.str(); - } - - // fill in values that could not be parsed from metadata - void populate_metadata_defaults(sensor::sensor_info& info, - sensor::lidar_mode specified_lidar_mode) { - if (!info.name.size()) info.name = "UNKNOWN"; - - if (!info.sn.size()) info.sn = "UNKNOWN"; - - ouster::util::version v = ouster::util::version_of_string(info.fw_rev); - if (v == ouster::util::invalid_version) - RCLCPP_WARN( - get_logger(), - "Unknown sensor firmware version; output may not be reliable"); - else if (v < sensor::min_version) - RCLCPP_WARN( - get_logger(), - "Firmware < %s not supported; output may not be reliable", - to_string(sensor::min_version).c_str()); - - if (!info.mode) { - RCLCPP_WARN( - get_logger(), - "Lidar mode not found in metadata; output may not be reliable"); - info.mode = specified_lidar_mode; - } - - if (!info.prod_line.size()) info.prod_line = "UNKNOWN"; - - if (info.beam_azimuth_angles.empty() || - info.beam_altitude_angles.empty()) { - RCLCPP_ERROR( - get_logger(), - "Beam angles not found in metadata; using design values"); - info.beam_azimuth_angles = sensor::gen1_azimuth_angles; - info.beam_altitude_angles = sensor::gen1_altitude_angles; } + } else { + RCLCPP_INFO(get_logger(), "Will use automatic UDP destination"); + config_flags |= ouster::sensor::CONFIG_UDP_DEST_AUTO; } - // try to write metadata file - bool write_metadata(const std::string& meta_file, - const std::string& metadata) { - std::ofstream ofs(meta_file); - if (ofs.is_open()) { - ofs << metadata << std::endl; - ofs.close(); - RCLCPP_INFO_STREAM(get_logger(), - "Wrote sensor metadata to " << meta_file); - } else { - RCLCPP_WARN( - get_logger(), - "Failed to write metadata to %s; check that the path is valid. " - "If you provided a relative path, please note that the working " - "directory of all ROS nodes is set by default to $ROS_HOME", - meta_file.c_str()); - return false; - } - return true; + if (force_sensor_reinit) { + force_sensor_reinit = false; + RCLCPP_INFO(get_logger(), "Forcing sensor to reinitialize"); + config_flags |= ouster::sensor::CONFIG_FORCE_REINIT; } - void create_publishers() { - rclcpp::SensorDataQoS qos; - lidar_packet_pub = create_publisher("lidar_packets", qos); - imu_packet_pub = create_publisher("imu_packets", qos); - } + return config_flags; +} - void allocate_buffers() { - auto& pf = sensor::get_format(info); - lidar_packet.buf.resize(pf.lidar_packet_size + 1); - imu_packet.buf.resize(pf.imu_packet_size + 1); - } - - bool init_id_changed(const sensor::packet_format& pf, - const uint8_t* lidar_buf) { - uint32_t current_init_id = pf.init_id(lidar_buf); - if (!last_init_id_initialized) { - last_init_id = current_init_id + 1; - last_init_id_initialized = true; - } - if (reset_last_init_id && last_init_id != current_init_id) { - last_init_id = current_init_id; - reset_last_init_id = false; - return false; - } - if (last_init_id == current_init_id) return false; +void OusterSensor::configure_sensor(const std::string& hostname, + sensor::sensor_config& config) { + if (config.udp_dest && sensor::in_multicast(config.udp_dest.value()) && + !mtp_main) { + if (!get_config(hostname, config, true)) { + RCLCPP_ERROR(get_logger(), "Error getting active config"); + } else { + RCLCPP_INFO(get_logger(), "Retrived active config of sensor"); + } + return; + } + + uint8_t config_flags = compose_config_flags(config); + if (!set_config(hostname, config, config_flags)) { + throw std::runtime_error("Error connecting to sensor " + hostname); + } + + RCLCPP_INFO_STREAM(get_logger(), + "Sensor " << hostname << " configured successfully"); +} + +// fill in values that could not be parsed from metadata +void OusterSensor::populate_metadata_defaults(sensor::sensor_info& info, + sensor::lidar_mode specified_lidar_mode) { + if (!info.name.size()) info.name = "UNKNOWN"; + if (!info.sn.size()) info.sn = "UNKNOWN"; + + ouster::util::version v = ouster::util::version_of_string(info.fw_rev); + if (v == ouster::util::invalid_version) + RCLCPP_WARN( + get_logger(), + "Unknown sensor firmware version; output may not be reliable"); + else if (v < sensor::min_version) + RCLCPP_WARN( + get_logger(), + "Firmware < %s not supported; output may not be reliable", + to_string(sensor::min_version).c_str()); + + if (!info.mode) { + RCLCPP_WARN( + get_logger(), + "Lidar mode not found in metadata; output may not be reliable"); + info.mode = specified_lidar_mode; + } + + if (!info.prod_line.size()) info.prod_line = "UNKNOWN"; + + if (info.beam_azimuth_angles.empty() || + info.beam_altitude_angles.empty()) { + RCLCPP_ERROR( + get_logger(), + "Beam angles not found in metadata; using design values"); + info.beam_azimuth_angles = sensor::gen1_azimuth_angles; + info.beam_altitude_angles = sensor::gen1_altitude_angles; + } +} + +void OusterSensor::on_metadata_updated(const sensor::sensor_info& info) { + display_lidar_info(info); +} + + +void OusterSensor::create_services() { + create_reset_service(); + create_get_metadata_service(); + create_get_config_service(); + create_set_config_service(); +} + +void OusterSensor::create_publishers() { + rclcpp::SensorDataQoS qos; + lidar_packet_pub = create_publisher("lidar_packets", qos); + imu_packet_pub = create_publisher("imu_packets", qos); +} + +void OusterSensor::allocate_buffers() { + auto& pf = sensor::get_format(info); + lidar_packet.buf.resize(pf.lidar_packet_size + 1); + imu_packet.buf.resize(pf.imu_packet_size + 1); +} + +bool OusterSensor::init_id_changed(const sensor::packet_format& pf, + const uint8_t* lidar_buf) { + uint32_t current_init_id = pf.init_id(lidar_buf); + if (!last_init_id_initialized) { + last_init_id = current_init_id + 1; + last_init_id_initialized = true; + } + if (reset_last_init_id && last_init_id != current_init_id) { last_init_id = current_init_id; - return true; - } - - static bool is_non_legacy_lidar_profile(const sensor::sensor_info& info) { - return info.format.udp_profile_lidar != - UDPProfileLidar::PROFILE_LIDAR_LEGACY; - } - - void handle_poll_client_error() { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 100, - "sensor::poll_client()) returned error"); - // in case error continues for a while attempt to recover by - // performing sensor reset - if (++poll_client_error_count > max_poll_client_error_count) { - RCLCPP_ERROR_STREAM( - get_logger(), - "maximum number of allowed errors from " - "sensor::poll_client() reached, performing self reset..."); - poll_client_error_count = 0; - reset_sensor(true); - } - } - - void handle_lidar_packet(sensor::client& cli, - const sensor::packet_format& pf) { - std::unique_lock lock(mtx); - cv.wait(lock, [this]{ return lidar_data_processed; }); - lidar_data_processed = false; - bool success = sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf); - lidar_data_available_for_processing = true; - lock.unlock(); - cv.notify_all(); - - // if (success) { - // read_lidar_packet_errors = 0; - // if (is_non_legacy_lidar_profile(info) && - // init_id_changed(pf, lidar_packet.buf.data())) { - // // TODO: short circut reset if no breaking changes occured? - // RCLCPP_WARN(get_logger(), - // "sensor init_id has changed! reactivating.."); - // reactivate_sensor(); - // } else { - // lidar_packet_pub->publish(lidar_packet); - // } - // } else { - // if (++read_lidar_packet_errors > max_read_lidar_packet_errors) { - // RCLCPP_ERROR_STREAM( - // get_logger(), - // "maximum number of allowed errors from " - // "sensor::read_lidar_packet() reached, reactivating..."); - // read_lidar_packet_errors = 0; - // reactivate_sensor(true); - // } - // } - } - - void handle_imu_packet(sensor::client& cli, - const sensor::packet_format& pf) { - std::unique_lock lock(mtx); - cv.wait(lock, [this]{ return imu_data_processed; }); - imu_data_processed = false; - bool success = sensor::read_imu_packet(cli, imu_packet.buf.data(), pf); - imu_data_available_for_processing = true; - lock.unlock(); - cv.notify_all(); - - // if (success) { - // imu_packet_pub->publish(imu_packet); - // } else { - // if (++read_imu_packet_errors > max_read_imu_packet_errors) { - // RCLCPP_ERROR_STREAM( - // get_logger(), - // "maximum number of allowed errors from " - // "sensor::read_imu_packet() reached, reactivating..."); - // read_imu_packet_errors = 0; - // reactivate_sensor(true); - // } - // } - } - - void cleanup() { - sensor_client.reset(); - lidar_packet_pub.reset(); - imu_packet_pub.reset(); - get_metadata_srv.reset(); - get_config_srv.reset(); - set_config_srv.reset(); - sensor_connection_thread.reset(); - packet_processing_thread.reset(); - } - - void connection_loop(sensor::client& cli, const sensor::packet_format& pf) { - auto state = sensor::poll_client(cli); - if (state == sensor::EXIT) { - RCLCPP_INFO(get_logger(), "poll_client: caught signal, exiting!"); - return; - } - if (state & sensor::CLIENT_ERROR) { - handle_poll_client_error(); - return; - } + reset_last_init_id = false; + return false; + } + if (last_init_id == current_init_id) return false; + last_init_id = current_init_id; + return true; +} + +void OusterSensor::handle_poll_client_error() { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 100, + "sensor::poll_client()) returned error"); + // in case error continues for a while attempt to recover by + // performing sensor reset + if (++poll_client_error_count > max_poll_client_error_count) { + RCLCPP_ERROR_STREAM( + get_logger(), + "maximum number of allowed errors from " + "sensor::poll_client() reached, performing self reset..."); poll_client_error_count = 0; - if (state & sensor::LIDAR_DATA) { - handle_lidar_packet(cli, pf); - } - if (state & sensor::IMU_DATA) { - handle_imu_packet(cli, pf); + reset_sensor(true); + } +} + +void OusterSensor::handle_lidar_packet(sensor::client& cli, + const sensor::packet_format& pf) { + std::unique_lock lock(mtx); + receiving_cv.wait(lock, [this]{ return lidar_data_processed; }); + bool success = sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf); + lidar_data_processed = false; + lock.unlock(); + processing_cv.notify_all(); + + // if (success) { + // read_lidar_packet_errors = 0; + // if (!is_legacy_lidar_profile(info) && + // init_id_changed(pf, lidar_packet.buf.data())) { + // // TODO: short circut reset if no breaking changes occured? + // RCLCPP_WARN(get_logger(), + // "sensor init_id has changed! reactivating.."); + // reactivate_sensor(); + // } else { + // lidar_packet_pub->publish(lidar_packet); + // } + // } else { + // if (++read_lidar_packet_errors > max_read_lidar_packet_errors) { + // RCLCPP_ERROR_STREAM( + // get_logger(), + // "maximum number of allowed errors from " + // "sensor::read_lidar_packet() reached, reactivating..."); + // read_lidar_packet_errors = 0; + // reactivate_sensor(true); + // } + // } +} + +void OusterSensor::handle_imu_packet(sensor::client& cli, + const sensor::packet_format& pf) { + std::unique_lock lock(mtx); + receiving_cv.wait(lock, [this]{ return imu_data_processed; }); + bool success = sensor::read_imu_packet(cli, imu_packet.buf.data(), pf); + imu_data_processed = false; + lock.unlock(); + processing_cv.notify_one(); + + // if (success) { + // imu_packet_pub->publish(imu_packet); + // } else { + // if (++read_imu_packet_errors > max_read_imu_packet_errors) { + // RCLCPP_ERROR_STREAM( + // get_logger(), + // "maximum number of allowed errors from " + // "sensor::read_imu_packet() reached, reactivating..."); + // read_imu_packet_errors = 0; + // reactivate_sensor(true); + // } + // } +} + +void OusterSensor::cleanup() { + sensor_client.reset(); + lidar_packet_pub.reset(); + imu_packet_pub.reset(); + get_metadata_srv.reset(); + get_config_srv.reset(); + set_config_srv.reset(); + sensor_connection_thread.reset(); + packet_processing_thread.reset(); +} + +void OusterSensor::connection_loop(sensor::client& cli, const sensor::packet_format& pf) { + auto state = sensor::poll_client(cli); + if (state == sensor::EXIT) { + RCLCPP_INFO(get_logger(), "poll_client: caught signal, exiting!"); + return; + } + if (state & sensor::CLIENT_ERROR) { + handle_poll_client_error(); + return; + } + poll_client_error_count = 0; + if (state & sensor::LIDAR_DATA) { + handle_lidar_packet(cli, pf); + } + if (state & sensor::IMU_DATA) { + handle_imu_packet(cli, pf); + } +} + +void OusterSensor::start_sensor_connection_thread() { + sensor_connection_active = true; + sensor_connection_thread = std::make_unique([this]() { + auto& pf = sensor::get_format(info); + while (sensor_connection_active) { + connection_loop(*sensor_client, pf); } - } + RCLCPP_DEBUG(get_logger(), "sensor_connection_thread done."); + }); +} - void start_sensor_connection_thread() { - sensor_connection_active = true; - sensor_connection_thread = std::make_unique([this]() { - auto& pf = sensor::get_format(info); - while (sensor_connection_active) { - connection_loop(*sensor_client, pf); - } - RCLCPP_DEBUG(get_logger(), "sensor_connection_thread done."); - }); +void OusterSensor::stop_sensor_connection_thread() { + RCLCPP_DEBUG(get_logger(), "sensor_connection_thread stopping."); + if (sensor_connection_thread->joinable()) { + sensor_connection_active = false; + sensor_connection_thread->join(); } +} - void stop_sensor_connection_thread() { - RCLCPP_DEBUG(get_logger(), "sensor_connection_thread stopping."); - if (sensor_connection_thread->joinable()) { - sensor_connection_active = false; - sensor_connection_thread->join(); +void OusterSensor::start_packet_processing_thread() { + packet_processing_active = true; + packet_processing_thread = std::make_unique([this]() { + while (packet_processing_active) { + process_packets(); } - } + RCLCPP_DEBUG(get_logger(), "packet_processing_thread done."); + }); +} - void start_packet_processing_thread() { - packet_processing_active = true; - packet_processing_thread = std::make_unique([this]() { - while (packet_processing_active) { - process_packets(); - } - RCLCPP_DEBUG(get_logger(), "packet_processing_thread done."); - }); +void OusterSensor::stop_packet_processing_thread() { + RCLCPP_DEBUG(get_logger(), "packet_processing_thread stopping."); + if (packet_processing_thread->joinable()) { + packet_processing_active = false; + packet_processing_thread->join(); } +} - void stop_packet_processing_thread() { - RCLCPP_DEBUG(get_logger(), "packet_processing_thread stopping."); - if (packet_processing_thread->joinable()) { - packet_processing_active = false; - packet_processing_thread->join(); - } - } +void OusterSensor::on_lidar_packet_msg(const PacketMsg& lidar_packet) { + lidar_packet_pub->publish(lidar_packet); +} - void process_packets() { - while (packet_processing_active) { - std::unique_lock lock(mtx); +void OusterSensor::on_imu_packet_msg(const PacketMsg& imu_packet) { + imu_packet_pub->publish(imu_packet); +} - // Wait until there is a job in the queue - cv.wait(lock, [this]{ - return lidar_data_available_for_processing || imu_data_available_for_processing; }); - if (lidar_data_available_for_processing) { - lidar_data_available_for_processing = false; - lidar_packet_pub->publish(lidar_packet); - lidar_data_processed = true; - } +void OusterSensor::process_packets() { + while (packet_processing_active) { + std::unique_lock lock(mtx); - if (imu_data_available_for_processing) { - imu_data_available_for_processing = false; - imu_packet_pub->publish(imu_packet); - imu_data_processed = true; - } + // Wait until there is a job in the queue + processing_cv.wait(lock, [this]{ + return !lidar_data_processed || !imu_data_processed; }); - lock.unlock(); - cv.notify_all(); + if (!lidar_data_processed) { + on_lidar_packet_msg(lidar_packet); + lidar_data_processed = true; } - } - private: - std::string sensor_hostname; - std::string staged_config; - std::string active_config; - std::string mtp_dest; - bool mtp_main; - std::shared_ptr sensor_client; - PacketMsg lidar_packet; - PacketMsg imu_packet; - rclcpp::Publisher::SharedPtr lidar_packet_pub; - rclcpp::Publisher::SharedPtr imu_packet_pub; - rclcpp::Service::SharedPtr reset_srv; - rclcpp::Service::SharedPtr get_config_srv; - rclcpp::Service::SharedPtr set_config_srv; - std::shared_ptr> change_state_client; - - std::mutex mtx; - std::condition_variable cv; - bool lidar_data_available_for_processing = false; - bool lidar_data_processed = true; - - bool imu_data_available_for_processing = false; - bool imu_data_processed = true; - - std::atomic sensor_connection_active = {false}; - std::unique_ptr sensor_connection_thread; - - std::atomic packet_processing_active = {false}; - std::unique_ptr packet_processing_thread; - - bool force_sensor_reinit = false; - bool reset_last_init_id = true; - - bool last_init_id_initialized = false; - uint32_t last_init_id; - - // TODO: add as a ros parameter - const int max_poll_client_error_count = 10; - int poll_client_error_count = 0; - // TODO: add as a ros parameter - const int max_read_lidar_packet_errors = 60; - int read_lidar_packet_errors = 0; - // TODO: add as a ros parameter - const int max_read_imu_packet_errors = 60; - int read_imu_packet_errors = 0; -}; + if (!imu_data_processed) { + on_imu_packet_msg(imu_packet); + imu_data_processed = true; + } + + lock.unlock(); + receiving_cv.notify_one(); + } +} } // namespace ouster_ros diff --git a/ouster-ros/src/os_sensor_node.h b/ouster-ros/src/os_sensor_node.h new file mode 100644 index 00000000..9d1443eb --- /dev/null +++ b/ouster-ros/src/os_sensor_node.h @@ -0,0 +1,178 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file os_sensor_node.h + * @brief A node that connects to a live ouster sensor + */ + +#pragma once + +// prevent clang-format from altering the location of "ouster_ros/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" +// clang-format on + +#include +#include + +#include +#include +#include +#include "ouster_msgs/msg/packet_msg.hpp" +#include "ouster_srvs/srv/get_config.hpp" +#include "ouster_srvs/srv/set_config.hpp" +#include "ouster_ros/visibility_control.h" +#include "ouster_ros/os_sensor_node_base.h" + +namespace sensor = ouster::sensor; +using lifecycle_msgs::srv::ChangeState; +using ouster_srvs::srv::GetConfig; +using ouster_srvs::srv::SetConfig; +using rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface; + + +namespace ouster_ros { + +class OusterSensor : public OusterSensorNodeBase { +public: + OUSTER_ROS_PUBLIC + OusterSensor(const std::string& name, const rclcpp::NodeOptions& options); + explicit OusterSensor(const rclcpp::NodeOptions& options); + + LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State& state); + LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State& state); + LifecycleNodeInterface::CallbackReturn on_error(const rclcpp_lifecycle::State& state); + LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& state); + LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State&); + LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& state); + +protected: + virtual void on_metadata_updated(const sensor::sensor_info& info); + + virtual void create_services(); + + virtual void create_publishers(); + + virtual void on_lidar_packet_msg(const ouster_msgs::msg::PacketMsg& lidar_packet); + + virtual void on_imu_packet_msg(const ouster_msgs::msg::PacketMsg& imu_packet); + +private: + void declare_parameters(); + std::string get_sensor_hostname(); + void update_config_and_metadata(sensor::client& cli); + void save_metadata(); + + static std::string transition_id_to_string(uint8_t transition_id); + template + bool change_state(std::uint8_t transition_id, CallbackT callback, + CallbackT_Args... callback_args, + std::chrono::seconds time_out = std::chrono::seconds{3}); + void execute_transitions_sequence(std::vector transitions_sequence, + size_t at); + + + // param init_id_reset is overriden to true when force_reinit is true + void reset_sensor(bool force_reinit, bool init_id_reset = false); + + // TODO: need to notify dependent node(s) of the update + void reactivate_sensor(bool init_id_reset); + + void create_reset_service(); + + void create_get_config_service(); + + void create_set_config_service(); + + std::shared_ptr create_sensor_client( + const std::string& hostname, const sensor::sensor_config& config); + + sensor::sensor_config parse_config_from_ros_parameters(); + + sensor::sensor_config parse_config_from_staged_config_string(); + + uint8_t compose_config_flags(const sensor::sensor_config& config); + + void configure_sensor(const std::string& hostname, sensor::sensor_config& config); + + std::string load_config_file(const std::string& config_file); + + // fill in values that could not be parsed from metadata + void populate_metadata_defaults(sensor::sensor_info& info, + sensor::lidar_mode specified_lidar_mode); + + void allocate_buffers(); + + bool init_id_changed(const sensor::packet_format& pf, + const uint8_t* lidar_buf); + + void handle_poll_client_error(); + + void handle_lidar_packet(sensor::client& cli, + const sensor::packet_format& pf); + + void handle_imu_packet(sensor::client& cli, + const sensor::packet_format& pf); + + void cleanup(); + + void connection_loop(sensor::client& cli, const sensor::packet_format& pf); + + void start_sensor_connection_thread(); + + void stop_sensor_connection_thread(); + + void start_packet_processing_thread(); + + void stop_packet_processing_thread(); + + void process_packets(); + + private: + std::string sensor_hostname; + std::string staged_config; + std::string active_config; + std::string mtp_dest; + bool mtp_main; + std::shared_ptr sensor_client; + ouster_msgs::msg::PacketMsg lidar_packet; + ouster_msgs::msg::PacketMsg imu_packet; + rclcpp::Publisher::SharedPtr lidar_packet_pub; + rclcpp::Publisher::SharedPtr imu_packet_pub; + rclcpp::Service::SharedPtr reset_srv; + rclcpp::Service::SharedPtr get_config_srv; + rclcpp::Service::SharedPtr set_config_srv; + std::shared_ptr> change_state_client; + + std::mutex mtx; + std::condition_variable receiving_cv; + std::condition_variable processing_cv; + bool lidar_data_processed = true; + bool imu_data_processed = true; + + std::atomic sensor_connection_active = {false}; + std::unique_ptr sensor_connection_thread; + + std::atomic packet_processing_active = {false}; + std::unique_ptr packet_processing_thread; + + bool force_sensor_reinit = false; + bool reset_last_init_id = true; + + bool last_init_id_initialized = false; + uint32_t last_init_id; + + // TODO: add as a ros parameter + const int max_poll_client_error_count = 10; + int poll_client_error_count = 0; + // TODO: add as a ros parameter + const int max_read_lidar_packet_errors = 60; + int read_lidar_packet_errors = 0; + // TODO: add as a ros parameter + const int max_read_imu_packet_errors = 60; + int read_imu_packet_errors = 0; +}; + +} // namespace ouster_ros \ No newline at end of file diff --git a/ouster-ros/src/os_sensor_node_base.cpp b/ouster-ros/src/os_sensor_node_base.cpp index 729a28f4..3328cc7a 100644 --- a/ouster-ros/src/os_sensor_node_base.cpp +++ b/ouster-ros/src/os_sensor_node_base.cpp @@ -6,6 +6,8 @@ * @brief implementation of OusterSensorNodeBase interface */ +#include + #include "ouster_ros/os_sensor_node_base.h" #include @@ -48,10 +50,28 @@ void OusterSensorNodeBase::display_lidar_info(const sensor::sensor_info& info) { get_logger(), "ouster client version: " << ouster::SDK_VERSION_FULL << "\n" - << "product: " << info.prod_line << ", sn: " << info.sn - << ", firmware rev: " << info.fw_rev << "\n" + << "product: " << info.prod_line << ", sn: " << info.sn << ", " + << "firmware rev: " << info.fw_rev << "\n" << "lidar mode: " << sensor::to_string(info.mode) << ", " << "lidar udp profile: " << sensor::to_string(lidar_profile)); } +std::string OusterSensorNodeBase::read_text_file(const std::string& text_file) { + std::ifstream ifs{}; + ifs.open(text_file); + if (ifs.fail()) return {}; + std::stringstream buf; + buf << ifs.rdbuf(); + return buf.str(); +} + +bool OusterSensorNodeBase::write_text_to_file(const std::string& file_path, + const std::string& text) { + std::ofstream ofs(file_path); + if (!ofs.is_open()) return false; + ofs << text << std::endl; + ofs.close(); + return true; +} + } // namespace ouster_ros diff --git a/ouster-ros/src/thread_safe_ring_buffer.h b/ouster-ros/src/thread_safe_ring_buffer.h new file mode 100644 index 00000000..0377036b --- /dev/null +++ b/ouster-ros/src/thread_safe_ring_buffer.h @@ -0,0 +1,129 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file thread_safe_ring_buffer.h + * @brief File contains thread safe implementation of a ring buffer + */ + +#pragma once + +#include +#include +#include +#include + +/** + * @class ThreadSafeRingBuffer thread safe ring buffer. + */ +class ThreadSafeRingBuffer { +public: + ThreadSafeRingBuffer(size_t item_size_, size_t items_count_) : + buffer(item_size_ * items_count_), item_size(item_size_), + max_items_count(items_count_), active_items_count(0), + write_idx(0), read_idx(0) { + } + + /** + * Gets the maximum number of items that this ring buffer can hold. + */ + size_t capacity() const { return max_items_count; } + + /** + * Gets the number of item that currently occupy the ring buffer. + */ + size_t size() const { + std::lock_guard lock(mutex); + return active_items_count; + } + + /** + * Checks if the ring buffer is empty. + */ + bool empty() const { + std::lock_guard lock(mutex); + return active_items_count == 0; + } + + /** + * Checks if the ring buffer is full. + */ + bool full() const { + std::lock_guard lock(mutex); + return active_items_count == max_items_count; + } + + /** + * Writes to the buffer safely, the method will keep blocking until the there + * is a space available within the buffer. + */ + template + void write(BufferWriteT buffer_write) { + std::unique_lock lock(mutex); + fullCondition.wait(lock, [this] { return active_items_count < capacity(); }); + buffer_write(&buffer[write_idx * item_size]); + write_idx = (write_idx + 1) % capacity(); + ++active_items_count; + emptyCondition.notify_one(); + } + + + /** + * Writes to the buffer safely, if there is not space left then this method + * will overite the last item. + */ + template + void write_overwrite(BufferWriteT buffer_write) { + std::unique_lock lock(mutex); + buffer_write(&buffer[write_idx * item_size]); + write_idx = (write_idx + 1) % capacity(); + if (active_items_count < capacity()) { + ++active_items_count; + } else { + read_idx = (read_idx + 1) % capacity(); + } + emptyCondition.notify_one(); + } + + /** + * Gives access to read the buffer through a callback, the method will block + * until there is something to read is available. + */ + template + void read(BufferReadT&& buffer_read) { + std::unique_lock lock(mutex); + emptyCondition.wait(lock, [this] { return active_items_count > 0; }); + buffer_read(&buffer[read_idx * item_size]); + read_idx = (read_idx + 1) % capacity(); + --active_items_count; + fullCondition.notify_one(); + } + + /** + * Gives access to read the buffer through a callback, if buffer is + * inaccessible the method will timeout and buffer_read gets a nullptr. + */ + template + void read_timeout(BufferReadT buffer_read, std::chrono::seconds timeout) { + std::unique_lock lock(mutex); + if (emptyCondition.wait_for(lock, timeout, [this] { return active_items_count > 0; })) { + buffer_read(&buffer[read_idx * item_size]); + read_idx = (read_idx + 1) % capacity(); + --active_items_count; + fullCondition.notify_one(); + } else { + buffer_read((uint8_t*)nullptr); + } + } + +private: + std::vector buffer; + size_t item_size; + size_t max_items_count; + size_t active_items_count; + size_t write_idx; + size_t read_idx; + mutable std::mutex mutex; + std::condition_variable fullCondition; + std::condition_variable emptyCondition; +}; \ No newline at end of file From 4b08308eda1c3b5563887b767901c7936ff79d46 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 31 May 2023 10:11:22 -0700 Subject: [PATCH 03/38] Move down pragma once in the handlers --- ouster-ros/src/imu_packet_handler.h | 3 ++- ouster-ros/src/lidar_packet_handler.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/ouster-ros/src/imu_packet_handler.h b/ouster-ros/src/imu_packet_handler.h index c8189d62..8c0bd235 100644 --- a/ouster-ros/src/imu_packet_handler.h +++ b/ouster-ros/src/imu_packet_handler.h @@ -1,4 +1,3 @@ -#pragma once /** * Copyright (c) 2018-2023, Ouster, Inc. * All rights reserved. @@ -7,6 +6,8 @@ * @brief ... */ +#pragma once + // prevent clang-format from altering the location of "ouster_ros/ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h index 9dcf0c59..de0d80ac 100644 --- a/ouster-ros/src/lidar_packet_handler.h +++ b/ouster-ros/src/lidar_packet_handler.h @@ -1,4 +1,3 @@ -#pragma once /** * Copyright (c) 2018-2023, Ouster, Inc. * All rights reserved. @@ -7,6 +6,8 @@ * @brief ... */ +#pragma once + // prevent clang-format from altering the location of "ouster_ros/ros.h", the // header file needs to be the first include due to PCL_NO_PRECOMPILE flag // clang-format off From fa93e0ab827c74d74ad7fbf3e3e11156b695f04c Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 1 Jun 2023 14:36:18 -0700 Subject: [PATCH 04/38] Adding unit tests for the ThreadSafeRingBuffer --- Dockerfile | 4 + ouster-ros/CMakeLists.txt | 12 +++ ouster-ros/src/os_sensor_node.cpp | 1 - ouster-ros/test/ring_buffer_test.cpp | 138 +++++++++++++++++++++++++++ 4 files changed, 154 insertions(+), 1 deletion(-) create mode 100644 ouster-ros/test/ring_buffer_test.cpp diff --git a/Dockerfile b/Dockerfile index 1d5e5b38..5cfdb777 100644 --- a/Dockerfile +++ b/Dockerfile @@ -53,10 +53,14 @@ RUN set -xe \ FROM build-env SHELL ["/bin/bash", "-c"] + RUN source /opt/ros/$ROS_DISTRO/setup.bash && colcon build \ --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release \ -DCMAKE_CXX_FLAGS="-Wno-deprecated-declarations" +RUN source /opt/ros/$ROS_DISTRO/setup.bash && colcon test \ + --ctest-args tests ouster_ros --rerun-failed --output-on-failure + # Entrypoint for running Ouster ros: # # Usage: docker run --rm -it ouster-ros [sensor.launch parameters ..] diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt index 693ff1cc..d91ea205 100644 --- a/ouster-ros/CMakeLists.txt +++ b/ouster-ros/CMakeLists.txt @@ -166,6 +166,18 @@ rclcpp_components_register_node(os_driver_component EXECUTABLE os_driver ) + +# ==== Test ==== +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(${PROJECT_NAME}_test test/ring_buffer_test.cpp) + target_include_directories(${PROJECT_NAME}_test PUBLIC + $ + $ + ) +endif() + + # ==== Install ==== install( TARGETS diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index 647ed4bd..4270ddcf 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -32,7 +32,6 @@ OusterSensor::OusterSensor(const std::string& name, const rclcpp::NodeOptions& o OusterSensor::OusterSensor(const rclcpp::NodeOptions& options) : OusterSensor("os_sensor", options) { - declare_parameters(); } void OusterSensor::declare_parameters() { diff --git a/ouster-ros/test/ring_buffer_test.cpp b/ouster-ros/test/ring_buffer_test.cpp new file mode 100644 index 00000000..f5e5967b --- /dev/null +++ b/ouster-ros/test/ring_buffer_test.cpp @@ -0,0 +1,138 @@ +#include +#include +#include +#include +#include "../src/thread_safe_ring_buffer.h" + +using namespace std::chrono_literals; + +class ThreadSafeRingBufferTest : public ::testing::Test { + protected: + static const int ITEM_SIZE = 4; + static const int ITEM_COUNT = 3; + + void SetUp() override { + buffer = std::make_unique(ITEM_SIZE, ITEM_COUNT); + } + + void TearDown() override { + buffer.reset(); + } + + std::string rand_str(int size) { + const std::string characters = + "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789"; + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution dist(0, characters.size() - 1); + + std::string result; + for (int i = 0; i < size; ++i) { + result += characters[dist(gen)]; + } + return result; + } + + std::vector rand_vector_str(int vec_size, int str_size) { + std::vector output(vec_size); + for (auto i = 0; i < vec_size; ++i) + output[i] = rand_str(str_size); + return output; + } + + std::vector known_vector_str(int vec_size, const std::string& known) { + std::vector output(vec_size); + for (auto i = 0; i < vec_size; ++i) + output[i] = known; + return output; + } + + std::unique_ptr buffer; +}; + +TEST_F(ThreadSafeRingBufferTest, ReadWriteToBuffer) { + + const int TOTAL_ITEMS = 10; // total items to process + const std::vector source = rand_vector_str(TOTAL_ITEMS, ITEM_SIZE); + std::vector target = known_vector_str(TOTAL_ITEMS, "0000"); + + EXPECT_TRUE(buffer->empty()); + EXPECT_FALSE(buffer->full()); + + std::thread producer([this, &source]() { + for (int i = 0; i < TOTAL_ITEMS; ++i) { + buffer->write([i, &source](uint8_t* buffer){ + std::memcpy(buffer, &source[i][0], ITEM_SIZE); + }); + } + }); + + std::thread consumer([this, &target]() { + for (int i = 0; i < TOTAL_ITEMS; ++i) { + buffer->read([i, &target](uint8_t* buffer){ + std::memcpy(&target[i][0], buffer, ITEM_SIZE); + }); + } + }); + + producer.join(); + consumer.join(); + + for (int i = 0; i < TOTAL_ITEMS; ++i) { + std::cout << "source " << source[i] << ", target " << target[i] << std::endl; + EXPECT_EQ(target[i], source[i]); + } +} + +TEST_F(ThreadSafeRingBufferTest, ReadWriteToBufferWithOverwrite) { + + const int TOTAL_ITEMS = 10; // total items to process + const std::vector source = rand_vector_str(TOTAL_ITEMS, ITEM_SIZE); + std::vector target = known_vector_str(TOTAL_ITEMS, "0000"); + + EXPECT_TRUE(buffer->empty()); + EXPECT_FALSE(buffer->full()); + + std::thread producer([this, &source]() { + for (int i = 0; i < TOTAL_ITEMS; ++i) { + buffer->write_overwrite([i, &source](uint8_t* buffer){ + std::memcpy(buffer, &source[i][0], ITEM_SIZE); + }); + } + }); + + // wait for 1 second before starting the consumer thread + // allowing sufficient time for the producer thread to be + // completely done + std::this_thread::sleep_for(1s); + std::thread consumer([this, &target]() { + for (int i = 0; i < TOTAL_ITEMS; ++i) { + buffer->read_timeout([i, &target](uint8_t* buffer){ + if (buffer != nullptr) + std::memcpy(&target[i][0], buffer, ITEM_SIZE); + }, 1s); + } + }); + + producer.join(); + consumer.join(); + + // Since our buffer can host only up to ITEM_COUNT simultanously only the + // last ITEM_COUNT items would have remained in the buffer by the time + // the consumer started processing. + for (int i = 0; i < ITEM_COUNT; ++i) { + std::cout << "source " << source[i] << ", target " << target[i] << std::endl; + EXPECT_EQ(target[i], source[TOTAL_ITEMS-ITEM_COUNT+i]); + } + + for (int i = ITEM_COUNT; i < TOTAL_ITEMS; ++i) { + std::cout << "source " << source[i] << ", target " << target[i] << std::endl; + EXPECT_EQ(target[i], "XSDS"); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file From 129bf0128a921ed45d4d3a33dfc61d3f3899bf66 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 1 Jun 2023 14:53:22 -0700 Subject: [PATCH 05/38] Add one more case to the unit test of ThreadSafeRingBuffer --- ouster-ros/test/ring_buffer_test.cpp | 49 ++++++++++++++++++++++++++-- 1 file changed, 46 insertions(+), 3 deletions(-) diff --git a/ouster-ros/test/ring_buffer_test.cpp b/ouster-ros/test/ring_buffer_test.cpp index f5e5967b..8d8bd7f4 100644 --- a/ouster-ros/test/ring_buffer_test.cpp +++ b/ouster-ros/test/ring_buffer_test.cpp @@ -8,8 +8,8 @@ using namespace std::chrono_literals; class ThreadSafeRingBufferTest : public ::testing::Test { protected: - static const int ITEM_SIZE = 4; - static const int ITEM_COUNT = 3; + static const int ITEM_SIZE = 4; // predefined size for all items used in + static const int ITEM_COUNT = 3; // number of item the buffer could hold void SetUp() override { buffer = std::make_unique(ITEM_SIZE, ITEM_COUNT); @@ -50,6 +50,49 @@ class ThreadSafeRingBufferTest : public ::testing::Test { std::unique_ptr buffer; }; +TEST_F(ThreadSafeRingBufferTest, ReadWriteToBufferSimple) { + + assert (ITEM_COUNT > 1 && "or this test can't run"); + + const int TOTAL_ITEMS = 10; // total items to process + const std::vector source = rand_vector_str(TOTAL_ITEMS, ITEM_SIZE); + std::vector target = known_vector_str(TOTAL_ITEMS, "0000"); + + EXPECT_TRUE(buffer->empty()); + EXPECT_FALSE(buffer->full()); + + for (int i = 0; i < ITEM_COUNT; ++i) { + buffer->write([i, &source](uint8_t* buffer) { + std::memcpy(buffer, &source[i][0], ITEM_SIZE); + }); + } + + EXPECT_FALSE(buffer->empty()); + EXPECT_TRUE(buffer->full()); + + // remove one item + buffer->read([&target](uint8_t* buffer){ + std::memcpy(&target[0][0], buffer, ITEM_SIZE); + }); + + EXPECT_FALSE(buffer->empty()); + EXPECT_FALSE(buffer->full()); + + for (int i = 1; i < ITEM_COUNT; ++i) { + buffer->read([i, &target](uint8_t* buffer){ + std::memcpy(&target[i][0], buffer, ITEM_SIZE); + }); + } + + EXPECT_TRUE(buffer->empty()); + EXPECT_FALSE(buffer->full()); + + for (int i = 0; i < ITEM_COUNT; ++i) { + std::cout << "source " << source[i] << ", target " << target[i] << std::endl; + EXPECT_EQ(target[i], source[i]); + } +} + TEST_F(ThreadSafeRingBufferTest, ReadWriteToBuffer) { const int TOTAL_ITEMS = 10; // total items to process @@ -127,7 +170,7 @@ TEST_F(ThreadSafeRingBufferTest, ReadWriteToBufferWithOverwrite) { for (int i = ITEM_COUNT; i < TOTAL_ITEMS; ++i) { std::cout << "source " << source[i] << ", target " << target[i] << std::endl; - EXPECT_EQ(target[i], "XSDS"); + EXPECT_EQ(target[i], "0000"); } } From f74a4928d58e2fb6745c9a6d5b42d244398bc73e Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 1 Jun 2023 18:45:52 -0700 Subject: [PATCH 06/38] Quick hook up of the ThreadSafeRingBuffer for os_sensor and os_driver --- ouster-ros/src/lidar_packet_handler.h | 2 +- ouster-ros/src/os_driver_node.cpp | 8 ++++---- ouster-ros/src/os_sensor_node.cpp | 29 +++++++++++++++++++++------ ouster-ros/src/os_sensor_node.h | 10 +++++++-- 4 files changed, 36 insertions(+), 13 deletions(-) diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h index de0d80ac..a8f43077 100644 --- a/ouster-ros/src/lidar_packet_handler.h +++ b/ouster-ros/src/lidar_packet_handler.h @@ -226,7 +226,7 @@ class LidarPacketHandler { return current_time - delta_time; } - bool lidar_handler_sensor_time(const sensor::packet_format& pf, const uint8_t* lidar_buf) { + bool lidar_handler_sensor_time(const sensor::packet_format&, const uint8_t* lidar_buf) { if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; auto scan_ts = compute_scan_ts(lidar_scan->timestamp()); convert_scan_to_pointcloud(scan_ts, rclcpp::Time(scan_ts)); diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index ed78b6c7..b0eb389d 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -78,15 +78,15 @@ class OusterDriver : public OusterSensor { info, sensor_frame, use_ros_time); // TODO: add an option to select sensor_frame } - virtual void on_lidar_packet_msg(const PacketMsg& lidar_packet) override { - auto point_cloud_msgs = lidar_packet_handler(lidar_packet.buf.data()); + virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override { + auto point_cloud_msgs = lidar_packet_handler(raw_lidar_packet); for (size_t i = 0; i < point_cloud_msgs.size(); ++i) { lidar_pubs[i]->publish(*point_cloud_msgs[i]); } } - virtual void on_imu_packet_msg(const PacketMsg& imu_packet) override { - auto imu_msg = imu_packet_handler(imu_packet.buf.data()); + virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet) override { + auto imu_msg = imu_packet_handler(raw_imu_packet); imu_pub->publish(imu_msg); } diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index 4270ddcf..c63a4e66 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -643,8 +643,13 @@ void OusterSensor::create_publishers() { void OusterSensor::allocate_buffers() { auto& pf = sensor::get_format(info); + // TODO: Do we need the +1? lidar_packet.buf.resize(pf.lidar_packet_size + 1); + lidar_packets = std::make_unique( + pf.lidar_packet_size + 1, 1024); imu_packet.buf.resize(pf.imu_packet_size + 1); + imu_packets = std::make_unique( + pf.imu_packet_size + 1, 1024); } bool OusterSensor::init_id_changed(const sensor::packet_format& pf, @@ -683,7 +688,10 @@ void OusterSensor::handle_lidar_packet(sensor::client& cli, const sensor::packet_format& pf) { std::unique_lock lock(mtx); receiving_cv.wait(lock, [this]{ return lidar_data_processed; }); - bool success = sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf); + bool success = true; + lidar_packets->write_overwrite([&cli, pf, &success](uint8_t* buffer) { + success = sensor::read_lidar_packet(cli, buffer, pf); + }); lidar_data_processed = false; lock.unlock(); processing_cv.notify_all(); @@ -715,7 +723,10 @@ void OusterSensor::handle_imu_packet(sensor::client& cli, const sensor::packet_format& pf) { std::unique_lock lock(mtx); receiving_cv.wait(lock, [this]{ return imu_data_processed; }); - bool success = sensor::read_imu_packet(cli, imu_packet.buf.data(), pf); + bool success = true; + imu_packets->write_overwrite([&cli, pf, &success](uint8_t* buffer) { + success = sensor::read_imu_packet(cli, buffer, pf); + }); imu_data_processed = false; lock.unlock(); processing_cv.notify_one(); @@ -801,11 +812,13 @@ void OusterSensor::stop_packet_processing_thread() { } } -void OusterSensor::on_lidar_packet_msg(const PacketMsg& lidar_packet) { +void OusterSensor::on_lidar_packet_msg(const uint8_t* raw_lidar_packet) { + std::memcpy(lidar_packet.buf.data(), raw_lidar_packet, lidar_packet.buf.size()); lidar_packet_pub->publish(lidar_packet); } -void OusterSensor::on_imu_packet_msg(const PacketMsg& imu_packet) { +void OusterSensor::on_imu_packet_msg(const uint8_t* raw_imu_packet) { + std::memcpy(imu_packet.buf.data(), raw_imu_packet, imu_packet.buf.size()); imu_packet_pub->publish(imu_packet); } @@ -819,12 +832,16 @@ void OusterSensor::process_packets() { return !lidar_data_processed || !imu_data_processed; }); if (!lidar_data_processed) { - on_lidar_packet_msg(lidar_packet); + lidar_packets->read([this](const uint8_t* buffer) { + on_lidar_packet_msg(buffer); + }); lidar_data_processed = true; } if (!imu_data_processed) { - on_imu_packet_msg(imu_packet); + imu_packets->read([this](const uint8_t* buffer) { + on_imu_packet_msg(buffer); + }); imu_data_processed = true; } diff --git a/ouster-ros/src/os_sensor_node.h b/ouster-ros/src/os_sensor_node.h index 9d1443eb..4c995c70 100644 --- a/ouster-ros/src/os_sensor_node.h +++ b/ouster-ros/src/os_sensor_node.h @@ -26,6 +26,8 @@ #include "ouster_ros/visibility_control.h" #include "ouster_ros/os_sensor_node_base.h" +#include "thread_safe_ring_buffer.h" + namespace sensor = ouster::sensor; using lifecycle_msgs::srv::ChangeState; using ouster_srvs::srv::GetConfig; @@ -55,9 +57,9 @@ class OusterSensor : public OusterSensorNodeBase { virtual void create_publishers(); - virtual void on_lidar_packet_msg(const ouster_msgs::msg::PacketMsg& lidar_packet); + virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet); - virtual void on_imu_packet_msg(const ouster_msgs::msg::PacketMsg& imu_packet); + virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet); private: void declare_parameters(); @@ -152,6 +154,10 @@ class OusterSensor : public OusterSensorNodeBase { bool lidar_data_processed = true; bool imu_data_processed = true; + // TODO: implement & utilize a lock-free ring buffer in future + std::unique_ptr lidar_packets; + std::unique_ptr imu_packets; + std::atomic sensor_connection_active = {false}; std::unique_ptr sensor_connection_thread; From a6e52f7d3950519ffdae3561c3d86f6a63bc772b Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Fri, 2 Jun 2023 10:42:11 -0700 Subject: [PATCH 07/38] Add an option to select the point_cloud frame --- ouster-ros/src/os_cloud_node.cpp | 16 +++++++++++++++- ouster-ros/src/os_driver_node.cpp | 17 ++++++++++++++++- ouster-ros/src/os_sensor_node.cpp | 8 ++++++++ ouster-ros/src/os_sensor_node.h | 1 + 4 files changed, 40 insertions(+), 2 deletions(-) diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 36afe33c..cda63810 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -59,6 +59,7 @@ class OusterCloud : public OusterProcessingNodeBase { declare_parameter("sensor_frame", "os_sensor"); declare_parameter("lidar_frame", "os_lidar"); declare_parameter("imu_frame", "os_imu"); + declare_parameter("point_cloud_frame", "os_lidar"); declare_parameter("timestamp_mode", ""); } @@ -66,6 +67,18 @@ class OusterCloud : public OusterProcessingNodeBase { sensor_frame = get_parameter("sensor_frame").as_string(); lidar_frame = get_parameter("lidar_frame").as_string(); imu_frame = get_parameter("imu_frame").as_string(); + point_cloud_frame = get_parameter("point_cloud_frame").as_string(); + + // validate point_cloud_frame + if (point_cloud_frame.empty()) { + point_cloud_frame = lidar_frame; // for ROS1 we'd still use sensor_frame + } else if (point_cloud_frame != sensor_frame && point_cloud_frame != lidar_frame) { + RCLCPP_WARN(get_logger(), + "point_cloud_frame value needs to match the value of either sensor_frame" + " or lidar_frame but a different value was supplied, using lidar_frame's" + " value as the value for point_cloud_frame"); + point_cloud_frame = lidar_frame; + } auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME"; @@ -111,7 +124,7 @@ class OusterCloud : public OusterProcessingNodeBase { }); lidar_packet_handler = LidarPacketHandler::create_handler( - info, sensor_frame, use_ros_time); // TODO: add an option to select sensor_frame + info, point_cloud_frame, use_ros_time); lidar_packet_sub = create_subscription( "lidar_packets", qos, [this](const PacketMsg::ConstSharedPtr msg) { @@ -133,6 +146,7 @@ class OusterCloud : public OusterProcessingNodeBase { std::string sensor_frame; std::string imu_frame; std::string lidar_frame; + std::string point_cloud_frame; tf2_ros::StaticTransformBroadcaster tf_bcast; diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index b0eb389d..2f9ddd91 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -38,12 +38,26 @@ class OusterDriver : public OusterSensor { declare_parameter("sensor_frame", "os_sensor"); declare_parameter("lidar_frame", "os_lidar"); declare_parameter("imu_frame", "os_imu"); + declare_parameter("point_cloud_frame", "os_lidar"); } void parse_parameters() { + // TODO: avoid duplication of tf frames code sensor_frame = get_parameter("sensor_frame").as_string(); lidar_frame = get_parameter("lidar_frame").as_string(); imu_frame = get_parameter("imu_frame").as_string(); + point_cloud_frame = get_parameter("point_cloud_frame").as_string(); + + // validate point_cloud_frame + if (point_cloud_frame.empty()) { + point_cloud_frame = lidar_frame; // for ROS1 we'd still use sensor_frame + } else if (point_cloud_frame != sensor_frame && point_cloud_frame != lidar_frame) { + RCLCPP_WARN(get_logger(), + "point_cloud_frame value needs to match the value of either sensor_frame" + " or lidar_frame but a different value was supplied, using lidar_frame's" + " value as the value for point_cloud_frame"); + point_cloud_frame = lidar_frame; + } } virtual void on_metadata_updated(const sensor::sensor_info& info) override { @@ -75,7 +89,7 @@ class OusterDriver : public OusterSensor { imu_packet_handler = ImuPacketHandler::create_handler( info, imu_frame, use_ros_time); lidar_packet_handler = LidarPacketHandler::create_handler( - info, sensor_frame, use_ros_time); // TODO: add an option to select sensor_frame + info, point_cloud_frame, use_ros_time); } virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override { @@ -94,6 +108,7 @@ class OusterDriver : public OusterSensor { std::string sensor_frame; std::string imu_frame; std::string lidar_frame; + std::string point_cloud_frame; tf2_ros::StaticTransformBroadcaster tf_bcast; diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index c63a4e66..dc8efad7 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -813,11 +813,19 @@ void OusterSensor::stop_packet_processing_thread() { } void OusterSensor::on_lidar_packet_msg(const uint8_t* raw_lidar_packet) { + // copying the data from queue buffer into the message buffer + // this can be avoided by constructing an abstraction where + // OusterSensor has its own RingBuffer of PacketMsg but for + // now we are focusing on optimizing the code for OusterDriver std::memcpy(lidar_packet.buf.data(), raw_lidar_packet, lidar_packet.buf.size()); lidar_packet_pub->publish(lidar_packet); } void OusterSensor::on_imu_packet_msg(const uint8_t* raw_imu_packet) { + // copying the data from queue buffer into the message buffer + // this can be avoided by constructing an abstraction where + // OusterSensor has its own RingBuffer of PacketMsg but for + // now we are focusing on optimizing the code for OusterDriver std::memcpy(imu_packet.buf.data(), raw_imu_packet, imu_packet.buf.size()); imu_packet_pub->publish(imu_packet); } diff --git a/ouster-ros/src/os_sensor_node.h b/ouster-ros/src/os_sensor_node.h index 4c995c70..39aa33ad 100644 --- a/ouster-ros/src/os_sensor_node.h +++ b/ouster-ros/src/os_sensor_node.h @@ -148,6 +148,7 @@ class OusterSensor : public OusterSensorNodeBase { rclcpp::Service::SharedPtr set_config_srv; std::shared_ptr> change_state_client; + // TODO: reduce sync objects std::mutex mtx; std::condition_variable receiving_cv; std::condition_variable processing_cv; From 82e1786c03a61fb04a5bd0a5e12d137db1fdb3db Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 5 Jun 2023 09:22:36 -0700 Subject: [PATCH 08/38] Keep transforms in Lidar Frame by default with option to switch --- ouster-ros/config/driver_params.yaml | 17 +++++++++++++ .../{parameters.yaml => legacy_params.yaml} | 7 +++--- ouster-ros/src/lidar_packet_handler.h | 24 ++++++++++++++----- ouster-ros/src/os_cloud_node.cpp | 3 ++- ouster-ros/src/os_driver_node.cpp | 3 ++- 5 files changed, 43 insertions(+), 11 deletions(-) create mode 100644 ouster-ros/config/driver_params.yaml rename ouster-ros/config/{parameters.yaml => legacy_params.yaml} (76%) diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml new file mode 100644 index 00000000..300c4505 --- /dev/null +++ b/ouster-ros/config/driver_params.yaml @@ -0,0 +1,17 @@ +ouster/os_driver: + ros__parameters: + sensor_hostname: '' + udp_dest: '' + mtp_dest: '' + mtp_main: false + lidar_mode: '' + timestamp_mode: '' + udp_profile_lidar: '' + metadata: '' + lidar_port: 0 + imu_port: 0 + sensor_frame: os_sensor + lidar_frame: os_lidar + imu_frame: os_imu + point_cloud_frame: os_lidar + timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode diff --git a/ouster-ros/config/parameters.yaml b/ouster-ros/config/legacy_params.yaml similarity index 76% rename from ouster-ros/config/parameters.yaml rename to ouster-ros/config/legacy_params.yaml index 7ea15975..33775bbc 100644 --- a/ouster-ros/config/parameters.yaml +++ b/ouster-ros/config/legacy_params.yaml @@ -12,7 +12,8 @@ ouster/os_sensor: imu_port: 0 ouster/os_cloud: ros__parameters: - sensor_frame: 'os_sensor' - lidar_frame: 'os_lidar' - imu_frame: 'os_imu' + sensor_frame: os_sensor + lidar_frame: os_lidar + imu_frame: os_imu + point_cloud_frame: os_lidar timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h index a8f43077..d980b5ed 100644 --- a/ouster-ros/src/lidar_packet_handler.h +++ b/ouster-ros/src/lidar_packet_handler.h @@ -65,9 +65,10 @@ class LidarPacketHandler { using HandlerType = std::function; public: - LidarPacketHandler(const ouster::sensor::sensor_info& info, const std::string& frame, bool use_ros_time) : + LidarPacketHandler(const ouster::sensor::sensor_info& info, const std::string& frame, + bool apply_lidar_to_sensor_transform, bool use_ros_time) : ref_frame(frame) { - create_lidarscan_objects(info); + create_lidarscan_objects(info, apply_lidar_to_sensor_transform); compute_scan_ts = [this](const auto& ts_v) { return compute_scan_ts_0(ts_v); }; @@ -88,8 +89,8 @@ class LidarPacketHandler { public: static HandlerType create_handler( - const ouster::sensor::sensor_info& info, const std::string& frame, bool use_ros_time) { - auto handler = std::make_shared(info, frame, use_ros_time); + const ouster::sensor::sensor_info& info, const std::string& frame, bool apply_lidar_to_sensor_transform, bool use_ros_time) { + auto handler = std::make_shared(info, frame, apply_lidar_to_sensor_transform, use_ros_time); return [handler](const uint8_t* lidar_buf) { return handler->lidar_packet_accumlator(lidar_buf) ? handler->pc_msgs : HandlerOutput{}; }; @@ -104,11 +105,22 @@ class LidarPacketHandler { } private: - void create_lidarscan_objects(const ouster::sensor::sensor_info& info) { + void create_lidarscan_objects(const ouster::sensor::sensor_info& info, bool apply_lidar_to_sensor_transform) { + ouster::mat4d additional_transform = + apply_lidar_to_sensor_transform ? + info.lidar_to_sensor_transform : + ouster::mat4d::Identity(); + auto xyz_lut = ouster::make_xyz_lut( + info.format.columns_per_frame, + info.format.pixels_per_column, + ouster::sensor::range_unit, + additional_transform, + ouster::mat4d::Identity(), + info.beam_azimuth_angles, + info.beam_altitude_angles); // The ouster_ros drive currently only uses single precision when it // produces the point cloud. So it isn't of a benefit to compute point // cloud xyz coordinates using double precision (for the time being). - auto xyz_lut = ouster::make_xyz_lut(info); lut_direction = xyz_lut.direction.cast(); lut_offset = xyz_lut.offset.cast(); points = ouster::PointsF(lut_direction.rows(), lut_offset.cols()); diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index cda63810..c6f0f8fb 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -123,8 +123,9 @@ class OusterCloud : public OusterProcessingNodeBase { imu_pub->publish(imu_msg); }); + bool apply_lidar_to_sensor_transform = point_cloud_frame == sensor_frame; lidar_packet_handler = LidarPacketHandler::create_handler( - info, point_cloud_frame, use_ros_time); + info, point_cloud_frame, apply_lidar_to_sensor_transform, use_ros_time); lidar_packet_sub = create_subscription( "lidar_packets", qos, [this](const PacketMsg::ConstSharedPtr msg) { diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 2f9ddd91..9f5476c5 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -88,8 +88,9 @@ class OusterDriver : public OusterSensor { imu_packet_handler = ImuPacketHandler::create_handler( info, imu_frame, use_ros_time); + bool apply_lidar_to_sensor_transform = point_cloud_frame == sensor_frame; lidar_packet_handler = LidarPacketHandler::create_handler( - info, point_cloud_frame, use_ros_time); + info, point_cloud_frame, apply_lidar_to_sensor_transform, use_ros_time); } virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override { From 18e66580eec4dee41cb6dacae3d580ed95b1b2fc Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 5 Jun 2023 12:21:11 -0700 Subject: [PATCH 09/38] Formatting os_sensor and os_driver --- ouster-ros/src/os_driver_node.cpp | 29 ++--- ouster-ros/src/os_sensor_node.cpp | 174 ++++++++++++++---------------- 2 files changed, 98 insertions(+), 105 deletions(-) diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 9f5476c5..3903a0a1 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -25,7 +25,7 @@ namespace ouster_ros { using ouster_msgs::msg::PacketMsg; class OusterDriver : public OusterSensor { - public: + public: OUSTER_ROS_PUBLIC explicit OusterDriver(const rclcpp::NodeOptions& options) : OusterSensor("os_driver", options), tf_bcast(this) { @@ -33,7 +33,7 @@ class OusterDriver : public OusterSensor { parse_parameters(); } -private: + private: void declare_parameters() { declare_parameter("sensor_frame", "os_sensor"); declare_parameter("lidar_frame", "os_lidar"); @@ -50,12 +50,15 @@ class OusterDriver : public OusterSensor { // validate point_cloud_frame if (point_cloud_frame.empty()) { - point_cloud_frame = lidar_frame; // for ROS1 we'd still use sensor_frame - } else if (point_cloud_frame != sensor_frame && point_cloud_frame != lidar_frame) { + point_cloud_frame = + lidar_frame; // for ROS1 we'd still use sensor_frame + } else if (point_cloud_frame != sensor_frame && + point_cloud_frame != lidar_frame) { RCLCPP_WARN(get_logger(), - "point_cloud_frame value needs to match the value of either sensor_frame" - " or lidar_frame but a different value was supplied, using lidar_frame's" - " value as the value for point_cloud_frame"); + "point_cloud_frame value needs to match the value of " + "either sensor_frame or lidar_frame but a different " + "value was supplied, using lidar_frame's value as the " + "value for point_cloud_frame"); point_cloud_frame = lidar_frame; } } @@ -86,11 +89,13 @@ class OusterDriver : public OusterSensor { auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME"; - imu_packet_handler = ImuPacketHandler::create_handler( - info, imu_frame, use_ros_time); - bool apply_lidar_to_sensor_transform = point_cloud_frame == sensor_frame; + imu_packet_handler = + ImuPacketHandler::create_handler(info, imu_frame, use_ros_time); + bool apply_lidar_to_sensor_transform = + point_cloud_frame == sensor_frame; lidar_packet_handler = LidarPacketHandler::create_handler( - info, point_cloud_frame, apply_lidar_to_sensor_transform, use_ros_time); + info, point_cloud_frame, apply_lidar_to_sensor_transform, + use_ros_time); } virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override { @@ -105,7 +110,7 @@ class OusterDriver : public OusterSensor { imu_pub->publish(imu_msg); } -private: + private: std::string sensor_frame; std::string imu_frame; std::string lidar_frame; diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index dc8efad7..997372d3 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -24,15 +24,16 @@ using namespace std::string_literals; namespace ouster_ros { -OusterSensor::OusterSensor(const std::string& name, const rclcpp::NodeOptions& options) +OusterSensor::OusterSensor(const std::string& name, + const rclcpp::NodeOptions& options) : OusterSensorNodeBase(name, options), - change_state_client{create_client(get_name() + "/change_state"s)} { + change_state_client{ + create_client(get_name() + "/change_state"s)} { declare_parameters(); } OusterSensor::OusterSensor(const rclcpp::NodeOptions& options) - : OusterSensor("os_sensor", options) { -} + : OusterSensor("os_sensor", options) {} void OusterSensor::declare_parameters() { declare_parameter("sensor_hostname"); @@ -54,8 +55,8 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_configure( try { sensor_hostname = get_sensor_hostname(); auto config = staged_config.empty() - ? parse_config_from_ros_parameters() - : parse_config_from_staged_config_string(); + ? parse_config_from_ros_parameters() + : parse_config_from_staged_config_string(); configure_sensor(sensor_hostname, config); sensor_client = create_sensor_client(sensor_hostname, config); if (!sensor_client) @@ -213,10 +214,11 @@ void OusterSensor::save_metadata() { } else { RCLCPP_WARN_STREAM( get_logger(), - "Failed to write metadata to " << meta_file << - "; check that the path is valid. " - "If you provided a relative path, please note that the working " - "directory of all ROS nodes is set by default to $ROS_HOME"); + "Failed to write metadata to " + << meta_file + << "; check that the path is valid. If you provided a relative " + "path, please note that the working directory of all ROS " + "nodes is set by default to $ROS_HOME"); } } @@ -241,13 +243,12 @@ std::string OusterSensor::transition_id_to_string(uint8_t transition_id) { template bool OusterSensor::change_state(std::uint8_t transition_id, CallbackT callback, - CallbackT_Args... callback_args, - std::chrono::seconds time_out) { + CallbackT_Args... callback_args, + std::chrono::seconds time_out) { if (!change_state_client->wait_for_service(time_out)) { - RCLCPP_ERROR_STREAM(get_logger(), - "Service " - << change_state_client->get_service_name() - << "is not available."); + RCLCPP_ERROR_STREAM( + get_logger(), "Service " << change_state_client->get_service_name() + << "is not available."); return false; } @@ -255,8 +256,8 @@ bool OusterSensor::change_state(std::uint8_t transition_id, CallbackT callback, request->transition.id = transition_id; // send an async request to perform the transition change_state_client->async_send_request( - request, [callback, callback_args...]( - rclcpp::Client::SharedFuture) { + request, [callback, + callback_args...](rclcpp::Client::SharedFuture) { callback(callback_args...); }); return true; @@ -265,22 +266,20 @@ bool OusterSensor::change_state(std::uint8_t transition_id, CallbackT callback, void OusterSensor::execute_transitions_sequence( std::vector transitions_sequence, size_t at) { assert(at < transitions_sequence.size() && - "at index exceeds the number of transitions"); + "at index exceeds the number of transitions"); auto transition_id = transitions_sequence[at]; - RCLCPP_DEBUG_STREAM(get_logger(), - "transition: [" - << transition_id_to_string(transition_id) - << "] started"); + RCLCPP_DEBUG_STREAM( + get_logger(), "transition: [" << transition_id_to_string(transition_id) + << "] started"); change_state(transition_id, [this, transitions_sequence, at]() { - RCLCPP_DEBUG_STREAM(get_logger(), - "transition: [" << transition_id_to_string( - transitions_sequence[at]) - << "] completed"); + RCLCPP_DEBUG_STREAM( + get_logger(), + "transition: [" << transition_id_to_string(transitions_sequence[at]) + << "] completed"); if (at < transitions_sequence.size() - 1) { execute_transitions_sequence(transitions_sequence, at + 1); } else { - RCLCPP_DEBUG_STREAM(get_logger(), - "transitions sequence completed"); + RCLCPP_DEBUG_STREAM(get_logger(), "transitions sequence completed"); } }); } @@ -324,9 +323,8 @@ void OusterSensor::reactivate_sensor(bool init_id_reset) { void OusterSensor::create_reset_service() { reset_srv = create_service( - "reset", - [this](const std::shared_ptr, - std::shared_ptr) { + "reset", [this](const std::shared_ptr, + std::shared_ptr) { RCLCPP_INFO(get_logger(), "reset service invoked"); reset_sensor(true); }); @@ -336,9 +334,8 @@ void OusterSensor::create_reset_service() { void OusterSensor::create_get_config_service() { get_config_srv = create_service( - "get_config", - [this](const std::shared_ptr, - std::shared_ptr response) { + "get_config", [this](const std::shared_ptr, + std::shared_ptr response) { response->config = active_config; return active_config.size() > 0; }); @@ -348,9 +345,8 @@ void OusterSensor::create_get_config_service() { void OusterSensor::create_set_config_service() { set_config_srv = create_service( - "set_config", - [this](const std::shared_ptr request, - std::shared_ptr response) { + "set_config", [this](const std::shared_ptr request, + std::shared_ptr response) { response->config = ""; try { @@ -365,10 +361,9 @@ void OusterSensor::create_set_config_service() { } } catch (const std::exception& e) { RCLCPP_ERROR_STREAM( - get_logger(), - "exception thrown while loading config file: " - << request->config_file - << ", exception details: " << e.what()); + get_logger(), "exception thrown while loading config file: " + << request->config_file + << ", exception details: " << e.what()); return false; } @@ -385,12 +380,10 @@ void OusterSensor::create_set_config_service() { std::shared_ptr OusterSensor::create_sensor_client( const std::string& hostname, const sensor::sensor_config& config) { - RCLCPP_INFO_STREAM(get_logger(), "Starting sensor " - << hostname - << " initialization..."); + RCLCPP_INFO_STREAM(get_logger(), + "Starting sensor " << hostname << " initialization..."); - int lidar_port = - config.udp_port_lidar ? config.udp_port_lidar.value() : 0; + int lidar_port = config.udp_port_lidar ? config.udp_port_lidar.value() : 0; int imu_port = config.udp_port_imu ? config.udp_port_imu.value() : 0; auto udp_dest = config.udp_dest ? config.udp_dest.value() : ""; @@ -407,9 +400,9 @@ std::shared_ptr OusterSensor::create_sensor_client( // use the full init_client to generate and assign random ports to // sensor auto udp_dest = config.udp_dest ? config.udp_dest.value() : ""; - cli = sensor::init_client(hostname, udp_dest, sensor::MODE_UNSPEC, - sensor::TIME_FROM_UNSPEC, lidar_port, - imu_port); + cli = + sensor::init_client(hostname, udp_dest, sensor::MODE_UNSPEC, + sensor::TIME_FROM_UNSPEC, lidar_port, imu_port); } if (!cli) { @@ -429,8 +422,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { auto imu_port = get_parameter("imu_port").as_int(); auto lidar_mode_arg = get_parameter("lidar_mode").as_string(); auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); - auto udp_profile_lidar_arg = - get_parameter("udp_profile_lidar").as_string(); + auto udp_profile_lidar_arg = get_parameter("udp_profile_lidar").as_string(); if (lidar_port < 0 || lidar_port > 65535) { auto error_msg = @@ -519,8 +511,7 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { if (is_arg_set(udp_dest)) { config.udp_dest = udp_dest; if (sensor::in_multicast(udp_dest)) { - mtp_dest = - is_arg_set(mtp_dest_arg) ? mtp_dest_arg : std::string{}; + mtp_dest = is_arg_set(mtp_dest_arg) ? mtp_dest_arg : std::string{}; mtp_main = mtp_main_arg; } } @@ -534,11 +525,12 @@ sensor::sensor_config OusterSensor::parse_config_from_staged_config_string() { return config; } -uint8_t OusterSensor::compose_config_flags(const sensor::sensor_config& config) { +uint8_t OusterSensor::compose_config_flags( + const sensor::sensor_config& config) { uint8_t config_flags = 0; if (config.udp_dest) { - RCLCPP_INFO_STREAM(get_logger(), "Will send UDP data to " - << config.udp_dest.value()); + RCLCPP_INFO_STREAM(get_logger(), + "Will send UDP data to " << config.udp_dest.value()); // TODO: revise multicast setup inference if (sensor::in_multicast(*config.udp_dest)) { if (is_arg_set(mtp_dest)) { @@ -567,7 +559,7 @@ uint8_t OusterSensor::compose_config_flags(const sensor::sensor_config& config) } void OusterSensor::configure_sensor(const std::string& hostname, - sensor::sensor_config& config) { + sensor::sensor_config& config) { if (config.udp_dest && sensor::in_multicast(config.udp_dest.value()) && !mtp_main) { if (!get_config(hostname, config, true)) { @@ -584,12 +576,12 @@ void OusterSensor::configure_sensor(const std::string& hostname, } RCLCPP_INFO_STREAM(get_logger(), - "Sensor " << hostname << " configured successfully"); + "Sensor " << hostname << " configured successfully"); } // fill in values that could not be parsed from metadata -void OusterSensor::populate_metadata_defaults(sensor::sensor_info& info, - sensor::lidar_mode specified_lidar_mode) { +void OusterSensor::populate_metadata_defaults( + sensor::sensor_info& info, sensor::lidar_mode specified_lidar_mode) { if (!info.name.size()) info.name = "UNKNOWN"; if (!info.sn.size()) info.sn = "UNKNOWN"; @@ -599,10 +591,9 @@ void OusterSensor::populate_metadata_defaults(sensor::sensor_info& info, get_logger(), "Unknown sensor firmware version; output may not be reliable"); else if (v < sensor::min_version) - RCLCPP_WARN( - get_logger(), - "Firmware < %s not supported; output may not be reliable", - to_string(sensor::min_version).c_str()); + RCLCPP_WARN(get_logger(), + "Firmware < %s not supported; output may not be reliable", + to_string(sensor::min_version).c_str()); if (!info.mode) { RCLCPP_WARN( @@ -613,11 +604,9 @@ void OusterSensor::populate_metadata_defaults(sensor::sensor_info& info, if (!info.prod_line.size()) info.prod_line = "UNKNOWN"; - if (info.beam_azimuth_angles.empty() || - info.beam_altitude_angles.empty()) { - RCLCPP_ERROR( - get_logger(), - "Beam angles not found in metadata; using design values"); + if (info.beam_azimuth_angles.empty() || info.beam_altitude_angles.empty()) { + RCLCPP_ERROR(get_logger(), + "Beam angles not found in metadata; using design values"); info.beam_azimuth_angles = sensor::gen1_azimuth_angles; info.beam_altitude_angles = sensor::gen1_altitude_angles; } @@ -627,7 +616,6 @@ void OusterSensor::on_metadata_updated(const sensor::sensor_info& info) { display_lidar_info(info); } - void OusterSensor::create_services() { create_reset_service(); create_get_metadata_service(); @@ -645,15 +633,15 @@ void OusterSensor::allocate_buffers() { auto& pf = sensor::get_format(info); // TODO: Do we need the +1? lidar_packet.buf.resize(pf.lidar_packet_size + 1); - lidar_packets = std::make_unique( - pf.lidar_packet_size + 1, 1024); + lidar_packets = + std::make_unique(pf.lidar_packet_size + 1, 1024); imu_packet.buf.resize(pf.imu_packet_size + 1); - imu_packets = std::make_unique( - pf.imu_packet_size + 1, 1024); + imu_packets = + std::make_unique(pf.imu_packet_size + 1, 1024); } bool OusterSensor::init_id_changed(const sensor::packet_format& pf, - const uint8_t* lidar_buf) { + const uint8_t* lidar_buf) { uint32_t current_init_id = pf.init_id(lidar_buf); if (!last_init_id_initialized) { last_init_id = current_init_id + 1; @@ -671,7 +659,7 @@ bool OusterSensor::init_id_changed(const sensor::packet_format& pf, void OusterSensor::handle_poll_client_error() { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 100, - "sensor::poll_client()) returned error"); + "sensor::poll_client()) returned error"); // in case error continues for a while attempt to recover by // performing sensor reset if (++poll_client_error_count > max_poll_client_error_count) { @@ -685,9 +673,9 @@ void OusterSensor::handle_poll_client_error() { } void OusterSensor::handle_lidar_packet(sensor::client& cli, - const sensor::packet_format& pf) { + const sensor::packet_format& pf) { std::unique_lock lock(mtx); - receiving_cv.wait(lock, [this]{ return lidar_data_processed; }); + receiving_cv.wait(lock, [this] { return lidar_data_processed; }); bool success = true; lidar_packets->write_overwrite([&cli, pf, &success](uint8_t* buffer) { success = sensor::read_lidar_packet(cli, buffer, pf); @@ -720,9 +708,9 @@ void OusterSensor::handle_lidar_packet(sensor::client& cli, } void OusterSensor::handle_imu_packet(sensor::client& cli, - const sensor::packet_format& pf) { + const sensor::packet_format& pf) { std::unique_lock lock(mtx); - receiving_cv.wait(lock, [this]{ return imu_data_processed; }); + receiving_cv.wait(lock, [this] { return imu_data_processed; }); bool success = true; imu_packets->write_overwrite([&cli, pf, &success](uint8_t* buffer) { success = sensor::read_imu_packet(cli, buffer, pf); @@ -756,7 +744,8 @@ void OusterSensor::cleanup() { packet_processing_thread.reset(); } -void OusterSensor::connection_loop(sensor::client& cli, const sensor::packet_format& pf) { +void OusterSensor::connection_loop(sensor::client& cli, + const sensor::packet_format& pf) { auto state = sensor::poll_client(cli); if (state == sensor::EXIT) { RCLCPP_INFO(get_logger(), "poll_client: caught signal, exiting!"); @@ -817,11 +806,12 @@ void OusterSensor::on_lidar_packet_msg(const uint8_t* raw_lidar_packet) { // this can be avoided by constructing an abstraction where // OusterSensor has its own RingBuffer of PacketMsg but for // now we are focusing on optimizing the code for OusterDriver - std::memcpy(lidar_packet.buf.data(), raw_lidar_packet, lidar_packet.buf.size()); + std::memcpy(lidar_packet.buf.data(), raw_lidar_packet, + lidar_packet.buf.size()); lidar_packet_pub->publish(lidar_packet); } -void OusterSensor::on_imu_packet_msg(const uint8_t* raw_imu_packet) { +void OusterSensor::on_imu_packet_msg(const uint8_t* raw_imu_packet) { // copying the data from queue buffer into the message buffer // this can be avoided by constructing an abstraction where // OusterSensor has its own RingBuffer of PacketMsg but for @@ -830,26 +820,24 @@ void OusterSensor::on_imu_packet_msg(const uint8_t* raw_imu_packet) { imu_packet_pub->publish(imu_packet); } - void OusterSensor::process_packets() { while (packet_processing_active) { std::unique_lock lock(mtx); // Wait until there is a job in the queue - processing_cv.wait(lock, [this]{ - return !lidar_data_processed || !imu_data_processed; }); + processing_cv.wait(lock, [this] { + return !lidar_data_processed || !imu_data_processed; + }); if (!lidar_data_processed) { - lidar_packets->read([this](const uint8_t* buffer) { - on_lidar_packet_msg(buffer); - }); + lidar_packets->read( + [this](const uint8_t* buffer) { on_lidar_packet_msg(buffer); }); lidar_data_processed = true; } if (!imu_data_processed) { - imu_packets->read([this](const uint8_t* buffer) { - on_imu_packet_msg(buffer); - }); + imu_packets->read( + [this](const uint8_t* buffer) { on_imu_packet_msg(buffer); }); imu_data_processed = true; } From 3b5023a179a3236a57bb3129ecd314043b859391 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 5 Jun 2023 14:09:08 -0700 Subject: [PATCH 10/38] Provide support for parsing the community driver params file with approprite launch file --- ouster-ros/config/community_driver.rviz | 262 ++++++++++++++++++ .../config/community_driver_config.yaml | 54 ++++ ...ms.yaml => os_sensor_os_cloud_params.yaml} | 4 + ouster-ros/launch/driver.launch.py | 6 +- ouster-ros/launch/driver_launch.py | 52 ++++ ouster-ros/src/os_cloud_node.cpp | 2 +- ouster-ros/src/os_driver_node.cpp | 2 +- ouster-ros/src/os_sensor_node.cpp | 19 +- 8 files changed, 393 insertions(+), 8 deletions(-) create mode 100644 ouster-ros/config/community_driver.rviz create mode 100644 ouster-ros/config/community_driver_config.yaml rename ouster-ros/config/{legacy_params.yaml => os_sensor_os_cloud_params.yaml} (63%) create mode 100644 ouster-ros/launch/driver_launch.py diff --git a/ouster-ros/config/community_driver.rviz b/ouster-ros/config/community_driver.rviz new file mode 100644 index 00000000..927c7778 --- /dev/null +++ b/ouster-ros/config/community_driver.rviz @@ -0,0 +1,262 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + - /TF1/Tree1 + - /range1 + - /range1/Topic1 + - /nearir1/Topic1 + - /reflec1 + - /signal1 + Splitter Ratio: 0.5 + Tree Height: 1185 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: range + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Max Intensity: 3000 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + " imu_data_frame": + Value: true + " laser_data_frame": + Value: true + " laser_sensor_frame": + Value: true + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + " laser_sensor_frame": + " imu_data_frame": + {} + " laser_data_frame": + {} + Update Interval: 0 + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: range + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /range_image + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: nearir + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /nearir_image + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: reflec + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /reflec_image + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: signal + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /signal_image + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: "laser_sensor_frame" + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 4.583338260650635 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9003978371620178 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.277213096618652 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2272 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f700000599fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000002b3000005990000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000007defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000007de0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000e7000000239fc0100000002fc0000000000000e700000008d00fffffffa000000010200000004fb0000000c007200650066006c006500630000000000ffffffff0000004a00fffffffb0000000c007300690067006e0061006c0100000000ffffffff0000004a00fffffffb0000000c006e006500610072006900720000000000ffffffff0000004a00fffffffb0000000a00720061006e00670065000000006e000002390000004a00fffffffb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e700000005afc0100000002fb0000000800540069006d0065010000000000000e700000057100fffffffb0000000800540069006d0065010000000000000450000000000000000000000c6d0000059900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3696 + X: 144 + Y: 54 + nearir: + collapsed: false + range: + collapsed: false + reflec: + collapsed: false + signal: + collapsed: false diff --git a/ouster-ros/config/community_driver_config.yaml b/ouster-ros/config/community_driver_config.yaml new file mode 100644 index 00000000..7cbe7eee --- /dev/null +++ b/ouster-ros/config/community_driver_config.yaml @@ -0,0 +1,54 @@ +# [Not recommended] Provided to support existing users of the ROS2 Community +# please migrate to the parameters format as specified in driver_params.yaml +ouster_driver: + ros__parameters: + lidar_ip: os-122250002426.local + # computer_ip: + lidar_mode: "1024x10" + imu_port: 7503 + lidar_port: 7502 + sensor_frame: laser_sensor_frame + laser_frame: laser_data_frame + imu_frame: imu_data_frame + + # if False, data are published with sensor 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 + + # Set the method used to timestamp measurements. + # Valid modes are: + # + # TIME_FROM_INTERNAL_OSC + # TIME_FROM_SYNC_PULSE_IN + # TIME_FROM_PTP_1588 + # TIME_FROM_ROS_RECEPTION + # + # (See this project's README and/or the Ouster Software Guide for more + # information). + # + timestamp_mode: TIME_FROM_INTERNAL_OSC + + # Mask-like-string used to define the data processors that should be + # activated upon startup of the driver. This will determine the topics + # that are available for client applications to consume. The defacto + # reference for these values are defined in: + # `include/ros2_ouster/processors/processor_factories.hpp` + # + # For convenience, the available data processors are: + # + # IMG - Provides 8-bit image topics suitable for ML applications encoding + # the range, ambient and intensity data from a scan + # PCL - Provides a point cloud encoding of a LiDAR scan + # IMU - Provides a data stream from the LiDARs integral IMU + # SCAN - Provides a synthesized 2D LaserScan from the 3D LiDAR data + # + # To construct a valid string for this parameter join the tokens from above + # (in any combination) with the pipe character. For example, valid strings + # include (but are not limited to): + # + # IMG|PCL + # IMG|PCL|IMU|SCAN + # PCL + # + proc_mask: IMG|PCL|IMU|SCAN \ No newline at end of file diff --git a/ouster-ros/config/legacy_params.yaml b/ouster-ros/config/os_sensor_os_cloud_params.yaml similarity index 63% rename from ouster-ros/config/legacy_params.yaml rename to ouster-ros/config/os_sensor_os_cloud_params.yaml index 33775bbc..79fe20b0 100644 --- a/ouster-ros/config/legacy_params.yaml +++ b/ouster-ros/config/os_sensor_os_cloud_params.yaml @@ -1,3 +1,7 @@ +# [Not recommended] Use this configuration only when utilizing the disjointed +# os_sensor and os_cloud configuration +# For the future and for better results consider migrating to combined node/component os_driver +# and utilize the driver_params.yaml along with it. ouster/os_sensor: ros__parameters: sensor_hostname: '' diff --git a/ouster-ros/launch/driver.launch.py b/ouster-ros/launch/driver.launch.py index af6ad6a4..ae935e33 100644 --- a/ouster-ros/launch/driver.launch.py +++ b/ouster-ros/launch/driver.launch.py @@ -39,10 +39,13 @@ def generate_launch_description(): rviz_enable = LaunchConfiguration('viz') rviz_enable_arg = DeclareLaunchArgument('viz', default_value='True') + os_driver_name = LaunchConfiguration('os_driver_name') + os_driver_name_arg = DeclareLaunchArgument('os_driver_name', default_value='os_driver') + os_driver = LifecycleNode( package='ouster_ros', executable='os_driver', - name='os_driver', + name=os_driver_name, namespace=ouster_ns, parameters=[params_file], output='screen', @@ -103,6 +106,7 @@ def generate_launch_description(): params_file_arg, ouster_ns_arg, rviz_enable_arg, + os_driver_name_arg, rviz_launch, os_driver, os_image, diff --git a/ouster-ros/launch/driver_launch.py b/ouster-ros/launch/driver_launch.py new file mode 100644 index 00000000..6072ba82 --- /dev/null +++ b/ouster-ros/launch/driver_launch.py @@ -0,0 +1,52 @@ +# Copyright 2023 Ouster, Inc. +# + +"""Launch a sensor node along with os_cloud and os_""" + +from pathlib import Path +import launch +import lifecycle_msgs.msg +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node, LifecycleNode +from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, + RegisterEventHandler, EmitEvent, LogInfo) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch.events import matches_action +from launch_ros.events.lifecycle import ChangeState +from launch_ros.event_handlers import OnStateTransition + + +def generate_launch_description(): + """ + Generate launch description for running ouster_ros components separately each + component will run in a separate process). + """ + ouster_ros_pkg_dir = get_package_share_directory('ouster_ros') + # use the community_driver_config.yaml by default + default_params_file = \ + Path(ouster_ros_pkg_dir) / 'config' / 'community_driver_config.yaml' + params_file = LaunchConfiguration('params_file') + params_file_arg = DeclareLaunchArgument('params_file', + default_value=str( + default_params_file), + description='name or path to the parameters file to use.') + + driver_launch_file_path = \ + Path(ouster_ros_pkg_dir) / 'launch' / 'driver.launch.py' + driver_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([str(driver_launch_file_path)]), + launch_arguments={ + 'params_file': params_file, + 'ouster_ns': '', + 'os_driver_name': 'ouster_driver', + 'viz': 'True', + 'rviz_config': './install/ouster_ros/share/ouster_ros/config/community_driver.rviz' + }.items() + ) + + return launch.LaunchDescription([ + params_file_arg, + driver_launch + ]) diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index c6f0f8fb..73044451 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -81,7 +81,7 @@ class OusterCloud : public OusterProcessingNodeBase { } auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); - use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME"; + use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME" || timestamp_mode_arg == "TIME_FROM_ROS_RECEPTION"; } void metadata_handler( diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 3903a0a1..aea361a6 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -87,7 +87,7 @@ class OusterDriver : public OusterSensor { } auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); - bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME"; + bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME" || timestamp_mode_arg == "TIME_FROM_ROS_RECEPTION"; imu_packet_handler = ImuPacketHandler::create_handler(info, imu_frame, use_ros_time); diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index 997372d3..af07fbb5 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -36,9 +36,11 @@ OusterSensor::OusterSensor(const rclcpp::NodeOptions& options) : OusterSensor("os_sensor", options) {} void OusterSensor::declare_parameters() { - declare_parameter("sensor_hostname"); + declare_parameter("sensor_hostname", ""); + declare_parameter("lidar_ip", ""); // community declare_parameter("metadata", ""); declare_parameter("udp_dest", ""); + declare_parameter("computer_ip", ""); // community declare_parameter("mtp_dest", ""); declare_parameter("mtp_main", false); declare_parameter("lidar_port", 0); @@ -150,9 +152,12 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_shutdown( std::string OusterSensor::get_sensor_hostname() { auto hostname = get_parameter("sensor_hostname").as_string(); if (!is_arg_set(hostname)) { - auto error_msg = "Must specify a sensor hostname"; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); - throw std::runtime_error(error_msg); + hostname = get_parameter("lidar_ip").as_string(); + if (!is_arg_set(hostname)) { + auto error_msg = "Must specify a sensor hostname"; + RCLCPP_ERROR_STREAM(get_logger(), error_msg); + throw std::runtime_error(error_msg); + } } return hostname; @@ -416,6 +421,9 @@ std::shared_ptr OusterSensor::create_sensor_client( sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { auto udp_dest = get_parameter("udp_dest").as_string(); + if (!is_arg_set(udp_dest)) + udp_dest = get_parameter("computer_ip").as_string(); + auto mtp_dest_arg = get_parameter("mtp_dest").as_string(); auto mtp_main_arg = get_parameter("mtp_main").as_bool(); auto lidar_port = get_parameter("lidar_port").as_int(); @@ -469,7 +477,8 @@ sensor::sensor_config OusterSensor::parse_config_from_ros_parameters() { if (is_arg_set(timestamp_mode_arg)) { // In case the option TIME_FROM_ROS_TIME is set then leave the // sensor timestamp_mode unmodified - if (timestamp_mode_arg == "TIME_FROM_ROS_TIME") { + if (timestamp_mode_arg == "TIME_FROM_ROS_TIME" || + timestamp_mode_arg == "TIME_FROM_ROS_RECEPTION") { RCLCPP_INFO(get_logger(), "TIME_FROM_ROS_TIME timestamp mode specified." " IMU and pointcloud messages will use ros time"); From cf8ae169f858526f78d55b593848105a7c309a63 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 5 Jun 2023 15:58:10 -0700 Subject: [PATCH 11/38] Factor out tf transforms broadcast --- ouster-ros/src/os_cloud_node.cpp | 46 +++--------- ouster-ros/src/os_driver_node.cpp | 62 +++------------- .../src/os_static_transforms_broadcaster.h | 70 +++++++++++++++++++ 3 files changed, 87 insertions(+), 91 deletions(-) create mode 100644 ouster-ros/src/os_static_transforms_broadcaster.h diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 73044451..3255140d 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -28,6 +28,7 @@ #include "imu_packet_handler.h" #include "lidar_packet_handler.h" +#include "os_static_transforms_broadcaster.h" namespace sensor = ouster::sensor; using ouster_msgs::msg::PacketMsg; @@ -38,7 +39,7 @@ class OusterCloud : public OusterProcessingNodeBase { public: OUSTER_ROS_PUBLIC explicit OusterCloud(const rclcpp::NodeOptions& options) - : OusterProcessingNodeBase("os_cloud", options), tf_bcast(this) { + : OusterProcessingNodeBase("os_cloud", options), os_tf_bcast(this) { on_init(); } @@ -56,30 +57,12 @@ class OusterCloud : public OusterProcessingNodeBase { } void declare_parameters() { - declare_parameter("sensor_frame", "os_sensor"); - declare_parameter("lidar_frame", "os_lidar"); - declare_parameter("imu_frame", "os_imu"); - declare_parameter("point_cloud_frame", "os_lidar"); + os_tf_bcast.declare_parameters(); declare_parameter("timestamp_mode", ""); } void parse_parameters() { - sensor_frame = get_parameter("sensor_frame").as_string(); - lidar_frame = get_parameter("lidar_frame").as_string(); - imu_frame = get_parameter("imu_frame").as_string(); - point_cloud_frame = get_parameter("point_cloud_frame").as_string(); - - // validate point_cloud_frame - if (point_cloud_frame.empty()) { - point_cloud_frame = lidar_frame; // for ROS1 we'd still use sensor_frame - } else if (point_cloud_frame != sensor_frame && point_cloud_frame != lidar_frame) { - RCLCPP_WARN(get_logger(), - "point_cloud_frame value needs to match the value of either sensor_frame" - " or lidar_frame but a different value was supplied, using lidar_frame's" - " value as the value for point_cloud_frame"); - point_cloud_frame = lidar_frame; - } - + os_tf_bcast.parse_parameters(); auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME" || timestamp_mode_arg == "TIME_FROM_ROS_RECEPTION"; } @@ -89,18 +72,11 @@ class OusterCloud : public OusterProcessingNodeBase { RCLCPP_INFO(get_logger(), "OusterCloud: retrieved new sensor metadata!"); info = sensor::parse_metadata(metadata_msg->data); - send_static_transforms(); + os_tf_bcast.broadcast_transforms(info); create_publishers(get_n_returns(info)); create_subscriptions(); } - void send_static_transforms() { - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( - info.lidar_to_sensor_transform, sensor_frame, lidar_frame, now())); - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( - info.imu_to_sensor_transform, sensor_frame, imu_frame, now())); - } - void create_publishers(int num_returns) { rclcpp::SensorDataQoS qos; imu_pub = create_publisher("imu", qos); @@ -115,7 +91,7 @@ class OusterCloud : public OusterProcessingNodeBase { rclcpp::SensorDataQoS qos; imu_packet_handler = ImuPacketHandler::create_handler( - info, imu_frame, use_ros_time); + info, os_tf_bcast.imu_frame_id(), use_ros_time); imu_packet_sub = create_subscription( "imu_packets", qos, [this](const PacketMsg::ConstSharedPtr msg) { @@ -123,9 +99,8 @@ class OusterCloud : public OusterProcessingNodeBase { imu_pub->publish(imu_msg); }); - bool apply_lidar_to_sensor_transform = point_cloud_frame == sensor_frame; lidar_packet_handler = LidarPacketHandler::create_handler( - info, point_cloud_frame, apply_lidar_to_sensor_transform, use_ros_time); + info, os_tf_bcast.point_cloud_frame_id(), os_tf_bcast.apply_lidar_to_sensor_transform(), use_ros_time); lidar_packet_sub = create_subscription( "lidar_packets", qos, [this](const PacketMsg::ConstSharedPtr msg) { @@ -144,12 +119,7 @@ class OusterCloud : public OusterProcessingNodeBase { rclcpp::Subscription::SharedPtr imu_packet_sub; rclcpp::Publisher::SharedPtr imu_pub; - std::string sensor_frame; - std::string imu_frame; - std::string lidar_frame; - std::string point_cloud_frame; - - tf2_ros::StaticTransformBroadcaster tf_bcast; + OusterStaticTransformsBroadcaster os_tf_bcast; bool use_ros_time; diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index aea361a6..04c928d1 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -13,12 +13,11 @@ #include "ouster_ros/os_ros.h" // clang-format on -#include - #include "os_sensor_node.h" #include "imu_packet_handler.h" #include "lidar_packet_handler.h" +#include "os_static_transforms_broadcaster.h" namespace ouster_ros { @@ -28,51 +27,14 @@ class OusterDriver : public OusterSensor { public: OUSTER_ROS_PUBLIC explicit OusterDriver(const rclcpp::NodeOptions& options) - : OusterSensor("os_driver", options), tf_bcast(this) { - declare_parameters(); - parse_parameters(); - } - - private: - void declare_parameters() { - declare_parameter("sensor_frame", "os_sensor"); - declare_parameter("lidar_frame", "os_lidar"); - declare_parameter("imu_frame", "os_imu"); - declare_parameter("point_cloud_frame", "os_lidar"); - } - - void parse_parameters() { - // TODO: avoid duplication of tf frames code - sensor_frame = get_parameter("sensor_frame").as_string(); - lidar_frame = get_parameter("lidar_frame").as_string(); - imu_frame = get_parameter("imu_frame").as_string(); - point_cloud_frame = get_parameter("point_cloud_frame").as_string(); - - // validate point_cloud_frame - if (point_cloud_frame.empty()) { - point_cloud_frame = - lidar_frame; // for ROS1 we'd still use sensor_frame - } else if (point_cloud_frame != sensor_frame && - point_cloud_frame != lidar_frame) { - RCLCPP_WARN(get_logger(), - "point_cloud_frame value needs to match the value of " - "either sensor_frame or lidar_frame but a different " - "value was supplied, using lidar_frame's value as the " - "value for point_cloud_frame"); - point_cloud_frame = lidar_frame; - } + : OusterSensor("os_driver", options), os_tf_bcast(this) { + os_tf_bcast.declare_parameters(); + os_tf_bcast.parse_parameters(); } virtual void on_metadata_updated(const sensor::sensor_info& info) override { OusterSensor::on_metadata_updated(info); - send_static_transforms(info); - } - - void send_static_transforms(const sensor::sensor_info& info) { - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( - info.lidar_to_sensor_transform, sensor_frame, lidar_frame, now())); - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( - info.imu_to_sensor_transform, sensor_frame, imu_frame, now())); + os_tf_bcast.broadcast_transforms(info); } virtual void create_publishers() override { @@ -90,11 +52,10 @@ class OusterDriver : public OusterSensor { bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME" || timestamp_mode_arg == "TIME_FROM_ROS_RECEPTION"; imu_packet_handler = - ImuPacketHandler::create_handler(info, imu_frame, use_ros_time); - bool apply_lidar_to_sensor_transform = - point_cloud_frame == sensor_frame; + ImuPacketHandler::create_handler(info, os_tf_bcast.imu_frame_id(), use_ros_time); lidar_packet_handler = LidarPacketHandler::create_handler( - info, point_cloud_frame, apply_lidar_to_sensor_transform, + info, os_tf_bcast.point_cloud_frame_id(), + os_tf_bcast.apply_lidar_to_sensor_transform(), use_ros_time); } @@ -111,12 +72,7 @@ class OusterDriver : public OusterSensor { } private: - std::string sensor_frame; - std::string imu_frame; - std::string lidar_frame; - std::string point_cloud_frame; - - tf2_ros::StaticTransformBroadcaster tf_bcast; + OusterStaticTransformsBroadcaster os_tf_bcast; std::vector::SharedPtr> lidar_pubs; diff --git a/ouster-ros/src/os_static_transforms_broadcaster.h b/ouster-ros/src/os_static_transforms_broadcaster.h new file mode 100644 index 00000000..1b368b1e --- /dev/null +++ b/ouster-ros/src/os_static_transforms_broadcaster.h @@ -0,0 +1,70 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file os_tf_publisher.h + * @brief ... + */ + +#pragma once + +#include + +namespace ouster_ros { + +template +class OusterStaticTransformsBroadcaster { + public: + OusterStaticTransformsBroadcaster(NodeT* parent) : node(parent), tf_bcast(parent){ + } + + void declare_parameters() { + node->declare_parameter("sensor_frame", "os_sensor"); + node->declare_parameter("lidar_frame", "os_lidar"); + node->declare_parameter("imu_frame", "os_imu"); + node->declare_parameter("point_cloud_frame", "os_lidar"); + } + + void parse_parameters() { + sensor_frame = node->get_parameter("sensor_frame").as_string(); + lidar_frame = node->get_parameter("lidar_frame").as_string(); + imu_frame = node->get_parameter("imu_frame").as_string(); + point_cloud_frame = node->get_parameter("point_cloud_frame").as_string(); + + // validate point_cloud_frame + if (point_cloud_frame.empty()) { + point_cloud_frame = + lidar_frame; // for ROS1 we'd still use sensor_frame + } else if (point_cloud_frame != sensor_frame && + point_cloud_frame != lidar_frame) { + RCLCPP_WARN(node->get_logger(), + "point_cloud_frame value needs to match the value of " + "either sensor_frame or lidar_frame but a different " + "value was supplied, using lidar_frame's value as the " + "value for point_cloud_frame"); + point_cloud_frame = lidar_frame; + } + } + + void broadcast_transforms(const sensor::sensor_info& info) { + auto now = node->get_clock()->now(); + tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + info.lidar_to_sensor_transform, sensor_frame, lidar_frame, now)); + tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + info.imu_to_sensor_transform, sensor_frame, imu_frame, now)); + } + + const std::string& imu_frame_id() const { return imu_frame; } + const std::string& point_cloud_frame_id() const { return point_cloud_frame; } + bool apply_lidar_to_sensor_transform() const { return point_cloud_frame == sensor_frame; } + + private: + NodeT* node; + tf2_ros::StaticTransformBroadcaster tf_bcast; + std::string sensor_frame; + std::string imu_frame; + std::string lidar_frame; + std::string point_cloud_frame; +}; + +} // namespace ouster_ros From bd01d654c5cefe76590398a63e8431b50450494e Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 5 Jun 2023 16:59:00 -0700 Subject: [PATCH 12/38] Formatting imu and lidar packet handlers --- ouster-ros/src/imu_packet_handler.h | 10 ++-- ouster-ros/src/lidar_packet_handler.h | 82 +++++++++++++++------------ 2 files changed, 51 insertions(+), 41 deletions(-) diff --git a/ouster-ros/src/imu_packet_handler.h b/ouster-ros/src/imu_packet_handler.h index 8c0bd235..15b02b12 100644 --- a/ouster-ros/src/imu_packet_handler.h +++ b/ouster-ros/src/imu_packet_handler.h @@ -15,14 +15,14 @@ // clang-format on class ImuPacketHandler { - public: + public: using HandlerOutput = sensor_msgs::msg::Imu; using HandlerType = std::function; public: - static HandlerType create_handler( - const ouster::sensor::sensor_info& info, const std::string& frame, - bool use_ros_time) { + static HandlerType create_handler(const ouster::sensor::sensor_info& info, + const std::string& frame, + bool use_ros_time) { const auto& pf = ouster::sensor::get_format(info); using Timestamper = std::function; // clang-format off @@ -34,7 +34,7 @@ class ImuPacketHandler { // clang-format on return [&pf, &frame, timestamper](const uint8_t* imu_buf) { return ouster_ros::packet_to_imu_msg(pf, timestamper(imu_buf), - frame, imu_buf); + frame, imu_buf); }; } }; \ No newline at end of file diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h index d980b5ed..91839c2e 100644 --- a/ouster-ros/src/lidar_packet_handler.h +++ b/ouster-ros/src/lidar_packet_handler.h @@ -55,19 +55,19 @@ uint64_t ulround(T value) { namespace sensor = ouster::sensor; - class LidarPacketHandler { using LidarPacketAccumlator = std::function; - - public: - using HandlerOutput = std::vector>; + public: + using HandlerOutput = + std::vector>; using HandlerType = std::function; public: - LidarPacketHandler(const ouster::sensor::sensor_info& info, const std::string& frame, - bool apply_lidar_to_sensor_transform, bool use_ros_time) : - ref_frame(frame) { + LidarPacketHandler(const ouster::sensor::sensor_info& info, + const std::string& frame, + bool apply_lidar_to_sensor_transform, bool use_ros_time) + : ref_frame(frame) { create_lidarscan_objects(info, apply_lidar_to_sensor_transform); compute_scan_ts = [this](const auto& ts_v) { return compute_scan_ts_0(ts_v); @@ -76,11 +76,14 @@ class LidarPacketHandler { scan_col_ts_spacing_ns = compute_scan_col_ts_spacing_ns(info.mode); const sensor::packet_format& pf = sensor::get_format(info); - lidar_packet_accumlator = use_ros_time ? - LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) { - return lidar_handler_ros_time(pf, lidar_buf); }} : - LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) { - return lidar_handler_sensor_time(pf, lidar_buf); }}; + lidar_packet_accumlator = + use_ros_time + ? LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) { + return lidar_handler_ros_time(pf, lidar_buf); + }} + : LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) { + return lidar_handler_sensor_time(pf, lidar_buf); + }}; } LidarPacketHandler(const LidarPacketHandler&) = delete; @@ -88,36 +91,40 @@ class LidarPacketHandler { ~LidarPacketHandler() = default; public: - static HandlerType create_handler( - const ouster::sensor::sensor_info& info, const std::string& frame, bool apply_lidar_to_sensor_transform, bool use_ros_time) { - auto handler = std::make_shared(info, frame, apply_lidar_to_sensor_transform, use_ros_time); + static HandlerType create_handler(const ouster::sensor::sensor_info& info, + const std::string& frame, + bool apply_lidar_to_sensor_transform, + bool use_ros_time) { + auto handler = std::make_shared( + info, frame, apply_lidar_to_sensor_transform, use_ros_time); return [handler](const uint8_t* lidar_buf) { - return handler->lidar_packet_accumlator(lidar_buf) ? handler->pc_msgs : HandlerOutput{}; + if (handler->lidar_packet_accumlator(lidar_buf)) { + return handler->pc_msgs + } else { + return HandlerOutput{}; + } }; } static int num_returns(const ouster::sensor::sensor_info& info) { using ouster::sensor::UDPProfileLidar; return info.format.udp_profile_lidar == - UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL - ? 2 - : 1; + UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL + ? 2 + : 1; } - private: - void create_lidarscan_objects(const ouster::sensor::sensor_info& info, bool apply_lidar_to_sensor_transform) { + private: + void create_lidarscan_objects(const ouster::sensor::sensor_info& info, + bool apply_lidar_to_sensor_transform) { ouster::mat4d additional_transform = - apply_lidar_to_sensor_transform ? - info.lidar_to_sensor_transform : - ouster::mat4d::Identity(); + apply_lidar_to_sensor_transform ? info.lidar_to_sensor_transform + : ouster::mat4d::Identity(); auto xyz_lut = ouster::make_xyz_lut( - info.format.columns_per_frame, - info.format.pixels_per_column, - ouster::sensor::range_unit, - additional_transform, - ouster::mat4d::Identity(), - info.beam_azimuth_angles, - info.beam_altitude_angles); + info.format.columns_per_frame, info.format.pixels_per_column, + ouster::sensor::range_unit, additional_transform, + ouster::mat4d::Identity(), info.beam_azimuth_angles, + info.beam_altitude_angles); // The ouster_ros drive currently only uses single precision when it // produces the point cloud. So it isn't of a benefit to compute point // cloud xyz coordinates using double precision (for the time being). @@ -130,7 +137,8 @@ class LidarPacketHandler { lidar_scan = std::make_unique( W, H, info.format.udp_profile_lidar); cloud = ouster_ros::Cloud{W, H}; - pc_msgs.resize(num_returns(info), std::make_shared()); + pc_msgs.resize(num_returns(info), + std::make_shared()); } void pcl_toROSMsg(const ouster_ros::Cloud& pcl_cloud, @@ -224,7 +232,8 @@ class LidarPacketHandler { return scan_ns; } - uint16_t packet_col_index(const sensor::packet_format& pf, const uint8_t* lidar_buf) { + uint16_t packet_col_index(const sensor::packet_format& pf, + const uint8_t* lidar_buf) { return pf.col_measurement_id(pf.nth_col(0, lidar_buf)); } @@ -238,7 +247,8 @@ class LidarPacketHandler { return current_time - delta_time; } - bool lidar_handler_sensor_time(const sensor::packet_format&, const uint8_t* lidar_buf) { + bool lidar_handler_sensor_time(const sensor::packet_format&, + const uint8_t* lidar_buf) { if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; auto scan_ts = compute_scan_ts(lidar_scan->timestamp()); convert_scan_to_pointcloud(scan_ts, rclcpp::Time(scan_ts)); @@ -269,7 +279,7 @@ class LidarPacketHandler { return one_sec_in_ns / (scan_width * scan_frequency); } - private: + private: std::string ref_frame; ouster::PointsF lut_direction; @@ -295,7 +305,7 @@ class LidarPacketHandler { std::function&)> compute_scan_ts; - public: + public: HandlerOutput pc_msgs; LidarPacketAccumlator lidar_packet_accumlator; }; \ No newline at end of file From 3ec9f41ac05f50cdc05a344acd228c22569a464a Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 5 Jun 2023 17:05:40 -0700 Subject: [PATCH 13/38] Fix build issue --- ouster-ros/src/lidar_packet_handler.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h index 91839c2e..74a5d3f1 100644 --- a/ouster-ros/src/lidar_packet_handler.h +++ b/ouster-ros/src/lidar_packet_handler.h @@ -99,7 +99,7 @@ class LidarPacketHandler { info, frame, apply_lidar_to_sensor_transform, use_ros_time); return [handler](const uint8_t* lidar_buf) { if (handler->lidar_packet_accumlator(lidar_buf)) { - return handler->pc_msgs + return handler->pc_msgs; } else { return HandlerOutput{}; } @@ -122,8 +122,8 @@ class LidarPacketHandler { : ouster::mat4d::Identity(); auto xyz_lut = ouster::make_xyz_lut( info.format.columns_per_frame, info.format.pixels_per_column, - ouster::sensor::range_unit, additional_transform, - ouster::mat4d::Identity(), info.beam_azimuth_angles, + ouster::sensor::range_unit, info.beam_to_lidar_transform, + additional_transform, info.beam_azimuth_angles, info.beam_altitude_angles); // The ouster_ros drive currently only uses single precision when it // produces the point cloud. So it isn't of a benefit to compute point From 4c45198d6710174a9c35121c96040097ad56e63e Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Tue, 6 Jun 2023 12:10:20 -0700 Subject: [PATCH 14/38] Incorporate LaserScan message composition --- ouster-ros/include/ouster_ros/os_ros.h | 20 +++++++++ ouster-ros/src/os_ros.cpp | 56 +++++++++++++++++++++----- 2 files changed, 67 insertions(+), 9 deletions(-) diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h index 617932fa..4e588cb8 100644 --- a/ouster-ros/include/ouster_ros/os_ros.h +++ b/ouster-ros/include/ouster_ros/os_ros.h @@ -20,6 +20,7 @@ #include #include +#include "sensor_msgs/msg/laser_scan.hpp" #include #include @@ -165,4 +166,23 @@ sensor_msgs::msg::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, geometry_msgs::msg::TransformStamped transform_to_tf_msg( const ouster::mat4d& mat, const std::string& frame, const std::string& child_frame, rclcpp::Time timestamp); + + +/** + * Convert transformation matrix return by sensor to ROS transform + * @param[in] ls lidar scan object + * @param[in] timestamp value to set as the timestamp of the generated + * @param[in] frame the parent frame of the generated laser scan message + * @param[in] lidar_mode lidar mode (width x frequency) + * @param[in] ring selected ring to be published + * @return ROS message suitable for publishing as a LaserScan + */ +sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg( + const ouster::LidarScan& ls, + const rclcpp::Time& timestamp, + const std::string &frame, + const ouster::sensor::lidar_mode lidar_mode, + const uint16_t ring, + const int return_index); + } // namespace ouster_ros diff --git a/ouster-ros/src/os_ros.cpp b/ouster-ros/src/os_ros.cpp index f756aa01..9a26c9e4 100644 --- a/ouster-ros/src/os_ros.cpp +++ b/ouster-ros/src/os_ros.cpp @@ -28,22 +28,22 @@ namespace ouster_ros { bool is_legacy_lidar_profile(const sensor::sensor_info& info) { using sensor::UDPProfileLidar; - return info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY; + return info.format.udp_profile_lidar == + UDPProfileLidar::PROFILE_LIDAR_LEGACY; } int get_n_returns(const sensor::sensor_info& info) { using sensor::UDPProfileLidar; return info.format.udp_profile_lidar == - UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL - ? 2 - : 1; + UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL + ? 2 + : 1; } std::string topic_for_return(const std::string& base, int idx) { return idx == 0 ? base : base + std::to_string(idx + 1); } - bool read_imu_packet(const sensor::client& cli, PacketMsg& pm, const sensor::packet_format& pf) { pm.buf.resize(pf.imu_packet_size + 1); @@ -57,9 +57,9 @@ bool read_lidar_packet(const sensor::client& cli, PacketMsg& pm, } sensor_msgs::msg::Imu packet_to_imu_msg(const ouster::sensor::packet_format& pf, - const rclcpp::Time& timestamp, - const std::string& frame, - const uint8_t* buf) { + const rclcpp::Time& timestamp, + const std::string& frame, + const uint8_t* buf) { sensor_msgs::msg::Imu m; m.header.stamp = timestamp; m.header.frame_id = frame; @@ -96,7 +96,6 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const PacketMsg& pm, const rclcpp::Time& timestamp, const std::string& frame, const sensor::packet_format& pf) { - return packet_to_imu_msg(pf, timestamp, frame, pm.buf.data()); } @@ -304,4 +303,43 @@ geometry_msgs::msg::TransformStamped transform_to_tf_msg( return msg; } + +sensor_msgs::msg::LaserScan lidar_scan_to_laser_scan_msg( + const ouster::LidarScan& ls, const rclcpp::Time& timestamp, + const std::string& frame, const ouster::sensor::lidar_mode ld_mode, + const uint16_t ring, const int return_index) { + sensor_msgs::msg::LaserScan msg; + msg.header.stamp = timestamp; + msg.header.frame_id = frame; + msg.angle_min = -M_PI; // TODO: configure to match the actual scan window + msg.angle_max = M_PI; // TODO: configure to match the actual scan window + msg.range_min = 0.1f; // TODO: fill per product type + msg.range_max = 120.0f; // TODO: fill per product type + + const auto scan_width = sensor::n_cols_of_lidar_mode(ld_mode); + const auto scan_frequency = sensor::frequency_of_lidar_mode(ld_mode); + msg.scan_time = 1.0f / scan_frequency; + msg.time_increment = 1.0f / (scan_width * scan_frequency); + msg.angle_increment = 2 * M_PI / scan_width; + + auto which_range = return_index == 0 ? sensor::ChanField::RANGE + : sensor::ChanField::RANGE2; + ouster::img_t range = ls.field(which_range); + auto which_signal = return_index == 0 ? sensor::ChanField::SIGNAL + : sensor::ChanField::SIGNAL2; + ouster::img_t signal = + get_or_fill_zero(which_signal, ls); + const auto rg = range.data(); + const auto sg = signal.data(); + msg.ranges.resize(ls.w); + msg.intensities.resize(ls.w); + int idx = ls.w * ring + ls.w; + for (int i = 0; idx-- > ls.w * ring; ++i) { + msg.ranges[i] = rg[idx] * ouster::sensor::range_unit; + msg.intensities[i] = static_cast(sg[idx]); + } + + return msg; +} + } // namespace ouster_ros From 552581d388a0d9313ecad87f32d420fe20c213a0 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Tue, 6 Jun 2023 19:53:35 -0700 Subject: [PATCH 15/38] Refactor a bit and add the ability to process and publish point clouds and laser scans --- ouster-ros/src/laser_scan_processor.h | 64 ++++++++++++ ouster-ros/src/lidar_packet_handler.h | 129 +++++++++---------------- ouster-ros/src/os_cloud_node.cpp | 37 ++++--- ouster-ros/src/os_driver_node.cpp | 44 +++++++-- ouster-ros/src/point_cloud_processor.h | 101 +++++++++++++++++++ 5 files changed, 266 insertions(+), 109 deletions(-) create mode 100644 ouster-ros/src/laser_scan_processor.h create mode 100644 ouster-ros/src/point_cloud_processor.h diff --git a/ouster-ros/src/laser_scan_processor.h b/ouster-ros/src/laser_scan_processor.h new file mode 100644 index 00000000..7b2b7a93 --- /dev/null +++ b/ouster-ros/src/laser_scan_processor.h @@ -0,0 +1,64 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file point_cloud_producer.h + * @brief takes in a lidar scan object and produces a PointCloud2 message + */ + +#pragma once + +// prevent clang-format from altering the location of "ouster_ros/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" +// clang-format on + +using ouster_ros::get_n_returns; +using ouster_ros::lidar_scan_to_laser_scan_msg; + +class LaserScanProcessor { + public: + using OutputType = std::vector>; + using PostProcessingFunc = std::function; + + public: + LaserScanProcessor(const ouster::sensor::sensor_info& info, + const std::string& frame_id, uint16_t ring, PostProcessingFunc func) : + frame(frame_id), ld_mode(info.mode), ring_(ring), + scan_msgs(get_n_returns(info), std::make_shared()), + func_(func) { + } + + private: + void process(const ouster::LidarScan& lidar_scan, + uint64_t scan_ts, const rclcpp::Time& msg_ts) { + for (int i = 0; i < static_cast(scan_msgs.size()); ++i) { + *scan_msgs[i] = lidar_scan_to_laser_scan_msg( + lidar_scan, msg_ts, frame, ld_mode, ring_, i); + } + + if (func_) + func_(scan_msgs); + } + + public: + static LidarScanProcessor create(const ouster::sensor::sensor_info& info, + const std::string& frame, uint16_t ring, PostProcessingFunc func) { + + auto handler = + std::make_shared(info, frame, ring, func); + + return [handler](const ouster::LidarScan& lidar_scan, + uint64_t scan_ts, const rclcpp::Time& msg_ts) { + handler->process(lidar_scan, scan_ts, msg_ts); + }; + } + + private: + std::string frame; + sensor::lidar_mode ld_mode; + uint16_t ring_; + OutputType scan_msgs; + PostProcessingFunc func_; +}; \ No newline at end of file diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h index 74a5d3f1..b31be574 100644 --- a/ouster-ros/src/lidar_packet_handler.h +++ b/ouster-ros/src/lidar_packet_handler.h @@ -55,25 +55,32 @@ uint64_t ulround(T value) { namespace sensor = ouster::sensor; +using LidarScanProcessor = + std::function; + class LidarPacketHandler { using LidarPacketAccumlator = std::function; public: - using HandlerOutput = - std::vector>; - using HandlerType = std::function; + using HandlerOutput = ouster::LidarScan; + + using HandlerType = std::function; public: LidarPacketHandler(const ouster::sensor::sensor_info& info, - const std::string& frame, - bool apply_lidar_to_sensor_transform, bool use_ros_time) - : ref_frame(frame) { - create_lidarscan_objects(info, apply_lidar_to_sensor_transform); + bool use_ros_time) { + // initialize lidar_scan processor and buffer + scan_batcher = std::make_unique(info); + lidar_scan = std::make_unique( + info.format.columns_per_frame, + info.format.pixels_per_column, + info.format.udp_profile_lidar); + + // initalize time handlers + scan_col_ts_spacing_ns = compute_scan_col_ts_spacing_ns(info.mode); compute_scan_ts = [this](const auto& ts_v) { return compute_scan_ts_0(ts_v); }; - - scan_col_ts_spacing_ns = compute_scan_col_ts_spacing_ns(info.mode); const sensor::packet_format& pf = sensor::get_format(info); lidar_packet_accumlator = @@ -90,74 +97,34 @@ class LidarPacketHandler { LidarPacketHandler& operator=(const LidarPacketHandler&) = delete; ~LidarPacketHandler() = default; + void register_lidar_scan_handler(LidarScanProcessor handler) { + lidar_scan_handlers.push_back(handler); + } + + void clear_registered_lidar_scan_handlers() { + lidar_scan_handlers.clear(); + } + public: static HandlerType create_handler(const ouster::sensor::sensor_info& info, - const std::string& frame, - bool apply_lidar_to_sensor_transform, - bool use_ros_time) { + bool use_ros_time, + const std::vector& handlers) { auto handler = std::make_shared( - info, frame, apply_lidar_to_sensor_transform, use_ros_time); + info, use_ros_time); + + handler->lidar_scan_handlers = handlers; + return [handler](const uint8_t* lidar_buf) { if (handler->lidar_packet_accumlator(lidar_buf)) { - return handler->pc_msgs; - } else { - return HandlerOutput{}; + for (auto h : handler->lidar_scan_handlers) { + h(*handler->lidar_scan, + handler->lidar_scan_estimated_ts, + handler->lidar_scan_estimated_msg_ts); + } } }; } - static int num_returns(const ouster::sensor::sensor_info& info) { - using ouster::sensor::UDPProfileLidar; - return info.format.udp_profile_lidar == - UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL - ? 2 - : 1; - } - - private: - void create_lidarscan_objects(const ouster::sensor::sensor_info& info, - bool apply_lidar_to_sensor_transform) { - ouster::mat4d additional_transform = - apply_lidar_to_sensor_transform ? info.lidar_to_sensor_transform - : ouster::mat4d::Identity(); - auto xyz_lut = ouster::make_xyz_lut( - info.format.columns_per_frame, info.format.pixels_per_column, - ouster::sensor::range_unit, info.beam_to_lidar_transform, - additional_transform, info.beam_azimuth_angles, - info.beam_altitude_angles); - // The ouster_ros drive currently only uses single precision when it - // produces the point cloud. So it isn't of a benefit to compute point - // cloud xyz coordinates using double precision (for the time being). - lut_direction = xyz_lut.direction.cast(); - lut_offset = xyz_lut.offset.cast(); - points = ouster::PointsF(lut_direction.rows(), lut_offset.cols()); - scan_batcher = std::make_unique(info); - uint32_t H = info.format.pixels_per_column; - uint32_t W = info.format.columns_per_frame; - lidar_scan = std::make_unique( - W, H, info.format.udp_profile_lidar); - cloud = ouster_ros::Cloud{W, H}; - pc_msgs.resize(num_returns(info), - std::make_shared()); - } - - void pcl_toROSMsg(const ouster_ros::Cloud& pcl_cloud, - sensor_msgs::msg::PointCloud2& cloud) { - // TODO: remove the staging step in the future - pcl::toPCLPointCloud2(pcl_cloud, staging_pcl_pc2); - pcl_conversions::moveFromPCL(staging_pcl_pc2, cloud); - } - - void convert_scan_to_pointcloud(uint64_t scan_ts, - const rclcpp::Time& msg_ts) { - for (int i = 0; i < static_cast(pc_msgs.size()); ++i) { - scan_to_cloud_f(points, lut_direction, lut_offset, scan_ts, - *lidar_scan, cloud, i); - pcl_toROSMsg(cloud, *pc_msgs[i]); - pc_msgs[i]->header.stamp = msg_ts; - pc_msgs[i]->header.frame_id = ref_frame; - } - } // time interpolation methods uint64_t impute_value(int last_scan_last_nonzero_idx, @@ -250,8 +217,8 @@ class LidarPacketHandler { bool lidar_handler_sensor_time(const sensor::packet_format&, const uint8_t* lidar_buf) { if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; - auto scan_ts = compute_scan_ts(lidar_scan->timestamp()); - convert_scan_to_pointcloud(scan_ts, rclcpp::Time(scan_ts)); + lidar_scan_estimated_ts = compute_scan_ts(lidar_scan->timestamp()); + lidar_scan_estimated_msg_ts = rclcpp::Time(lidar_scan_estimated_ts); return true; } @@ -264,8 +231,8 @@ class LidarPacketHandler { lidar_handler_ros_time_frame_ts_initialized = true; } if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false; - auto scan_ts = compute_scan_ts(lidar_scan->timestamp()); - convert_scan_to_pointcloud(scan_ts, lidar_handler_ros_time_frame_ts); + lidar_scan_estimated_ts = compute_scan_ts(lidar_scan->timestamp()); + lidar_scan_estimated_msg_ts = lidar_handler_ros_time_frame_ts; // set time for next point cloud msg lidar_handler_ros_time_frame_ts = extrapolate_frame_ts(pf, lidar_buf, packet_receive_time); @@ -280,18 +247,10 @@ class LidarPacketHandler { } private: - std::string ref_frame; - - ouster::PointsF lut_direction; - ouster::PointsF lut_offset; - ouster::PointsF points; - std::unique_ptr lidar_scan; - ouster_ros::Cloud cloud; std::unique_ptr scan_batcher; - - // a buffer used for staging during the conversion - // from a PCL point cloud to a ros point cloud message - pcl::PCLPointCloud2 staging_pcl_pc2; + std::unique_ptr lidar_scan; + uint64_t lidar_scan_estimated_ts; + rclcpp::Time lidar_scan_estimated_msg_ts; bool lidar_handler_ros_time_frame_ts_initialized = false; rclcpp::Time lidar_handler_ros_time_frame_ts; @@ -305,7 +264,7 @@ class LidarPacketHandler { std::function&)> compute_scan_ts; - public: - HandlerOutput pc_msgs; + std::vector lidar_scan_handlers; + LidarPacketAccumlator lidar_packet_accumlator; }; \ No newline at end of file diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 3255140d..f811ce23 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -85,6 +85,12 @@ class OusterCloud : public OusterProcessingNodeBase { lidar_pubs[i] = create_publisher( topic_for_return("points", i), qos); } + + scan_pubs.resize(num_returns); + for (int i = 0; i < num_returns; i++) { + scan_pubs[i] = create_publisher( + topic_for_return("scan", i), qos); + } } void create_subscriptions() { @@ -99,32 +105,33 @@ class OusterCloud : public OusterProcessingNodeBase { imu_pub->publish(imu_msg); }); - lidar_packet_handler = LidarPacketHandler::create_handler( - info, os_tf_bcast.point_cloud_frame_id(), os_tf_bcast.apply_lidar_to_sensor_transform(), use_ros_time); - lidar_packet_sub = create_subscription( - "lidar_packets", qos, - [this](const PacketMsg::ConstSharedPtr msg) { - auto point_cloud_msgs = lidar_packet_handler(msg->buf.data()); - for (size_t i = 0; i < point_cloud_msgs.size(); ++i) { - lidar_pubs[i]->publish(*point_cloud_msgs[i]); - } - }); + // lidar_packet_handler = LidarPacketHandler::create_handler( + // info, use_ros_time); + // lidar_packet_sub = create_subscription( + // "lidar_packets", qos, + // [this](const PacketMsg::ConstSharedPtr msg) { + // auto point_cloud_msgs = lidar_packet_handler(msg->buf.data()); + // for (size_t i = 0; i < point_cloud_msgs.size(); ++i) { + // lidar_pubs[i]->publish(*point_cloud_msgs[i]); + // } + // }); } private: - rclcpp::Subscription::SharedPtr lidar_packet_sub; - std::vector::SharedPtr> - lidar_pubs; rclcpp::Subscription::SharedPtr imu_packet_sub; rclcpp::Publisher::SharedPtr imu_pub; - OusterStaticTransformsBroadcaster os_tf_bcast; + rclcpp::Subscription::SharedPtr lidar_packet_sub; + std::vector::SharedPtr> + lidar_pubs; + std::vector::SharedPtr> + scan_pubs; + OusterStaticTransformsBroadcaster os_tf_bcast; bool use_ros_time; ImuPacketHandler::HandlerType imu_packet_handler; - LidarPacketHandler::HandlerType lidar_packet_handler; }; diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 04c928d1..80619383 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -15,9 +15,11 @@ #include "os_sensor_node.h" +#include "os_static_transforms_broadcaster.h" #include "imu_packet_handler.h" #include "lidar_packet_handler.h" -#include "os_static_transforms_broadcaster.h" +#include "point_cloud_processor.h" +#include "laser_scan_processor.h" namespace ouster_ros { @@ -48,22 +50,44 @@ class OusterDriver : public OusterSensor { topic_for_return("points", i), qos); } + scan_pubs.resize(num_returns); + for (int i = 0; i < num_returns; i++) { + scan_pubs[i] = create_publisher( + topic_for_return("scan", i), qos); + } + auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME" || timestamp_mode_arg == "TIME_FROM_ROS_RECEPTION"; imu_packet_handler = ImuPacketHandler::create_handler(info, os_tf_bcast.imu_frame_id(), use_ros_time); + + std::vector processors; + auto process_pc = true; + if (process_pc) + processors.push_back(PointCloudProcessor::create( + info, os_tf_bcast.point_cloud_frame_id(), + os_tf_bcast.apply_lidar_to_sensor_transform(), + [this](PointCloudProcessor::OutputType point_cloud_msg) { + for (size_t i = 0; i < point_cloud_msg.size(); ++i) + lidar_pubs[i]->publish(*point_cloud_msg[i]); + })); + + auto process_scan = true; + if (process_scan) + processors.push_back(LaserScanProcessor::create( + info, os_tf_bcast.point_cloud_frame_id(), // TODO: should we have different frame for the laser scan than point cloud??? + 0, [this](LaserScanProcessor::OutputType laser_scan_msg) { + for (size_t i = 0; i < laser_scan_msg.size(); ++i) + scan_pubs[i]->publish(*laser_scan_msg[i]); + })); + lidar_packet_handler = LidarPacketHandler::create_handler( - info, os_tf_bcast.point_cloud_frame_id(), - os_tf_bcast.apply_lidar_to_sensor_transform(), - use_ros_time); + info, use_ros_time, processors); } virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override { - auto point_cloud_msgs = lidar_packet_handler(raw_lidar_packet); - for (size_t i = 0; i < point_cloud_msgs.size(); ++i) { - lidar_pubs[i]->publish(*point_cloud_msgs[i]); - } + lidar_packet_handler(raw_lidar_packet); } virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet) override { @@ -74,9 +98,11 @@ class OusterDriver : public OusterSensor { private: OusterStaticTransformsBroadcaster os_tf_bcast; + rclcpp::Publisher::SharedPtr imu_pub; std::vector::SharedPtr> lidar_pubs; - rclcpp::Publisher::SharedPtr imu_pub; + std::vector::SharedPtr> + scan_pubs; ImuPacketHandler::HandlerType imu_packet_handler; LidarPacketHandler::HandlerType lidar_packet_handler; }; diff --git a/ouster-ros/src/point_cloud_processor.h b/ouster-ros/src/point_cloud_processor.h new file mode 100644 index 00000000..32d89639 --- /dev/null +++ b/ouster-ros/src/point_cloud_processor.h @@ -0,0 +1,101 @@ +/** + * Copyright (c) 2018-2023, Ouster, Inc. + * All rights reserved. + * + * @file point_cloud_producer.h + * @brief takes in a lidar scan object and produces a PointCloud2 message + */ + +#pragma once + +// prevent clang-format from altering the location of "ouster_ros/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" +// clang-format on + +#include "lidar_packet_handler.h" + +using ouster_ros::get_n_returns; + +class PointCloudProcessor { + public: + using OutputType = std::vector>; + using PostProcessingFunc = std::function; + + public: + PointCloudProcessor(const ouster::sensor::sensor_info& info, + const std::string& frame_id, bool apply_lidar_to_sensor_transform, PostProcessingFunc func) + : frame(frame_id), cloud{info.format.columns_per_frame, info.format.pixels_per_column}, + pc_msgs(get_n_returns(info), std::make_shared()), + func_(func) { + + ouster::mat4d additional_transform = + apply_lidar_to_sensor_transform ? info.lidar_to_sensor_transform + : ouster::mat4d::Identity(); + auto xyz_lut = ouster::make_xyz_lut( + info.format.columns_per_frame, info.format.pixels_per_column, + ouster::sensor::range_unit, info.beam_to_lidar_transform, + additional_transform, info.beam_azimuth_angles, + info.beam_altitude_angles); + // The ouster_ros drive currently only uses single precision when it + // produces the point cloud. So it isn't of a benefit to compute point + // cloud xyz coordinates using double precision (for the time being). + lut_direction = xyz_lut.direction.cast(); + lut_offset = xyz_lut.offset.cast(); + points = ouster::PointsF(lut_direction.rows(), lut_offset.cols()); + } + + private: + void pcl_toROSMsg(const ouster_ros::Cloud& pcl_cloud, + sensor_msgs::msg::PointCloud2& cloud) { + // TODO: remove the staging step in the future + pcl::toPCLPointCloud2(pcl_cloud, staging_pcl_pc2); + pcl_conversions::moveFromPCL(staging_pcl_pc2, cloud); + } + + void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts, const rclcpp::Time& msg_ts) { + for (int i = 0; i < static_cast(pc_msgs.size()); ++i) { + ouster_ros::scan_to_cloud_f( + points, + lut_direction, + lut_offset, scan_ts, + lidar_scan, cloud, i); + pcl_toROSMsg(cloud, *pc_msgs[i]); + pc_msgs[i]->header.stamp = msg_ts; + pc_msgs[i]->header.frame_id = frame; + } + + if (func_) + func_(pc_msgs); + } + + public: + static LidarScanProcessor create(const ouster::sensor::sensor_info& info, + const std::string& frame, bool apply_lidar_to_sensor_transform, + PostProcessingFunc func) { + + auto handler = std::make_shared( + info, frame, apply_lidar_to_sensor_transform, func); + + return [handler](const ouster::LidarScan& lidar_scan, uint64_t scan_ts, const rclcpp::Time& msg_ts) { + handler->process(lidar_scan, scan_ts, msg_ts); + }; + } + + private: + // a buffer used for staging during the conversion + // from a PCL point cloud to a ros point cloud message + pcl::PCLPointCloud2 staging_pcl_pc2; + + std::string frame; + + ouster::PointsF lut_direction; + ouster::PointsF lut_offset; + ouster::PointsF points; + ouster_ros::Cloud cloud; + + OutputType pc_msgs; + + PostProcessingFunc func_; +}; \ No newline at end of file From 3e98683c64ab6a612f1b80a7d8ef71090beafde6 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Tue, 6 Jun 2023 20:01:58 -0700 Subject: [PATCH 16/38] Restor os_cloud_node ability to process point clouds --- ouster-ros/src/os_cloud_node.cpp | 42 +++++++++++++++++++++++--------- 1 file changed, 31 insertions(+), 11 deletions(-) diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index f811ce23..fa9da729 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -26,9 +26,11 @@ #include "ouster_ros/os_processing_node_base.h" #include "ouster_ros/visibility_control.h" +#include "os_static_transforms_broadcaster.h" #include "imu_packet_handler.h" #include "lidar_packet_handler.h" -#include "os_static_transforms_broadcaster.h" +#include "point_cloud_processor.h" +#include "laser_scan_processor.h" namespace sensor = ouster::sensor; using ouster_msgs::msg::PacketMsg; @@ -105,16 +107,34 @@ class OusterCloud : public OusterProcessingNodeBase { imu_pub->publish(imu_msg); }); - // lidar_packet_handler = LidarPacketHandler::create_handler( - // info, use_ros_time); - // lidar_packet_sub = create_subscription( - // "lidar_packets", qos, - // [this](const PacketMsg::ConstSharedPtr msg) { - // auto point_cloud_msgs = lidar_packet_handler(msg->buf.data()); - // for (size_t i = 0; i < point_cloud_msgs.size(); ++i) { - // lidar_pubs[i]->publish(*point_cloud_msgs[i]); - // } - // }); + std::vector processors; + auto process_pc = true; + if (process_pc) + processors.push_back(PointCloudProcessor::create( + info, os_tf_bcast.point_cloud_frame_id(), + os_tf_bcast.apply_lidar_to_sensor_transform(), + [this](PointCloudProcessor::OutputType point_cloud_msg) { + for (size_t i = 0; i < point_cloud_msg.size(); ++i) + lidar_pubs[i]->publish(*point_cloud_msg[i]); + })); + + auto process_scan = true; + if (process_scan) + processors.push_back(LaserScanProcessor::create( + info, os_tf_bcast.point_cloud_frame_id(), // TODO: should we have different frame for the laser scan than point cloud??? + 0, [this](LaserScanProcessor::OutputType laser_scan_msg) { + for (size_t i = 0; i < laser_scan_msg.size(); ++i) + scan_pubs[i]->publish(*laser_scan_msg[i]); + })); + + lidar_packet_handler = LidarPacketHandler::create_handler( + info, use_ros_time, processors); + + lidar_packet_sub = create_subscription( + "lidar_packets", qos, + [this](const PacketMsg::ConstSharedPtr msg) { + lidar_packet_handler(msg->buf.data()); + }); } private: From cdb032f7c0d95558b59e5fe6142e572387f68bb4 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 7 Jun 2023 09:09:38 -0700 Subject: [PATCH 17/38] Parse proc_mask and hook to launch files and config --- ouster-ros/config/driver_params.yaml | 1 + .../config/os_sensor_os_cloud_params.yaml | 1 + ouster-ros/src/lidar_packet_handler.h | 17 +++++++++++++++++ ouster-ros/src/os_cloud_node.cpp | 9 +++++---- ouster-ros/src/os_driver_node.cpp | 10 +++++----- 5 files changed, 29 insertions(+), 9 deletions(-) diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index 300c4505..64610684 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/ouster-ros/config/driver_params.yaml @@ -15,3 +15,4 @@ ouster/os_driver: imu_frame: os_imu point_cloud_frame: os_lidar timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode + proc_mask: IMG|PCL|IMU|SCAN # currently only PCL and SCAN are effective \ No newline at end of file diff --git a/ouster-ros/config/os_sensor_os_cloud_params.yaml b/ouster-ros/config/os_sensor_os_cloud_params.yaml index 79fe20b0..05f90ba4 100644 --- a/ouster-ros/config/os_sensor_os_cloud_params.yaml +++ b/ouster-ros/config/os_sensor_os_cloud_params.yaml @@ -21,3 +21,4 @@ ouster/os_cloud: imu_frame: os_imu point_cloud_frame: os_lidar timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode + proc_mask: PCL|SCAN # select PCL and SCAN or either diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h index b31be574..40e23494 100644 --- a/ouster-ros/src/lidar_packet_handler.h +++ b/ouster-ros/src/lidar_packet_handler.h @@ -51,6 +51,23 @@ uint64_t ulround(T value) { return static_cast(rounded_value); } +// TODO: move to a separate file +std::set parse_tokens(const std::string& input, char delim) { + std::set 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; +} + } // namespace namespace sensor = ouster::sensor; diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index fa9da729..81ca314f 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -61,6 +61,7 @@ class OusterCloud : public OusterProcessingNodeBase { void declare_parameters() { os_tf_bcast.declare_parameters(); declare_parameter("timestamp_mode", ""); + declare_parameter("proc_mask", std::string("PCL|SCAN")); } void parse_parameters() { @@ -108,8 +109,9 @@ class OusterCloud : public OusterProcessingNodeBase { }); std::vector processors; - auto process_pc = true; - if (process_pc) + auto proc_mask = get_parameter("proc_mask").as_string(); + auto tokens = parse_tokens(proc_mask, '|'); + if (tokens.find("PCL") != tokens.end()) processors.push_back(PointCloudProcessor::create( info, os_tf_bcast.point_cloud_frame_id(), os_tf_bcast.apply_lidar_to_sensor_transform(), @@ -118,8 +120,7 @@ class OusterCloud : public OusterProcessingNodeBase { lidar_pubs[i]->publish(*point_cloud_msg[i]); })); - auto process_scan = true; - if (process_scan) + if (tokens.find("SCAN") != tokens.end()) processors.push_back(LaserScanProcessor::create( info, os_tf_bcast.point_cloud_frame_id(), // TODO: should we have different frame for the laser scan than point cloud??? 0, [this](LaserScanProcessor::OutputType laser_scan_msg) { diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 80619383..44ac5187 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -32,6 +32,7 @@ class OusterDriver : public OusterSensor { : OusterSensor("os_driver", options), os_tf_bcast(this) { os_tf_bcast.declare_parameters(); os_tf_bcast.parse_parameters(); + declare_parameter("proc_mask", std::string("PCL|SCAN")); } virtual void on_metadata_updated(const sensor::sensor_info& info) override { @@ -63,8 +64,9 @@ class OusterDriver : public OusterSensor { ImuPacketHandler::create_handler(info, os_tf_bcast.imu_frame_id(), use_ros_time); std::vector processors; - auto process_pc = true; - if (process_pc) + auto proc_mask = get_parameter("proc_mask").as_string(); + auto tokens = parse_tokens(proc_mask, '|'); + if (tokens.find("PCL") != tokens.end()) processors.push_back(PointCloudProcessor::create( info, os_tf_bcast.point_cloud_frame_id(), os_tf_bcast.apply_lidar_to_sensor_transform(), @@ -72,9 +74,7 @@ class OusterDriver : public OusterSensor { for (size_t i = 0; i < point_cloud_msg.size(); ++i) lidar_pubs[i]->publish(*point_cloud_msg[i]); })); - - auto process_scan = true; - if (process_scan) + if (tokens.find("SCAN") != tokens.end()) processors.push_back(LaserScanProcessor::create( info, os_tf_bcast.point_cloud_frame_id(), // TODO: should we have different frame for the laser scan than point cloud??? 0, [this](LaserScanProcessor::OutputType laser_scan_msg) { From acf7b9d472526d1f52566f4172aedfc6714eee44 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 7 Jun 2023 16:48:18 -0700 Subject: [PATCH 18/38] Add support for the selecting IMU + create topics/subs when their respective flags enabled --- ouster-ros/config/driver_params.yaml | 2 +- .../config/os_sensor_os_cloud_params.yaml | 2 +- ouster-ros/src/lidar_packet_handler.h | 25 +++---- ouster-ros/src/os_cloud_node.cpp | 58 +++++++++------- ouster-ros/src/os_driver_node.cpp | 67 +++++++++++-------- 5 files changed, 89 insertions(+), 65 deletions(-) diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index 64610684..515097c7 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/ouster-ros/config/driver_params.yaml @@ -15,4 +15,4 @@ ouster/os_driver: imu_frame: os_imu point_cloud_frame: os_lidar timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode - proc_mask: IMG|PCL|IMU|SCAN # currently only PCL and SCAN are effective \ No newline at end of file + proc_mask: IMG|PCL|IMU|SCAN # currently IMG is not supported \ No newline at end of file diff --git a/ouster-ros/config/os_sensor_os_cloud_params.yaml b/ouster-ros/config/os_sensor_os_cloud_params.yaml index 05f90ba4..6d1f6969 100644 --- a/ouster-ros/config/os_sensor_os_cloud_params.yaml +++ b/ouster-ros/config/os_sensor_os_cloud_params.yaml @@ -21,4 +21,4 @@ ouster/os_cloud: imu_frame: os_imu point_cloud_frame: os_lidar timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode - proc_mask: PCL|SCAN # select PCL and SCAN or either + proc_mask: IMU|PCL|SCAN # pick IMU, PCL, SCAN or any combination of the three options diff --git a/ouster-ros/src/lidar_packet_handler.h b/ouster-ros/src/lidar_packet_handler.h index 40e23494..3ba4b052 100644 --- a/ouster-ros/src/lidar_packet_handler.h +++ b/ouster-ros/src/lidar_packet_handler.h @@ -68,12 +68,17 @@ std::set parse_tokens(const std::string& input, char delim) { return tokens; } +inline bool check_token(const std::set& tokens, + const std::string& token) { + return tokens.find(token) != tokens.end(); +} + } // namespace namespace sensor = ouster::sensor; -using LidarScanProcessor = - std::function; +using LidarScanProcessor = std::function; class LidarPacketHandler { using LidarPacketAccumlator = std::function; @@ -89,8 +94,7 @@ class LidarPacketHandler { // initialize lidar_scan processor and buffer scan_batcher = std::make_unique(info); lidar_scan = std::make_unique( - info.format.columns_per_frame, - info.format.pixels_per_column, + info.format.columns_per_frame, info.format.pixels_per_column, info.format.udp_profile_lidar); // initalize time handlers @@ -123,26 +127,23 @@ class LidarPacketHandler { } public: - static HandlerType create_handler(const ouster::sensor::sensor_info& info, - bool use_ros_time, - const std::vector& handlers) { - auto handler = std::make_shared( - info, use_ros_time); + static HandlerType create_handler( + const ouster::sensor::sensor_info& info, bool use_ros_time, + const std::vector& handlers) { + auto handler = std::make_shared(info, use_ros_time); handler->lidar_scan_handlers = handlers; return [handler](const uint8_t* lidar_buf) { if (handler->lidar_packet_accumlator(lidar_buf)) { for (auto h : handler->lidar_scan_handlers) { - h(*handler->lidar_scan, - handler->lidar_scan_estimated_ts, + h(*handler->lidar_scan, handler->lidar_scan_estimated_ts, handler->lidar_scan_estimated_msg_ts); } } }; } - // time interpolation methods uint64_t impute_value(int last_scan_last_nonzero_idx, uint64_t last_scan_last_nonzero_value, diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 81ca314f..bd85682d 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -61,13 +61,14 @@ class OusterCloud : public OusterProcessingNodeBase { void declare_parameters() { os_tf_bcast.declare_parameters(); declare_parameter("timestamp_mode", ""); - declare_parameter("proc_mask", std::string("PCL|SCAN")); + declare_parameter("proc_mask", std::string("IMU|PCL|SCAN")); } void parse_parameters() { os_tf_bcast.parse_parameters(); auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); - use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME" || timestamp_mode_arg == "TIME_FROM_ROS_RECEPTION"; + use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME" || + timestamp_mode_arg == "TIME_FROM_ROS_RECEPTION"; } void metadata_handler( @@ -97,21 +98,24 @@ class OusterCloud : public OusterProcessingNodeBase { } void create_subscriptions() { + auto proc_mask = get_parameter("proc_mask").as_string(); + auto tokens = parse_tokens(proc_mask, '|'); + rclcpp::SensorDataQoS qos; - imu_packet_handler = ImuPacketHandler::create_handler( - info, os_tf_bcast.imu_frame_id(), use_ros_time); - imu_packet_sub = create_subscription( - "imu_packets", qos, - [this](const PacketMsg::ConstSharedPtr msg) { - auto imu_msg = imu_packet_handler(msg->buf.data()); - imu_pub->publish(imu_msg); - }); + if (check_token(tokens, "IMU")) { + imu_packet_handler = ImuPacketHandler::create_handler( + info, os_tf_bcast.imu_frame_id(), use_ros_time); + imu_packet_sub = create_subscription( + "imu_packets", qos, + [this](const PacketMsg::ConstSharedPtr msg) { + auto imu_msg = imu_packet_handler(msg->buf.data()); + imu_pub->publish(imu_msg); + }); + } std::vector processors; - auto proc_mask = get_parameter("proc_mask").as_string(); - auto tokens = parse_tokens(proc_mask, '|'); - if (tokens.find("PCL") != tokens.end()) + if (check_token(tokens, "PCL")) { processors.push_back(PointCloudProcessor::create( info, os_tf_bcast.point_cloud_frame_id(), os_tf_bcast.apply_lidar_to_sensor_transform(), @@ -119,27 +123,33 @@ class OusterCloud : public OusterProcessingNodeBase { for (size_t i = 0; i < point_cloud_msg.size(); ++i) lidar_pubs[i]->publish(*point_cloud_msg[i]); })); + } - if (tokens.find("SCAN") != tokens.end()) + if (check_token(tokens, "SCAN")) { processors.push_back(LaserScanProcessor::create( - info, os_tf_bcast.point_cloud_frame_id(), // TODO: should we have different frame for the laser scan than point cloud??? + info, + os_tf_bcast + .point_cloud_frame_id(), // TODO: should we have different + // frame for the laser scan than + // point cloud??? 0, [this](LaserScanProcessor::OutputType laser_scan_msg) { for (size_t i = 0; i < laser_scan_msg.size(); ++i) scan_pubs[i]->publish(*laser_scan_msg[i]); })); + } - lidar_packet_handler = LidarPacketHandler::create_handler( - info, use_ros_time, processors); - - lidar_packet_sub = create_subscription( - "lidar_packets", qos, - [this](const PacketMsg::ConstSharedPtr msg) { - lidar_packet_handler(msg->buf.data()); - }); + if (check_token(tokens, "PCL") || check_token(tokens, "SCAN")) { + lidar_packet_handler = LidarPacketHandler::create_handler( + info, use_ros_time, processors); + lidar_packet_sub = create_subscription( + "lidar_packets", qos, + [this](const PacketMsg::ConstSharedPtr msg) { + lidar_packet_handler(msg->buf.data()); + }); + } } private: - rclcpp::Subscription::SharedPtr imu_packet_sub; rclcpp::Publisher::SharedPtr imu_pub; diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 44ac5187..a5066392 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -32,7 +32,7 @@ class OusterDriver : public OusterSensor { : OusterSensor("os_driver", options), os_tf_bcast(this) { os_tf_bcast.declare_parameters(); os_tf_bcast.parse_parameters(); - declare_parameter("proc_mask", std::string("PCL|SCAN")); + declare_parameter("proc_mask", std::string("IMU|PCL|SCAN")); } virtual void on_metadata_updated(const sensor::sensor_info& info) override { @@ -42,31 +42,29 @@ class OusterDriver : public OusterSensor { virtual void create_publishers() override { int num_returns = get_n_returns(info); + auto proc_mask = get_parameter("proc_mask").as_string(); + auto tokens = parse_tokens(proc_mask, '|'); rclcpp::SensorDataQoS qos; - imu_pub = create_publisher("imu", qos); - lidar_pubs.resize(num_returns); - for (int i = 0; i < num_returns; i++) { - lidar_pubs[i] = create_publisher( - topic_for_return("points", i), qos); - } - - scan_pubs.resize(num_returns); - for (int i = 0; i < num_returns; i++) { - scan_pubs[i] = create_publisher( - topic_for_return("scan", i), qos); - } auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); - bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME" || timestamp_mode_arg == "TIME_FROM_ROS_RECEPTION"; + bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME" || + timestamp_mode_arg == "TIME_FROM_ROS_RECEPTION"; - imu_packet_handler = - ImuPacketHandler::create_handler(info, os_tf_bcast.imu_frame_id(), use_ros_time); + if (check_token(tokens, "IMU")) { + imu_pub = create_publisher("imu", qos); + imu_packet_handler = ImuPacketHandler::create_handler( + info, os_tf_bcast.imu_frame_id(), use_ros_time); + } std::vector processors; - auto proc_mask = get_parameter("proc_mask").as_string(); - auto tokens = parse_tokens(proc_mask, '|'); - if (tokens.find("PCL") != tokens.end()) + if (check_token(tokens, "PCL")) { + lidar_pubs.resize(num_returns); + for (int i = 0; i < num_returns; ++i) { + lidar_pubs[i] = create_publisher( + topic_for_return("points", i), qos); + } + processors.push_back(PointCloudProcessor::create( info, os_tf_bcast.point_cloud_frame_id(), os_tf_bcast.apply_lidar_to_sensor_transform(), @@ -74,29 +72,44 @@ class OusterDriver : public OusterSensor { for (size_t i = 0; i < point_cloud_msg.size(); ++i) lidar_pubs[i]->publish(*point_cloud_msg[i]); })); - if (tokens.find("SCAN") != tokens.end()) + } + + if (check_token(tokens, "SCAN")) { + scan_pubs.resize(num_returns); + for (int i = 0; i < num_returns; ++i) { + scan_pubs[i] = create_publisher( + topic_for_return("scan", i), qos); + } + processors.push_back(LaserScanProcessor::create( - info, os_tf_bcast.point_cloud_frame_id(), // TODO: should we have different frame for the laser scan than point cloud??? + info, + os_tf_bcast + .point_cloud_frame_id(), // TODO: should we have different + // frame for the laser scan than + // point cloud??? 0, [this](LaserScanProcessor::OutputType laser_scan_msg) { for (size_t i = 0; i < laser_scan_msg.size(); ++i) scan_pubs[i]->publish(*laser_scan_msg[i]); })); + } - lidar_packet_handler = LidarPacketHandler::create_handler( - info, use_ros_time, processors); + if (check_token(tokens, "PCL") || check_token(tokens, "SCAN")) + lidar_packet_handler = LidarPacketHandler::create_handler( + info, use_ros_time, processors); } virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override { - lidar_packet_handler(raw_lidar_packet); + if (lidar_packet_handler) lidar_packet_handler(raw_lidar_packet); } virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet) override { - auto imu_msg = imu_packet_handler(raw_imu_packet); - imu_pub->publish(imu_msg); + if (imu_packet_handler) + imu_pub->publish(imu_packet_handler(raw_imu_packet)); } private: - OusterStaticTransformsBroadcaster os_tf_bcast; + OusterStaticTransformsBroadcaster + os_tf_bcast; rclcpp::Publisher::SharedPtr imu_pub; std::vector::SharedPtr> From c3f7f5336ff45da62ab31df27769c3f0004d2ba7 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 8 Jun 2023 12:28:18 -0700 Subject: [PATCH 19/38] Reduce sync operations + restore sensor reset/reactivation --- ouster-ros/src/os_sensor_node.cpp | 186 ++++++++++++++---------------- ouster-ros/src/os_sensor_node.h | 59 +++++----- 2 files changed, 117 insertions(+), 128 deletions(-) diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index af07fbb5..afaac09d 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -37,10 +37,10 @@ OusterSensor::OusterSensor(const rclcpp::NodeOptions& options) void OusterSensor::declare_parameters() { declare_parameter("sensor_hostname", ""); - declare_parameter("lidar_ip", ""); // community + declare_parameter("lidar_ip", ""); // community driver param declare_parameter("metadata", ""); declare_parameter("udp_dest", ""); - declare_parameter("computer_ip", ""); // community + declare_parameter("computer_ip", ""); // community driver param declare_parameter("mtp_dest", ""); declare_parameter("mtp_main", false); declare_parameter("lidar_port", 0); @@ -66,7 +66,6 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_configure( create_metadata_publisher(); update_config_and_metadata(*sensor_client); create_services(); - create_publishers(); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM( get_logger(), @@ -83,8 +82,11 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_activate( const rclcpp_lifecycle::State& state) { RCLCPP_DEBUG(get_logger(), "on_activate() is called."); LifecycleNode::on_activate(state); + if (active_config.empty() || cached_metadata.empty()) + update_config_and_metadata(*sensor_client); + create_publishers(); allocate_buffers(); - start_packet_processing_thread(); + start_packet_processing_threads(); start_sensor_connection_thread(); return LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -100,8 +102,8 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_deactivate( const rclcpp_lifecycle::State& state) { RCLCPP_DEBUG(get_logger(), "on_deactivate() is called."); LifecycleNode::on_deactivate(state); + stop_packet_processing_threads(); stop_sensor_connection_thread(); - stop_packet_processing_thread(); return LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -132,7 +134,7 @@ LifecycleNodeInterface::CallbackReturn OusterSensor::on_shutdown( if (state.label() == "active") { stop_sensor_connection_thread(); - stop_packet_processing_thread(); + stop_packet_processing_threads(); } // whether state was 'active' or 'inactive' do cleanup @@ -319,7 +321,10 @@ void OusterSensor::reactivate_sensor(bool init_id_reset) { } reset_last_init_id = init_id_reset; - update_config_and_metadata(*sensor_client); + active_config.clear(); + cached_metadata.clear(); + imu_packets_skip = false; + lidar_packets_skip = false; auto request_transitions = std::vector{ lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE}; @@ -640,13 +645,16 @@ void OusterSensor::create_publishers() { void OusterSensor::allocate_buffers() { auto& pf = sensor::get_format(info); - // TODO: Do we need the +1? - lidar_packet.buf.resize(pf.lidar_packet_size + 1); + + lidar_packet.buf.resize(pf.lidar_packet_size); + // TODO: gauge necessary queue size for lidar packets lidar_packets = - std::make_unique(pf.lidar_packet_size + 1, 1024); - imu_packet.buf.resize(pf.imu_packet_size + 1); + std::make_unique(pf.lidar_packet_size, 1024); + + imu_packet.buf.resize(pf.imu_packet_size); + // TODO: gauge necessary queue size for lidar packets imu_packets = - std::make_unique(pf.imu_packet_size + 1, 1024); + std::make_unique(pf.imu_packet_size, 1024); } bool OusterSensor::init_id_changed(const sensor::packet_format& pf, @@ -683,63 +691,44 @@ void OusterSensor::handle_poll_client_error() { void OusterSensor::handle_lidar_packet(sensor::client& cli, const sensor::packet_format& pf) { - std::unique_lock lock(mtx); - receiving_cv.wait(lock, [this] { return lidar_data_processed; }); - bool success = true; - lidar_packets->write_overwrite([&cli, pf, &success](uint8_t* buffer) { - success = sensor::read_lidar_packet(cli, buffer, pf); + lidar_packets->write_overwrite([this, &cli, pf](uint8_t* buffer) { + bool success = sensor::read_lidar_packet(cli, buffer, pf); + if (success) { + read_lidar_packet_errors = 0; + if (!is_legacy_lidar_profile(info) && init_id_changed(pf, buffer)) { + // TODO: short circut reset if no breaking changes occured? + RCLCPP_WARN(get_logger(), + "sensor init_id has changed! reactivating.."); + reactivate_sensor(); + } + } else { + if (++read_lidar_packet_errors > max_read_lidar_packet_errors) { + RCLCPP_ERROR_STREAM( + get_logger(), + "maximum number of allowed errors from " + "sensor::read_lidar_packet() reached, reactivating..."); + read_lidar_packet_errors = 0; + reactivate_sensor(true); + } + } }); - lidar_data_processed = false; - lock.unlock(); - processing_cv.notify_all(); - - // if (success) { - // read_lidar_packet_errors = 0; - // if (!is_legacy_lidar_profile(info) && - // init_id_changed(pf, lidar_packet.buf.data())) { - // // TODO: short circut reset if no breaking changes occured? - // RCLCPP_WARN(get_logger(), - // "sensor init_id has changed! reactivating.."); - // reactivate_sensor(); - // } else { - // lidar_packet_pub->publish(lidar_packet); - // } - // } else { - // if (++read_lidar_packet_errors > max_read_lidar_packet_errors) { - // RCLCPP_ERROR_STREAM( - // get_logger(), - // "maximum number of allowed errors from " - // "sensor::read_lidar_packet() reached, reactivating..."); - // read_lidar_packet_errors = 0; - // reactivate_sensor(true); - // } - // } } void OusterSensor::handle_imu_packet(sensor::client& cli, const sensor::packet_format& pf) { - std::unique_lock lock(mtx); - receiving_cv.wait(lock, [this] { return imu_data_processed; }); - bool success = true; - imu_packets->write_overwrite([&cli, pf, &success](uint8_t* buffer) { - success = sensor::read_imu_packet(cli, buffer, pf); + imu_packets->write_overwrite([this, &cli, pf](uint8_t* buffer) { + bool success = sensor::read_imu_packet(cli, buffer, pf); + if (!success) { + if (++read_imu_packet_errors > max_read_imu_packet_errors) { + RCLCPP_ERROR_STREAM( + get_logger(), + "maximum number of allowed errors from " + "sensor::read_imu_packet() reached, reactivating..."); + read_imu_packet_errors = 0; + reactivate_sensor(true); + } + } }); - imu_data_processed = false; - lock.unlock(); - processing_cv.notify_one(); - - // if (success) { - // imu_packet_pub->publish(imu_packet); - // } else { - // if (++read_imu_packet_errors > max_read_imu_packet_errors) { - // RCLCPP_ERROR_STREAM( - // get_logger(), - // "maximum number of allowed errors from " - // "sensor::read_imu_packet() reached, reactivating..."); - // read_imu_packet_errors = 0; - // reactivate_sensor(true); - // } - // } } void OusterSensor::cleanup() { @@ -750,7 +739,8 @@ void OusterSensor::cleanup() { get_config_srv.reset(); set_config_srv.reset(); sensor_connection_thread.reset(); - packet_processing_thread.reset(); + imu_packets_processing_thread.reset(); + lidar_packets_processing_thread.reset(); } void OusterSensor::connection_loop(sensor::client& cli, @@ -792,21 +782,41 @@ void OusterSensor::stop_sensor_connection_thread() { } } -void OusterSensor::start_packet_processing_thread() { - packet_processing_active = true; - packet_processing_thread = std::make_unique([this]() { - while (packet_processing_active) { - process_packets(); +void OusterSensor::start_packet_processing_threads() { + imu_packets_skip = false; + imu_packets_processing_thread_active = true; + imu_packets_processing_thread = std::make_unique([this]() { + while (imu_packets_processing_thread_active) { + imu_packets->read([this](const uint8_t* buffer) { + if (!imu_packets_skip) on_imu_packet_msg(buffer); + }); } - RCLCPP_DEBUG(get_logger(), "packet_processing_thread done."); + RCLCPP_DEBUG(get_logger(), "imu_packets_processing_thread done."); + }); + + lidar_packets_skip = false; + lidar_packets_processing_thread_active = true; + lidar_packets_processing_thread = std::make_unique([this]() { + while (lidar_packets_processing_thread_active) { + lidar_packets->read([this](const uint8_t* buffer) { + if (!lidar_packets_skip) on_lidar_packet_msg(buffer); + }); + } + RCLCPP_DEBUG(get_logger(), "lidar_packets_processing_thread done."); }); } -void OusterSensor::stop_packet_processing_thread() { - RCLCPP_DEBUG(get_logger(), "packet_processing_thread stopping."); - if (packet_processing_thread->joinable()) { - packet_processing_active = false; - packet_processing_thread->join(); +void OusterSensor::stop_packet_processing_threads() { + RCLCPP_DEBUG(get_logger(), "stopping packet processing threads."); + + if (imu_packets_processing_thread->joinable()) { + imu_packets_processing_thread_active = false; + imu_packets_processing_thread->join(); + } + + if (lidar_packets_processing_thread->joinable()) { + lidar_packets_processing_thread_active = false; + lidar_packets_processing_thread->join(); } } @@ -829,32 +839,6 @@ void OusterSensor::on_imu_packet_msg(const uint8_t* raw_imu_packet) { imu_packet_pub->publish(imu_packet); } -void OusterSensor::process_packets() { - while (packet_processing_active) { - std::unique_lock lock(mtx); - - // Wait until there is a job in the queue - processing_cv.wait(lock, [this] { - return !lidar_data_processed || !imu_data_processed; - }); - - if (!lidar_data_processed) { - lidar_packets->read( - [this](const uint8_t* buffer) { on_lidar_packet_msg(buffer); }); - lidar_data_processed = true; - } - - if (!imu_data_processed) { - imu_packets->read( - [this](const uint8_t* buffer) { on_imu_packet_msg(buffer); }); - imu_data_processed = true; - } - - lock.unlock(); - receiving_cv.notify_one(); - } -} - } // namespace ouster_ros #include diff --git a/ouster-ros/src/os_sensor_node.h b/ouster-ros/src/os_sensor_node.h index 39aa33ad..e36de40c 100644 --- a/ouster-ros/src/os_sensor_node.h +++ b/ouster-ros/src/os_sensor_node.h @@ -34,23 +34,28 @@ using ouster_srvs::srv::GetConfig; using ouster_srvs::srv::SetConfig; using rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface; - namespace ouster_ros { class OusterSensor : public OusterSensorNodeBase { -public: + public: OUSTER_ROS_PUBLIC OusterSensor(const std::string& name, const rclcpp::NodeOptions& options); explicit OusterSensor(const rclcpp::NodeOptions& options); - LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State& state); - LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State& state); - LifecycleNodeInterface::CallbackReturn on_error(const rclcpp_lifecycle::State& state); - LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& state); - LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State&); - LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& state); - -protected: + LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State& state); + LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State& state); + LifecycleNodeInterface::CallbackReturn on_error( + const rclcpp_lifecycle::State& state); + LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State& state); + LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State&); + LifecycleNodeInterface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State& state); + + protected: virtual void on_metadata_updated(const sensor::sensor_info& info); virtual void create_services(); @@ -61,10 +66,13 @@ class OusterSensor : public OusterSensorNodeBase { virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet); -private: + private: void declare_parameters(); + std::string get_sensor_hostname(); + void update_config_and_metadata(sensor::client& cli); + void save_metadata(); static std::string transition_id_to_string(uint8_t transition_id); @@ -72,15 +80,15 @@ class OusterSensor : public OusterSensorNodeBase { bool change_state(std::uint8_t transition_id, CallbackT callback, CallbackT_Args... callback_args, std::chrono::seconds time_out = std::chrono::seconds{3}); + void execute_transitions_sequence(std::vector transitions_sequence, size_t at); - // param init_id_reset is overriden to true when force_reinit is true void reset_sensor(bool force_reinit, bool init_id_reset = false); // TODO: need to notify dependent node(s) of the update - void reactivate_sensor(bool init_id_reset); + void reactivate_sensor(bool init_id_reset = false); void create_reset_service(); @@ -97,7 +105,8 @@ class OusterSensor : public OusterSensorNodeBase { uint8_t compose_config_flags(const sensor::sensor_config& config); - void configure_sensor(const std::string& hostname, sensor::sensor_config& config); + void configure_sensor(const std::string& hostname, + sensor::sensor_config& config); std::string load_config_file(const std::string& config_file); @@ -126,11 +135,9 @@ class OusterSensor : public OusterSensorNodeBase { void stop_sensor_connection_thread(); - void start_packet_processing_thread(); - - void stop_packet_processing_thread(); + void start_packet_processing_threads(); - void process_packets(); + void stop_packet_processing_threads(); private: std::string sensor_hostname; @@ -148,13 +155,6 @@ class OusterSensor : public OusterSensorNodeBase { rclcpp::Service::SharedPtr set_config_srv; std::shared_ptr> change_state_client; - // TODO: reduce sync objects - std::mutex mtx; - std::condition_variable receiving_cv; - std::condition_variable processing_cv; - bool lidar_data_processed = true; - bool imu_data_processed = true; - // TODO: implement & utilize a lock-free ring buffer in future std::unique_ptr lidar_packets; std::unique_ptr imu_packets; @@ -162,8 +162,13 @@ class OusterSensor : public OusterSensorNodeBase { std::atomic sensor_connection_active = {false}; std::unique_ptr sensor_connection_thread; - std::atomic packet_processing_active = {false}; - std::unique_ptr packet_processing_thread; + std::atomic imu_packets_processing_thread_active = {false}; + std::atomic imu_packets_skip; + std::unique_ptr imu_packets_processing_thread; + + std::atomic lidar_packets_processing_thread_active = {false}; + std::atomic lidar_packets_skip; + std::unique_ptr lidar_packets_processing_thread; bool force_sensor_reinit = false; bool reset_last_init_id = true; From 092330fe57a9f3c29d47c75fe807ed71fee70c2c Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 8 Jun 2023 13:16:42 -0700 Subject: [PATCH 20/38] Add the ability to override current qos settings --- ouster-ros/config/driver_params.yaml | 6 +++- .../config/os_sensor_os_cloud_params.yaml | 4 +++ ouster-ros/src/os_cloud_node.cpp | 32 +++++++++++++------ ouster-ros/src/os_driver_node.cpp | 17 +++++++--- ouster-ros/src/os_image_node.cpp | 19 +++++++---- ouster-ros/src/os_sensor_node.cpp | 18 ++++++++--- 6 files changed, 69 insertions(+), 27 deletions(-) diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index 515097c7..6bdc8925 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/ouster-ros/config/driver_params.yaml @@ -15,4 +15,8 @@ ouster/os_driver: imu_frame: os_imu point_cloud_frame: os_lidar timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode - proc_mask: IMG|PCL|IMU|SCAN # currently IMG is not supported \ No newline at end of file + proc_mask: IMG|PCL|IMU|SCAN # currently IMG is not supported + # if False, data are published with sensor 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 diff --git a/ouster-ros/config/os_sensor_os_cloud_params.yaml b/ouster-ros/config/os_sensor_os_cloud_params.yaml index 6d1f6969..e6d2aae0 100644 --- a/ouster-ros/config/os_sensor_os_cloud_params.yaml +++ b/ouster-ros/config/os_sensor_os_cloud_params.yaml @@ -14,6 +14,7 @@ ouster/os_sensor: metadata: '' lidar_port: 0 imu_port: 0 + use_system_default_qos: False ouster/os_cloud: ros__parameters: sensor_frame: os_sensor @@ -22,3 +23,6 @@ ouster/os_cloud: point_cloud_frame: os_lidar timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode proc_mask: IMU|PCL|SCAN # pick IMU, PCL, SCAN or any combination of the three options + use_system_default_qos: False # needs to match the value defined for os_sensor node +ouster/os_image: + use_system_default_qos: False # needs to match the value defined for os_sensor node diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index bd85682d..c50f5fe2 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -62,6 +62,7 @@ class OusterCloud : public OusterProcessingNodeBase { os_tf_bcast.declare_parameters(); declare_parameter("timestamp_mode", ""); declare_parameter("proc_mask", std::string("IMU|PCL|SCAN")); + declare_parameter("use_system_default_qos", false); } void parse_parameters() { @@ -82,18 +83,24 @@ class OusterCloud : public OusterProcessingNodeBase { } void create_publishers(int num_returns) { - rclcpp::SensorDataQoS qos; - imu_pub = create_publisher("imu", qos); + bool use_system_default_qos = + get_parameter("use_system_default_qos").as_bool(); + rclcpp::QoS system_default_qos = rclcpp::SystemDefaultsQoS(); + rclcpp::QoS sensor_data_qos = rclcpp::SensorDataQoS(); + auto selected_qos = + use_system_default_qos ? system_default_qos : sensor_data_qos; + + imu_pub = create_publisher("imu", selected_qos); lidar_pubs.resize(num_returns); for (int i = 0; i < num_returns; i++) { lidar_pubs[i] = create_publisher( - topic_for_return("points", i), qos); + topic_for_return("points", i), selected_qos); } scan_pubs.resize(num_returns); for (int i = 0; i < num_returns; i++) { scan_pubs[i] = create_publisher( - topic_for_return("scan", i), qos); + topic_for_return("scan", i), selected_qos); } } @@ -101,13 +108,18 @@ class OusterCloud : public OusterProcessingNodeBase { auto proc_mask = get_parameter("proc_mask").as_string(); auto tokens = parse_tokens(proc_mask, '|'); - rclcpp::SensorDataQoS qos; + bool use_system_default_qos = + get_parameter("use_system_default_qos").as_bool(); + rclcpp::QoS system_default_qos = rclcpp::SystemDefaultsQoS(); + rclcpp::QoS sensor_data_qos = rclcpp::SensorDataQoS(); + auto selected_qos = + use_system_default_qos ? system_default_qos : sensor_data_qos; if (check_token(tokens, "IMU")) { imu_packet_handler = ImuPacketHandler::create_handler( info, os_tf_bcast.imu_frame_id(), use_ros_time); imu_packet_sub = create_subscription( - "imu_packets", qos, + "imu_packets", selected_qos, [this](const PacketMsg::ConstSharedPtr msg) { auto imu_msg = imu_packet_handler(msg->buf.data()); imu_pub->publish(imu_msg); @@ -129,9 +141,9 @@ class OusterCloud : public OusterProcessingNodeBase { processors.push_back(LaserScanProcessor::create( info, os_tf_bcast - .point_cloud_frame_id(), // TODO: should we have different - // frame for the laser scan than - // point cloud??? + .point_cloud_frame_id(), // TODO: should we allow having a + // different frame for the laser + // scan than point cloud??? 0, [this](LaserScanProcessor::OutputType laser_scan_msg) { for (size_t i = 0; i < laser_scan_msg.size(); ++i) scan_pubs[i]->publish(*laser_scan_msg[i]); @@ -142,7 +154,7 @@ class OusterCloud : public OusterProcessingNodeBase { lidar_packet_handler = LidarPacketHandler::create_handler( info, use_ros_time, processors); lidar_packet_sub = create_subscription( - "lidar_packets", qos, + "lidar_packets", selected_qos, [this](const PacketMsg::ConstSharedPtr msg) { lidar_packet_handler(msg->buf.data()); }); diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index a5066392..5ba686e3 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -41,28 +41,35 @@ class OusterDriver : public OusterSensor { } virtual void create_publishers() override { - int num_returns = get_n_returns(info); auto proc_mask = get_parameter("proc_mask").as_string(); auto tokens = parse_tokens(proc_mask, '|'); - rclcpp::SensorDataQoS qos; + bool use_system_default_qos = + get_parameter("use_system_default_qos").as_bool(); + rclcpp::QoS system_default_qos = rclcpp::SystemDefaultsQoS(); + rclcpp::QoS sensor_data_qos = rclcpp::SensorDataQoS(); + auto selected_qos = + use_system_default_qos ? system_default_qos : sensor_data_qos; auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME" || timestamp_mode_arg == "TIME_FROM_ROS_RECEPTION"; if (check_token(tokens, "IMU")) { - imu_pub = create_publisher("imu", qos); + imu_pub = + create_publisher("imu", selected_qos); imu_packet_handler = ImuPacketHandler::create_handler( info, os_tf_bcast.imu_frame_id(), use_ros_time); } + int num_returns = get_n_returns(info); + std::vector processors; if (check_token(tokens, "PCL")) { lidar_pubs.resize(num_returns); for (int i = 0; i < num_returns; ++i) { lidar_pubs[i] = create_publisher( - topic_for_return("points", i), qos); + topic_for_return("points", i), selected_qos); } processors.push_back(PointCloudProcessor::create( @@ -78,7 +85,7 @@ class OusterDriver : public OusterSensor { scan_pubs.resize(num_returns); for (int i = 0; i < num_returns; ++i) { scan_pubs[i] = create_publisher( - topic_for_return("scan", i), qos); + topic_for_return("scan", i), selected_qos); } processors.push_back(LaserScanProcessor::create( diff --git a/ouster-ros/src/os_image_node.cpp b/ouster-ros/src/os_image_node.cpp index a874af8f..14546f0f 100644 --- a/ouster-ros/src/os_image_node.cpp +++ b/ouster-ros/src/os_image_node.cpp @@ -52,6 +52,7 @@ class OusterImage : public OusterProcessingNodeBase { private: void on_init() { + declare_parameter("use_system_default_qos", false); create_metadata_subscriber( [this](const auto& msg) { metadata_handler(msg); }); RCLCPP_INFO(get_logger(), "OusterImage: node initialized!"); @@ -74,22 +75,28 @@ class OusterImage : public OusterProcessingNodeBase { } void create_publishers(int n_returns) { - rclcpp::SensorDataQoS qos; - nearir_image_pub = - create_publisher("nearir_image", qos); + bool use_system_default_qos = + get_parameter("use_system_default_qos").as_bool(); + rclcpp::QoS system_default_qos = rclcpp::SystemDefaultsQoS(); + rclcpp::QoS sensor_data_qos = rclcpp::SensorDataQoS(); + auto selected_qos = + use_system_default_qos ? system_default_qos : sensor_data_qos; + + nearir_image_pub = create_publisher( + "nearir_image", selected_qos); rclcpp::Publisher::SharedPtr a_pub; for (int i = 0; i < n_returns; i++) { a_pub = create_publisher( - topic_for_return("range_image", i), qos); + topic_for_return("range_image", i), selected_qos); range_image_pubs.push_back(a_pub); a_pub = create_publisher( - topic_for_return("signal_image", i), qos); + topic_for_return("signal_image", i), selected_qos); signal_image_pubs.push_back(a_pub); a_pub = create_publisher( - topic_for_return("reflec_image", i), qos); + topic_for_return("reflec_image", i), selected_qos); reflec_image_pubs.push_back(a_pub); } } diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index afaac09d..facdc929 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -37,10 +37,11 @@ OusterSensor::OusterSensor(const rclcpp::NodeOptions& options) void OusterSensor::declare_parameters() { declare_parameter("sensor_hostname", ""); - declare_parameter("lidar_ip", ""); // community driver param + declare_parameter("lidar_ip", ""); // community driver param declare_parameter("metadata", ""); declare_parameter("udp_dest", ""); - declare_parameter("computer_ip", ""); // community driver param + declare_parameter("computer_ip", + ""); // community driver param declare_parameter("mtp_dest", ""); declare_parameter("mtp_main", false); declare_parameter("lidar_port", 0); @@ -48,6 +49,7 @@ void OusterSensor::declare_parameters() { declare_parameter("lidar_mode", ""); declare_parameter("timestamp_mode", ""); declare_parameter("udp_profile_lidar", ""); + declare_parameter("use_system_default_qos", false); } LifecycleNodeInterface::CallbackReturn OusterSensor::on_configure( @@ -638,9 +640,15 @@ void OusterSensor::create_services() { } void OusterSensor::create_publishers() { - rclcpp::SensorDataQoS qos; - lidar_packet_pub = create_publisher("lidar_packets", qos); - imu_packet_pub = create_publisher("imu_packets", qos); + bool use_system_default_qos = + get_parameter("use_system_default_qos").as_bool(); + rclcpp::QoS system_default_qos = rclcpp::SystemDefaultsQoS(); + rclcpp::QoS sensor_data_qos = rclcpp::SensorDataQoS(); + auto selected_qos = + use_system_default_qos ? system_default_qos : sensor_data_qos; + lidar_packet_pub = + create_publisher("lidar_packets", selected_qos); + imu_packet_pub = create_publisher("imu_packets", selected_qos); } void OusterSensor::allocate_buffers() { From 78ce55c6fd90c84a633aff9e98baf2ffb768290c Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 8 Jun 2023 13:24:53 -0700 Subject: [PATCH 21/38] Add minor note --- ouster-ros/config/driver_params.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index 6bdc8925..47e0161d 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/ouster-ros/config/driver_params.yaml @@ -15,7 +15,8 @@ ouster/os_driver: imu_frame: os_imu point_cloud_frame: os_lidar timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode - proc_mask: IMG|PCL|IMU|SCAN # currently IMG is not supported + proc_mask: IMG|PCL|IMU|SCAN # currently IMG is not supported and does not + # affect anything # if False, data are published with sensor data QoS. This is preferrable # for production but default QoS is needed for rosbag. # See: https://github.com/ros2/rosbag2/issues/125 From 48eadc6b7281e5c886f81347c45033075182d64f Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 8 Jun 2023 13:26:36 -0700 Subject: [PATCH 22/38] More detailed explanation about the IMG node --- ouster-ros/config/driver_params.yaml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index 47e0161d..2596271f 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/ouster-ros/config/driver_params.yaml @@ -16,7 +16,9 @@ ouster/os_driver: point_cloud_frame: os_lidar timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode proc_mask: IMG|PCL|IMU|SCAN # currently IMG is not supported and does not - # affect anything + # affect anything, to disable image topics + # you would need to omit the os_image node + # from the launch file # if False, data are published with sensor data QoS. This is preferrable # for production but default QoS is needed for rosbag. # See: https://github.com/ros2/rosbag2/issues/125 From b15b76744c98ef7c6866a28e35d5271b69c0a9b2 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 8 Jun 2023 13:44:00 -0700 Subject: [PATCH 23/38] Rename the file os_sensor_cloud_image_params to os_sensor_cloud_image_params.yaml and update corresponding launch files --- ...r_os_cloud_params.yaml => os_sensor_cloud_image_params.yaml} | 0 ouster-ros/launch/sensor.composite.launch.py | 2 +- ouster-ros/launch/sensor.independent.launch.py | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename ouster-ros/config/{os_sensor_os_cloud_params.yaml => os_sensor_cloud_image_params.yaml} (100%) diff --git a/ouster-ros/config/os_sensor_os_cloud_params.yaml b/ouster-ros/config/os_sensor_cloud_image_params.yaml similarity index 100% rename from ouster-ros/config/os_sensor_os_cloud_params.yaml rename to ouster-ros/config/os_sensor_cloud_image_params.yaml diff --git a/ouster-ros/launch/sensor.composite.launch.py b/ouster-ros/launch/sensor.composite.launch.py index c1db8dcf..0326e4a3 100644 --- a/ouster-ros/launch/sensor.composite.launch.py +++ b/ouster-ros/launch/sensor.composite.launch.py @@ -22,7 +22,7 @@ def generate_launch_description(): """ ouster_ros_pkg_dir = get_package_share_directory('ouster_ros') default_params_file = \ - Path(ouster_ros_pkg_dir) / 'config' / 'parameters.yaml' + Path(ouster_ros_pkg_dir) / 'config' / 'os_sensor_cloud_image_params.yaml' params_file = LaunchConfiguration('params_file') params_file_arg = DeclareLaunchArgument('params_file', default_value=str( diff --git a/ouster-ros/launch/sensor.independent.launch.py b/ouster-ros/launch/sensor.independent.launch.py index 5d190093..6a75656f 100644 --- a/ouster-ros/launch/sensor.independent.launch.py +++ b/ouster-ros/launch/sensor.independent.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): """ ouster_ros_pkg_dir = get_package_share_directory('ouster_ros') default_params_file = \ - Path(ouster_ros_pkg_dir) / 'config' / 'parameters.yaml' + Path(ouster_ros_pkg_dir) / 'config' / 'os_sensor_cloud_image_params.yaml' params_file = LaunchConfiguration('params_file') params_file_arg = DeclareLaunchArgument('params_file', default_value=str( From 7a6329d82d8eb097ac0cf93075191c0386e81e91 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Thu, 8 Jun 2023 14:02:40 -0700 Subject: [PATCH 24/38] Expose use_system_default_qos parameter to xml launch file and use proper defaults --- ouster-ros/launch/record.composite.launch.xml | 8 +++++++- ouster-ros/launch/replay.composite.launch.xml | 8 +++++++- ouster-ros/launch/sensor.composite.launch.xml | 8 +++++++- ouster-ros/launch/sensor.independent.launch.xml | 8 +++++++- 4 files changed, 28 insertions(+), 4 deletions(-) diff --git a/ouster-ros/launch/record.composite.launch.xml b/ouster-ros/launch/record.composite.launch.xml index d81f1173..31c4e133 100644 --- a/ouster-ros/launch/record.composite.launch.xml +++ b/ouster-ros/launch/record.composite.launch.xml @@ -46,6 +46,8 @@ + + @@ -60,14 +62,18 @@ + + + + + - diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml index cbca5733..f4f9f83b 100644 --- a/ouster-ros/launch/replay.composite.launch.xml +++ b/ouster-ros/launch/replay.composite.launch.xml @@ -25,10 +25,13 @@ + + + @@ -36,8 +39,11 @@ + + + + - diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml index 717cabd0..a723c2f7 100644 --- a/ouster-ros/launch/sensor.composite.launch.xml +++ b/ouster-ros/launch/sensor.composite.launch.xml @@ -44,6 +44,8 @@ + + @@ -58,14 +60,18 @@ + + + + + - diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml index 1bb6f464..1575abbd 100644 --- a/ouster-ros/launch/sensor.independent.launch.xml +++ b/ouster-ros/launch/sensor.independent.launch.xml @@ -44,6 +44,8 @@ + + @@ -57,14 +59,18 @@ + + + + + -