Port ROS-363 to ROS2

ouster-lidar · Sep 16, 2024 · 71550ca · 71550ca
1 parent 8b4364c
commit 71550ca
Show file tree

Hide file tree

Showing 23 changed files with 443 additions and 679 deletions.
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -23,6 +23,22 @@ Changelog
   - Doesn't handle detect and handle invalid configurations
 * Added an automatic start mode to make it easier to start the node without using time actions.
   - To disable set ``auto_start`` to ``false`` during launch
+* Added a new parameter ``organized`` to request publishing unorganized point cloud
+* Added a new parameter ``destagger`` to request publishing staggered point cloud
+* Added two parameters ``min_range``, ``max_range`` to limit the lidar effective range
+* Updated ouster_client to the release of ``20240425`` [v0.11.1]; changes listed below.
+
+ouster_client
+-------------
+* Added a new buffered UDP source implementation BufferedUDPSource.
+* The method version_of_string is marked as deprecated, use version_from_string
+instead.
+* Added a new method firmware_version_from_metadata which works across firmwares.
+* Added support for return order configuration parameter.
+* Added support for gyro and accelerometer FSR configuration parameters.
+* [BUGFIX] mtp_init_client throws a bad optional access.
+* [BUGFIX] properly handle 32-bit frame IDs from the
+* FUSA_RNG15_RFL8_NIR8_DUAL sensor UDP profile.
 
 
 ouster_ros v0.12.0

diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt
@@ -46,6 +46,7 @@ set(BUILD_SHARED_LIBS OFF)
 
 option(BUILD_VIZ "Turn off building VIZ" OFF)
 option(BUILD_PCAP "Turn off building PCAP" OFF)
+option(BUILD_OSF "Turn off building OSF" ON)
 find_package(OusterSDK REQUIRED)
 
 set(BUILD_SHARED_LIBS ${_SAVE_BUILD_SHARED_LIBS})
@@ -189,7 +190,6 @@ if(BUILD_TESTING)
   ament_add_gtest(${PROJECT_NAME}_test
     src/os_ros.cpp
     test/test_main.cpp
-    test/ring_buffer_test.cpp
     test/lock_free_ring_buffer_test.cpp
     test/point_accessor_test.cpp
     test/point_transform_test.cpp

diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml
@@ -59,7 +59,7 @@ ouster/os_driver:
     point_cloud_frame: os_lidar 
     # proc_mask[optional]: use any combination of the 4 flags IMG, PCL, IMU and
     # SCAN to enable or disable their respective messages.
-    proc_mask: IMG|PCL|IMU|SCAN
+    proc_mask: IMU|PCL|SCAN|IMG|RAW
     # scan_ring[optional]: use this parameter in conjunction with the SCAN flag
     # to select which beam of the LidarScan to use when producing the LaserScan
     # message. Choose a value the range [0, sensor_beams_count).
@@ -102,3 +102,12 @@ ouster/os_driver:
     # max_failed_reconnect_attempts[optional]: maximum number of attempts trying
     # to communicate with the sensor. Counter resets upon successful connection.
     max_failed_reconnect_attempts: 2147483647
+    # organized[optional]: whether to generate an organized point cloud. default
+    # is organized.
+    organized: true
+    # destagger[optional]: enable or disable point cloud destaggering, default enabled.
+    destagger: true
+    # min_range[optional]: minimum lidar range to consider (meters).
+    min_range: 0
+    # max_range[optional]: maximum lidar range to consider (meters).
+    max_range: 1000
diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h
@@ -158,6 +158,14 @@ inline bool check_token(const std::set<std::string>& tokens,
 
 ouster::util::version parse_version(const std::string& fw_rev);
 
+template <typename T>
+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<uint64_t>(rounded_value);
+}
+
 } // namespace impl
 
 }  // namespace ouster_ros
diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml
@@ -62,6 +62,16 @@
     xyzir
     }"/>
 
+  <arg name="organized" default="true"
+    description="generate an organzied point cloud"/>
+  <arg name="destagger" default="true"
+    description="enable or disable point cloud destaggering"/>
+
+  <arg name="min_range" default="0.0"
+    description="minimum lidar range to consider (meters)"/>
+  <arg name="max_range" default="1000.0"
+    description="minimum lidar range to consider (meters)"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node if="$(var _use_metadata_file)" pkg="ouster_ros" exec="os_replay" name="os_replay" output="screen">
@@ -80,6 +90,10 @@
         <param name="proc_mask" value="$(var proc_mask)"/>
         <param name="scan_ring" value="$(var scan_ring)"/>
         <param name="point_type" value="$(var point_type)"/>
+        <param name="organized" value="$(var organized)"/>
+        <param name="destagger" value="$(var destagger)"/>
+        <param name="min_range" value="$(var min_range)"/>
+        <param name="max_range" value="$(var max_range)"/>
       </composable_node>
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterImage" name="os_image">
         <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>

diff --git a/ouster-ros/launch/replay_pcap.launch.xml b/ouster-ros/launch/replay_pcap.launch.xml
@@ -53,6 +53,16 @@
     xyzir
     }"/>
 
+  <arg name="organized" default="true"
+    description="generate an organzied point cloud"/>
+  <arg name="destagger" default="true"
+    description="enable or disable point cloud destaggering"/>
+
+  <arg name="min_range" default="0.0"
+    description="minimum lidar range to consider (meters)"/>
+  <arg name="max_range" default="1000.0"
+    description="minimum lidar range to consider (meters)"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node if="$(var _use_metadata_file)" pkg="ouster_ros" exec="os_pcap" name="os_pcap" output="screen">
@@ -62,20 +72,24 @@
     </node>
     <node_container pkg="rclcpp_components" exec="component_container_mt" name="os_container" output="screen" namespace="">
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterCloud" name="os_cloud">
-      <param name="sensor_frame" value="$(var sensor_frame)"/>
-      <param name="lidar_frame" value="$(var lidar_frame)"/>
-      <param name="imu_frame" value="$(var imu_frame)"/>
-      <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
-      <param name="timestamp_mode" value="$(var timestamp_mode)"/>
-      <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
-      <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
-      <param name="proc_mask" value="$(var proc_mask)"/>
-      <param name="scan_ring" value="$(var scan_ring)"/>
-      <param name="point_type" value="$(var point_type)"/>
+        <param name="sensor_frame" value="$(var sensor_frame)"/>
+        <param name="lidar_frame" value="$(var lidar_frame)"/>
+        <param name="imu_frame" value="$(var imu_frame)"/>
+        <param name="point_cloud_frame" value="$(var point_cloud_frame)"/>
+        <param name="timestamp_mode" value="$(var timestamp_mode)"/>
+        <param name="ptp_utc_tai_offset" value="$(var ptp_utc_tai_offset)"/>
+        <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
+        <param name="proc_mask" value="$(var proc_mask)"/>
+        <param name="scan_ring" value="$(var scan_ring)"/>
+        <param name="point_type" value="$(var point_type)"/>
+        <param name="organized" value="$(var organized)"/>
+        <param name="destagger" value="$(var destagger)"/>
+        <param name="min_range" value="$(var min_range)"/>
+        <param name="max_range" value="$(var max_range)"/>
       </composable_node>
       <composable_node pkg="ouster_ros" plugin="ouster_ros::OusterImage" name="os_image">
-      <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
-      <param name="proc_mask" value="$(var proc_mask)"/>
+        <param name="use_system_default_qos" value="$(var use_system_default_qos)"/>
+        <param name="proc_mask" value="$(var proc_mask)"/>
       </composable_node>
     </node_container>
   </group>

diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml
@@ -93,6 +93,16 @@
   <arg name="auto_start" default="true"
     description="automatically configure and activate the node"/>
 
+  <arg name="organized" default="true"
+    description="generate an organzied point cloud"/>
+  <arg name="destagger" default="true"
+    description="enable or disable point cloud destaggering"/>
+
+  <arg name="min_range" default="0.0"
+    description="minimum lidar range to consider (meters)"/>
+  <arg name="max_range" default="1000.0"
+    description="minimum lidar range to consider (meters)"/>
+
   <group>
     <push-ros-namespace namespace="$(var ouster_ns)"/>
     <node pkg="ouster_ros" exec="os_driver" name="os_driver" output="screen">
@@ -124,6 +134,10 @@
       <param name="max_failed_reconnect_attempts"
              value="$(var max_failed_reconnect_attempts)"/>
       <param name="auto_start" value="$(var auto_start)"/>
+      <param name="organized" value="$(var organized)"/>
+      <param name="destagger" value="$(var destagger)"/>
+      <param name="min_range" value="$(var min_range)"/>
+      <param name="max_range" value="$(var max_range)"/>
     </node>
   </group>
 

diff --git a/ouster-ros/ouster-sdk b/ouster-ros/ouster-sdk
diff --git a/ouster-ros/src/impl/cartesian.h b/ouster-ros/src/impl/cartesian.h
@@ -16,7 +16,10 @@ namespace ouster {
  * LidarScan.
  * @param[in] direction the direction of an xyz lut.
  * @param[in] offset the offset of an xyz lut.
- * @param[in] invalid the value to assign of an xyz lut.
+ * @param[in] min_range minimum lidar range to consider (millimeters).
+ * @param[in] max_range maximum lidar range to consider (millimeters).
+ * @param[in] invalid the value to assign of an xyz lut when range values are
+ * equal to or exceed the min_range and max_range values.
  *
  * @return Cartesian points where ith row is a 3D point which corresponds
  *         to ith pixel in LidarScan where i = row * w + col.
@@ -25,7 +28,7 @@ template <typename T>
 void cartesianT(PointsT<T>& points,
                 const Eigen::Ref<const img_t<uint32_t>>& range,
                 const PointsT<T>& direction, const PointsT<T>& offset,
-                T invalid) {
+                uint32_t min_r, uint32_t max_r, T invalid) {
     assert(points.rows() == direction.rows() &&
            "points & direction row count mismatch");
     assert(points.rows() == offset.rows() &&
@@ -51,7 +54,7 @@ void cartesianT(PointsT<T>& points,
         const auto idx_x = col_x + i;
         const auto idx_y = col_y + i;
         const auto idx_z = col_z + i;
-        if (r == 0) {
+        if (r <= min_r || r >= max_r) {
             pts[idx_x] = pts[idx_y] = pts[idx_z] = invalid;
         } else {
             pts[idx_x] = r * dir[idx_x] + ofs[idx_x];

diff --git a/ouster-ros/src/imu_packet_handler.h b/ouster-ros/src/imu_packet_handler.h
@@ -19,36 +19,39 @@ namespace ouster_ros {
 class ImuPacketHandler {
    public:
     using HandlerOutput = sensor_msgs::msg::Imu;
-    using HandlerType = std::function<HandlerOutput(const uint8_t*)>;
+    using HandlerType = std::function<HandlerOutput(const sensor::ImuPacket&)>;
 
    public:
-    static HandlerType create_handler(const ouster::sensor::sensor_info& info,
+    static HandlerType create_handler(const sensor::sensor_info& info,
                                       const std::string& frame,
                                       const std::string& timestamp_mode,
                                       int64_t ptp_utc_tai_offset) {
-        const auto& pf = ouster::sensor::get_format(info);
-        using Timestamper = std::function<rclcpp::Time(const uint8_t*)>;
+        const auto& pf = sensor::get_format(info);
+        using Timestamper = std::function<rclcpp::Time(const sensor::ImuPacket&)>;
         Timestamper timestamper;
-
         if (timestamp_mode == "TIME_FROM_ROS_TIME") {
-            timestamper = Timestamper{[](const uint8_t* /*imu_buf*/) {
-                return rclcpp::Clock(RCL_ROS_TIME).now();
-            }};
+            timestamper = Timestamper{
+                [](const sensor::ImuPacket& imu_packet) {
+                    return rclcpp::Time(imu_packet.host_timestamp);
+                }};
         } else if (timestamp_mode == "TIME_FROM_PTP_1588") {
-            timestamper =
-                Timestamper{[pf, ptp_utc_tai_offset](const uint8_t* imu_buf) {
-                    uint64_t ts = pf.imu_gyro_ts(imu_buf);
+            timestamper = Timestamper{
+                [pf, ptp_utc_tai_offset](const sensor::ImuPacket& imu_packet) {
+                    auto ts = pf.imu_gyro_ts(imu_packet.buf.data());
                     ts = impl::ts_safe_offset_add(ts, ptp_utc_tai_offset);
                     return rclcpp::Time(ts);
                 }};
         } else {
-            timestamper = Timestamper{[pf](const uint8_t* imu_buf) {
-                return rclcpp::Time(pf.imu_gyro_ts(imu_buf));
-            }};
+            timestamper = Timestamper{
+                [pf](const sensor::ImuPacket& imu_packet) {
+                    auto ts = pf.imu_gyro_ts(imu_packet.buf.data());
+                    return rclcpp::Time(ts);
+                }};
         }
 
-        return [&pf, &frame, timestamper](const uint8_t* imu_buf) {
-            return packet_to_imu_msg(pf, timestamper(imu_buf), frame, imu_buf);
+        return [&pf, &frame, timestamper](const sensor::ImuPacket& imu_packet) {
+            return packet_to_imu_msg(pf, timestamper(imu_packet), frame,
+                                     imu_packet.buf.data());
         };
     }
 };