├── launch
    ├── debug.conf
    ├── viz.launch
    ├── example.launch
    ├── driver.launch
    └── decoder.launch
├── .clang-format
├── .gitignore
├── package.xml
├── CMakeLists.txt
├── .cmake-format.yaml
├── .clang-tidy
├── LICENSE
├── cmake
    └── CompilerWarnings.cmake
├── README.md
└── src
    ├── viz.cpp
    ├── lidar.h
    ├── lidar.cpp
    ├── driver.cpp
    └── decoder.cpp


/launch/debug.conf:
--------------------------------------------------------------------------------
1 | log4j.logger.ros.ouster_decoder=DEBUG
2 | 


--------------------------------------------------------------------------------
/.clang-format:
--------------------------------------------------------------------------------
1 | BasedOnStyle: Google
2 | BinPackArguments: false
3 | BinPackParameters: false
4 | PointerAlignment: Left
5 | DerivePointerAlignment: false
6 | 


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | # Prerequisites
 2 | *.d
 3 | 
 4 | # Compiled Object files
 5 | *.slo
 6 | *.lo
 7 | *.o
 8 | *.obj
 9 | 
10 | # Precompiled Headers
11 | *.gch
12 | *.pch
13 | 
14 | # Compiled Dynamic libraries
15 | *.so
16 | *.dylib
17 | *.dll
18 | 
19 | # Fortran module files
20 | *.mod
21 | *.smod
22 | 
23 | # Compiled Static libraries
24 | *.lai
25 | *.la
26 | *.a
27 | *.lib
28 | 
29 | # Executables
30 | *.exe
31 | *.out
32 | *.app
33 | 
34 | build/
35 | .vscode/
36 | ouster_decoder.*
37 | 


--------------------------------------------------------------------------------
/launch/viz.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     <arg name="debug" default="false"/>
 3 |     <env if="$(arg debug)" name="ROSCONSOLE_CONFIG_FILE" value="$(find ouster_decoder)/launch/debug.conf"/>
 4 | 
 5 |     <arg name="lidar_ns" default="os_node"/>
 6 |     <arg name="driver_ns" default="/os_node"/>
 7 | 
 8 |     <node pkg="ouster_decoder" name="os_viz" type="ouster_viz" output="screen" ns="$(arg lidar_ns)">
 9 |         <remap from="~image" to="image"/>
10 |         <remap from="~camera_info" to="camera_info"/>
11 |     </node>
12 | </launch>


--------------------------------------------------------------------------------
/launch/example.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     <arg name="debug" default="false"/>
 3 |     <env if="$(arg debug)" name="ROSCONSOLE_CONFIG_FILE" value="$(find ouster_decoder)/launch/debug.conf"/>
 4 | 
 5 |     <arg name="lidar_ns" default="os"/>
 6 | 
 7 |     <node pkg="ouster_decoder" type="ouster_decoder_example" name="os_example" output="screen" ns="$(arg lidar_ns)">
 8 |         <!-- input topics -->
 9 |         <remap from="~cloud_me" to="cloud"/>
10 |         <remap from="~cloud_os" to="/os_cloud/points"/>
11 |     </node>
12 | 
13 | </launch>


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |     <name>ouster_decoder</name>
 4 |     <version>0.1.0</version>
 5 |     <description>Ouster decoder</description>
 6 |     <maintainer email="quchao@seas.upenn.edu">Chao Qu</maintainer>
 7 |     <license>BSD</license>
 8 |     <buildtool_depend>catkin</buildtool_depend>
 9 | 
10 |     <depend>cv_bridge</depend>
11 |     <depend>image_transport</depend>
12 |     <depend>pcl_ros</depend>
13 |     <depend>sensor_msgs</depend>
14 | 
15 |     <depend>ouster_ros</depend>
16 | 
17 |     <export></export>
18 | </package>
19 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.15)
 2 | project(ouster_decoder)
 3 | 
 4 | set(CMAKE_CXX_STANDARD 17)
 5 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
 6 | include(CompilerWarnings)
 7 | 
 8 | find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport pcl_ros
 9 |                                         sensor_msgs ouster_ros)
10 | catkin_package()
11 | 
12 | add_executable(ouster_decoder src/lidar.cpp src/decoder.cpp)
13 | target_include_directories(ouster_decoder PRIVATE src ${catkin_INCLUDE_DIRS})
14 | target_link_libraries(ouster_decoder PRIVATE ${catkin_LIBRARIES})
15 | enable_warnings(ouster_decoder)
16 | 
17 | add_executable(ouster_driver src/driver.cpp)
18 | target_include_directories(ouster_driver PRIVATE ${catkin_INCLUDE_DIRS})
19 | target_link_libraries(ouster_driver PRIVATE ${catkin_LIBRARIES})
20 | 


--------------------------------------------------------------------------------
/.cmake-format.yaml:
--------------------------------------------------------------------------------
 1 | additional_commands:
 2 |   catkin_package:
 3 |     kwargs:
 4 |       INCLUDE_DIRS: "*"
 5 |       LIBRARIES: "*"
 6 |       CATKIN_DEPENDS: "*"
 7 |       DEPENDS: "*"
 8 |       CFG_EXTRAS: "*"
 9 |   cc_library:
10 |     flags:
11 |       - INTERFACE
12 |     kwargs:
13 |       HDRS: "*"
14 |       SRCS: "*"
15 |       COPTS: "*"
16 |       DEFINES: "*"
17 |       LINKOPTS: "*"
18 |       DEPS: "*"
19 |       INCDIRS: "*"
20 |   cc_binary:
21 |     kwargs:
22 |       SRCS: "*"
23 |       COPTS: "*"
24 |       DEFINES: "*"
25 |       LINKOPTS: "*"
26 |       DEPS: "*"
27 |   cc_test:
28 |     kwargs:
29 |       SRCS: "*"
30 |       COPTS: "*"
31 |       DEFINES: "*"
32 |       LINKOPTS: "*"
33 |       DEPS: "*"
34 |   cc_bench:
35 |     kwargs:
36 |       SRCS: "*"
37 |       COPTS: "*"
38 |       DEFINES: "*"
39 |       LINKOPTS: "*"
40 |       DEPS: "*"
41 | dangle_parens: false
42 | line_ending: unix
43 | line_width: 80
44 | tab_size: 2
45 | 


--------------------------------------------------------------------------------
/.clang-tidy:
--------------------------------------------------------------------------------
 1 | ---
 2 | # NOTE there must be no spaces before the '-', so put the comma last.
 3 | InheritParentConfig: true
 4 | Checks: '
 5 | bugprone-*,
 6 | -bugprone-forward-declaration-namespace,
 7 | -bugprone-macro-parentheses,
 8 | -bugprone-lambda-function-name,
 9 | -bugprone-reserved-identifier,
10 | cppcoreguidelines-*,
11 | -cppcoreguidelines-avoid-magic-numbers,
12 | -cppcoreguidelines-interfaces-global-init,
13 | -cppcoreguidelines-macro-usage,
14 | -cppcoreguidelines-owning-memory,
15 | -cppcoreguidelines-pro-bounds-array-to-pointer-decay,
16 | -cppcoreguidelines-pro-bounds-constant-array-index,
17 | -cppcoreguidelines-pro-bounds-pointer-arithmetic,
18 | -cppcoreguidelines-pro-type-cstyle-cast,
19 | -cppcoreguidelines-pro-type-reinterpret-cast,
20 | -cppcoreguidelines-pro-type-static-cast-downcast,
21 | -cppcoreguidelines-pro-type-union-access,
22 | -cppcoreguidelines-pro-type-vararg,
23 | -cppcoreguidelines-special-member-functions,
24 | -facebook-hte-RelativeInclude,
25 | hicpp-exception-baseclass,
26 | hicpp-avoid-goto,
27 | modernize-*,
28 | -modernize-concat-nested-namespaces,
29 | -modernize-return-braced-init-list,
30 | -modernize-use-auto,
31 | -modernize-use-default-member-init,
32 | -modernize-use-using,
33 | -modernize-use-trailing-return-type,
34 | performance-*,
35 | -performance-noexcept-move-constructor,
36 | -performance-unnecessary-value-param,
37 | '
38 | HeaderFilterRegex: 'torch/csrc/.*'
39 | AnalyzeTemporaryDtors: false
40 | CheckOptions:
41 | ...


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | BSD 3-Clause License
 2 | 
 3 | Copyright (c) 2018, ouster-lidar
 4 | All rights reserved.
 5 | 
 6 | Redistribution and use in source and binary forms, with or without
 7 | modification, are permitted provided that the following conditions are met:
 8 | 
 9 | * Redistributions of source code must retain the above copyright notice, this
10 |   list of conditions and the following disclaimer.
11 | 
12 | * Redistributions in binary form must reproduce the above copyright notice,
13 |   this list of conditions and the following disclaimer in the documentation
14 |   and/or other materials provided with the distribution.
15 | 
16 | * Neither the name of the copyright holder nor the names of its
17 |   contributors may be used to endorse or promote products derived from
18 |   this software without specific prior written permission.
19 | 
20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 | 


--------------------------------------------------------------------------------
/launch/driver.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     <arg name="sensor_hostname" default="" doc="hostname or IP in dotted decimal form of the sensor"/>
 3 |     <arg name="udp_dest" default="" doc="hostname or IP where the sensor will send data packets"/>
 4 |     <arg name="lidar_port" default="0" doc="port to which the sensor should send lidar data"/>
 5 |     <arg name="imu_port" default="0" doc="port to which the sensor should send imu data"/>
 6 |     <arg name="replay" default="true" doc="do not connect to a sensor; expect /os_node/{lidar,imu}_packets from replay"/>
 7 |     <arg name="lidar_mode" default="" doc="resolution and rate: either 512x10, 512x20, 1024x10, 1024x20, or 2048x10"/>
 8 |     <arg name="timestamp_mode" default="" doc="method used to timestamp measurements: TIME_FROM_INTERNAL_OSC, TIME_FROM_SYNC_PULSE_IN, TIME_FROM_PTP_1588"/>
 9 |     <arg name="metadata" default="" doc="override default metadata file for replays"/>
10 |     <arg name="tf_prefix" default="" doc="namespace for tf transforms"/>
11 |     <arg name="driver_ns" default="os_node"/>
12 | 
13 |     <!-- <node pkg="ouster_ros" type="os_node" name="os_node" output="screen" required="true"> -->
14 |     <node pkg="ouster_decoder" type="ouster_driver" name="$(arg driver_ns)" output="screen" required="true">
15 |         <param name="~/lidar_mode" type="string" value="$(arg lidar_mode)"/>
16 |         <param name="~/timestamp_mode" type="string" value="$(arg timestamp_mode)"/>
17 |         <param name="~/replay" value="$(arg replay)"/>
18 |         <param name="~/sensor_hostname" value="$(arg sensor_hostname)"/>
19 |         <param name="~/udp_dest" value="$(arg udp_dest)"/>
20 |         <param name="~/lidar_port" value="$(arg lidar_port)"/>
21 |         <param name="~/imu_port" value="$(arg imu_port)"/>
22 |         <param name="~/metadata" value="$(arg metadata)"/>
23 |     </node>
24 | </launch>
25 | 


--------------------------------------------------------------------------------
/cmake/CompilerWarnings.cmake:
--------------------------------------------------------------------------------
 1 | # from here:
 2 | #
 3 | # https://github.com/lefticus/cppbestpractices/blob/master/02-Use_the_Tools_Available.md
 4 | 
 5 | function(enable_warnings target)
 6 | 
 7 |   set(CLANG_WARNINGS
 8 |       -Wall
 9 |       -Wextra # reasonable and standard
10 |       # -Wshadow # warn the user if a variable declaration shadows one from a
11 |       # parent context
12 |       -Wnon-virtual-dtor # warn the user if a class with virtual functions has a
13 |                          # non-virtual destructor. This helps catch hard to
14 |                          # track down memory errors
15 |       # -Wold-style-cast # warn for c-style casts
16 |       -Wcast-align # warn for potential performance problem casts
17 |       -Wunused # warn on anything being unused
18 |       -Woverloaded-virtual # warn if you overload (not override) a virtual
19 |                            # function
20 |       # -Wpedantic # warn if non-standard C++ is used
21 |       # -Wconversion # warn on type conversions that may lose data
22 |       # -Wsign-conversion # warn on sign conversions
23 |       -Wnull-dereference # warn if a null dereference is detected
24 |       # -Wdouble-promotion # warn if float is implicit promoted to double
25 |       -Wformat=2 # warn on security issues around functions that format output
26 |                  # (ie printf)
27 |   )
28 | 
29 |   set(GCC_WARNINGS
30 |       ${CLANG_WARNINGS}
31 |       -Wmisleading-indentation # warn if indentation implies blocks where blocks
32 |                                # do not exist
33 |       -Wduplicated-cond # warn if if / else chain has duplicated conditions
34 |       # -Wduplicated-branches # warn if if / else branches have duplicated code
35 |       -Wlogical-op # warn about logical operations being used where bitwise were
36 |                    # probably wanted
37 |       # -Wuseless-cast # warn if you perform a cast to the same type
38 |   )
39 | 
40 |   if(CMAKE_CXX_COMPILER_ID MATCHES ".*Clang")
41 |     set(PROJECT_WARNINGS ${CLANG_WARNINGS})
42 |   elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
43 |     set(PROJECT_WARNINGS ${GCC_WARNINGS})
44 |   else()
45 |     message(
46 |       AUTHOR_WARNING
47 |         "No compiler warnings set for '${CMAKE_CXX_COMPILER_ID}' compiler.")
48 |   endif()
49 | 
50 |   target_compile_options(${target} INTERFACE ${PROJECT_WARNINGS})
51 | 
52 | endfunction()
53 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # ouster_decoder
 2 | 
 3 | This decoder is intended to extend the ouster_ros package from https://github.com/ouster-lidar/ouster_example
 4 | 
 5 | It has very low latency (<0.2ms) compared to ouster_example (>3ms), tested on Intel i7 11th gen cpu.
 6 | 
 7 | The decoder only supports LEGACY and single return profile. Currently there's no plan for dual return profile.
 8 | 
 9 | ## Important
10 | 
11 | The timestamp of both the cloud and image message is the time of the last column, not the first column.
12 | This is different from the official driver, which uses timestamp of the first column. 
13 | 
14 | ## Usage
15 | 
16 | Run the ouster driver 
17 | ```
18 | roslaunch ouster_decoder driver.launch replay:=true/false
19 | ```
20 | 
21 | Then run the decoder
22 | ```
23 | roslaunch ouster_decoder decoder.launch
24 | ```
25 | 
26 | The driver node is the same as the one from `ouster_example` except that it publishes a string message to topic `/os_node/metadata` that you should also record. When in replay mode, there's no need to specify a metadata file. The metadata file will still be saved in case one forgot to record the metadata message.
27 | 
28 | ## Decoder
29 | 
30 | The decoder allocates storge for a range image and a point cloud on startup.
31 | When receiving a packet, it fills the range image and the point cloud instead of doing all computations at the end of a sweep. This drastically redueces latency when publishing the cloud/image.
32 | 
33 | Each pixel in the range image has the following fields:
34 | ```
35 | struct Data {
36 |     float x;
37 |     float y;
38 |     float z;
39 |     uint16_t r16u; // range
40 |     uint16_t s16u; // signal
41 | };
42 | ```
43 | 
44 | The corresponding point cloud has point type XYZI.
45 | 
46 | During each callback, we process the incoming packet and immediately decode and convert to 3d points. When we reach the end of a sweep, the data is directly published without any extra computations.
47 | 
48 | Therefore, the publish latency of our decoder is typically less than 0.2ms, while the ouster `os_cloud_node` takes more than 3ms to publish a point cloud.
49 | 
50 | ## Data Drops
51 | 
52 | Our decoder also checks for missing data. This is very useful for debugging purposes. The decoder tracks the frame and column id of each packet and reports any jumps (both forward and backward) if detected.
53 | The decoder then makes sure that missing data is zeroed out in the message. 
54 | 
55 | Therefore, one can safely stop and replay a recorded rosbag without restarting the driver and decoder.
56 | 


--------------------------------------------------------------------------------
/src/viz.cpp:
--------------------------------------------------------------------------------
  1 | #include <cv_bridge/cv_bridge.h>
  2 | #include <fmt/core.h>
  3 | #include <image_transport/image_transport.h>
  4 | #include <ros/ros.h>
  5 | #include <sensor_msgs/CameraInfo.h>
  6 | #include <sensor_msgs/Image.h>
  7 | #include <sensor_msgs/PointCloud2.h>
  8 | 
  9 | #include <opencv2/highgui.hpp>
 10 | #include <opencv2/imgproc.hpp>
 11 | 
 12 | namespace ouster_decoder {
 13 | 
 14 | namespace it = image_transport;
 15 | namespace sm = sensor_msgs;
 16 | 
 17 | cv::Mat ApplyCmap(const cv::Mat& input, int cmap, uint8_t bad_color) {
 18 |   cv::Mat disp;
 19 |   input.convertTo(disp, CV_8UC1);
 20 |   cv::applyColorMap(disp, disp, cmap);
 21 |   return disp;
 22 | }
 23 | 
 24 | class Viz {
 25 |  public:
 26 |   explicit Viz(const ros::NodeHandle& pnh);
 27 | 
 28 |   void CameraCb(const sm::ImageConstPtr& image_ptr,
 29 |                 const sm::CameraInfoConstPtr& cinfo_ptr);
 30 | 
 31 |  private:
 32 |   ros::NodeHandle pnh_;
 33 |   it::ImageTransport it_;
 34 |   it::CameraSubscriber sub_camera_;
 35 |   std::string cmap_{"gray"};
 36 | };
 37 | 
 38 | Viz::Viz(const ros::NodeHandle& pnh) : pnh_{pnh}, it_{pnh} {
 39 |   sub_camera_ = it_.subscribeCamera("image", 1, &Viz::CameraCb, this);
 40 |   ROS_INFO_STREAM("Subscribing to: " << sub_camera_.getTopic());
 41 | }
 42 | 
 43 | void Viz::CameraCb(const sm::ImageConstPtr& image_ptr,
 44 |                    const sm::CameraInfoConstPtr& cinfo_ptr) {
 45 |   const auto mat = cv_bridge::toCvShare(image_ptr)->image;
 46 |   const auto h = mat.rows;
 47 |   const auto w = mat.cols;
 48 | 
 49 |   // Extract range and intensity
 50 |   const cv::Mat mat_map(mat.rows, mat.cols, CV_16UC(8), mat.data);
 51 |   cv::Mat range_raw, signal_raw;
 52 |   cv::extractChannel(mat_map, range_raw, 6);
 53 |   cv::extractChannel(mat_map, signal_raw, 7);
 54 | 
 55 |   auto range_color = ApplyCmap(range_raw / 100, cv::COLORMAP_PINK, 0);
 56 |   auto signal_color = ApplyCmap(signal_raw / 4, cv::COLORMAP_PINK, 0);
 57 | 
 58 |   // set invalid range (0) to black
 59 |   range_color.setTo(0, range_raw == 0);
 60 |   signal_color.setTo(0, range_raw == 0);
 61 | 
 62 |   cv::Mat signal_half0;
 63 |   cv::Mat signal_half1;
 64 |   signal_half0.create(h / 2, w, CV_8UC3);
 65 |   signal_half1.create(h / 2, w, CV_8UC3);
 66 | 
 67 |   for (int r = 0; r < h; ++r) {
 68 |     const int rr = r / 2;
 69 |     if (r % 2 == 0) {
 70 |       signal_color.row(r).copyTo(signal_half0.row(rr));
 71 |     } else {
 72 |       signal_color.row(r).copyTo(signal_half1.row(rr));
 73 |     }
 74 |   }
 75 | 
 76 |   // save raw data to ouster
 77 |   const int id = image_ptr->header.seq;
 78 |   const std::vector<int> png_flags = {cv::IMWRITE_PNG_COMPRESSION, 0};
 79 |   cv::imwrite(fmt::format("/tmp/ouster/range_{:04d}.png", id), range_raw, png_flags);
 80 |   cv::imwrite(fmt::format("/tmp/ouster/signal_{:04d}.png", id), signal_raw, png_flags);
 81 |   ROS_INFO_STREAM("Writing message with id: " << id);
 82 | 
 83 |   constexpr auto win_flags =
 84 |       cv::WINDOW_FREERATIO | cv::WINDOW_GUI_EXPANDED | cv::WINDOW_NORMAL;
 85 | 
 86 |   cv::namedWindow("range", win_flags);
 87 |   cv::imshow("range", range_color);
 88 | 
 89 |   cv::namedWindow("signal", win_flags);
 90 |   cv::imshow("signal", signal_color);
 91 | 
 92 |   cv::namedWindow("signal_half0", win_flags);
 93 |   cv::imshow("signal_half0", signal_half0);
 94 | 
 95 |   cv::namedWindow("signal_half1", win_flags);
 96 |   cv::imshow("signal_half1", signal_half1);
 97 | 
 98 |   cv::waitKey(1);
 99 | }
100 | 
101 | }  // namespace ouster_decoder
102 | 
103 | int main(int argc, char** argv) {
104 |   ros::init(argc, argv, "os_viz");
105 | 
106 |   ouster_decoder::Viz node(ros::NodeHandle("~"));
107 |   ros::spin();
108 | 
109 |   return 0;
110 | }
111 | 


--------------------------------------------------------------------------------
/launch/decoder.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     <arg name="debug" default="false"/>
 3 |     <env if="$(arg debug)" name="ROSCONSOLE_CONFIG_FILE" value="$(find ouster_decoder)/launch/debug.conf"/>
 4 | 
 5 |     <arg name="lidar_ns" default="os_node"/>
 6 |     <arg name="driver_ns" default="/os_node"/>
 7 | 
 8 |     <!-- ouster: https://data.ouster.io/downloads/datasheets/datasheet-revd-v2p1-os1.pdf -->
 9 |     <!-- anything < min_range or > max_range will be set to nan -->
10 |     <arg name="min_range" default="0.5"/>
11 |     <arg name="max_range" default="127.0"/>
12 |     <arg name="range_scale" default="512.0"/>
13 |     <!-- Gravity at philadelphia from https://www.ngs.noaa.gov/cgi-bin/grav_pdx.prl -->
14 |     <!-- Lat: 39.9526 N, Lon: 75.1652 W, MSL Height: 11.8872, Predicted gravity:  980184 +/- 2 milligals -->
15 |     <arg name="gravity" default="9.80184"/>
16 |     <!-- Divide one scan to n subscans, default is 1, can be 2, 4, 8, ... -->
17 |     <arg name="divide" default="1"/>
18 |     <!-- Replay mode will reinitilize when detecting forward/backward jumps -->
19 |     <arg name="replay" default="true"/>
20 |     <!-- Scale signal channel for visualization only -->
21 |     <arg name="vis_signal_scale" default="4.0"/>
22 | 
23 |     <!-- Destagger and align are currently disabled since there's really no point in changing them -->
24 |     <!-- Destagger will be disabled if div_exp > 0 or subscan > 1.
25 |     It is strongly advised to set destagger to false. 
26 |     Due to the non-central projection model of the outser lidar, destaggering by pixel offset 
27 |     would still produce artifacts in the range image.
28 |     It is recommended to re-compute a range image from the point cloud if needed -->
29 |     <arg name="destagger" default="false"/>
30 | 
31 |     <!-- imu: https://3cfeqx1hf82y3xcoull08ihx-wpengine.netdna-ssl.com/wp-content/uploads/2021/07/DS-000189-ICM-20948-v1.4.pdf -->
32 |     <!-- These are all continuous time noise density, need to multiply by sqrt(rate) to get distcrete time noise density -->
33 |     <!-- which we will do in the node, so the imu covariance is in discrete time -->
34 |     <!-- https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model -->
35 |     <!-- acc noise density 0.00023 g/sqrt(Hz) -->
36 |     <!-- gyr noise density 0.00026 rad/sqrt(Hz) -->
37 |     <arg name="acc_noise_std" default="0.0023"/>
38 |     <arg name="gyr_noise_std" default="0.00026"/>
39 | 
40 |     <!-- Compatible with ouster_ros  -->
41 |     <arg name="sensor_frame" default="os_sensor"/>
42 |     <arg name="lidar_frame" default="os_lidar"/>
43 |     <arg name="imu_frame" default="os_imu"/>
44 | 
45 |     <node pkg="ouster_decoder" name="os_decoder" type="ouster_decoder" output="screen" ns="$(arg lidar_ns)">
46 |         <!-- params -->
47 |         <param name="divide" type="int" value="$(arg divide)"/>
48 |         <param name="replay" type="bool" value="$(arg replay)"/>
49 |         <param name="gravity" type="double" value="$(arg gravity)"/>
50 |         <param name="destagger" type="bool" value="$(arg destagger)"/>
51 | 
52 |         <param name="min_range" type="double" value="$(arg min_range)"/>
53 |         <param name="max_range" type="double" value="$(arg max_range)"/>
54 |         <param name="range_scale" type="double" value="$(arg range_scale)"/>
55 |         <param name="vis_signal_scale" type="double" value="$(arg vis_signal_scale)"/>
56 | 
57 |         <param name="acc_noise_std" type="double" value="$(arg acc_noise_std)"/>
58 |         <param name="gyr_noise_std" type="double" value="$(arg gyr_noise_std)"/>
59 | 
60 |         <!-- frames -->
61 |         <param name="imu_frame" type="string" value="$(arg imu_frame)"/>
62 |         <param name="lidar_frame" type="string" value="$(arg lidar_frame)"/>
63 |         <param name="sensor_frame" type="string" value="$(arg sensor_frame)"/>
64 | 
65 |         <!-- input topics -->
66 |         <remap from="~metadata" to="$(arg driver_ns)/metadata"/>
67 |         <remap from="~get_metadata" to="$(arg driver_ns)/get_metadata"/>
68 |         <remap from="~imu_packets" to="$(arg driver_ns)/imu_packets"/>
69 |         <remap from="~lidar_packets" to="$(arg driver_ns)/lidar_packets"/>
70 | 
71 |         <!-- output topics -->
72 |         <remap from="~imu" to="imu"/>
73 |         <remap from="~cloud" to="cloud"/>
74 |         <remap from="~image" to="image"/>
75 |         <remap from="~camera_info" to="camera_info"/>
76 |         <remap from="~range" to="range"/>
77 |         <remap from="~signal" to="signal"/>
78 |     </node>
79 | </launch>
80 | 


--------------------------------------------------------------------------------
/src/lidar.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <sensor_msgs/CameraInfo.h>
  4 | #include <sensor_msgs/Image.h>
  5 | #include <sensor_msgs/PointCloud2.h>
  6 | 
  7 | #include "ouster_ros/os_ros.h"
  8 | 
  9 | namespace ouster_decoder {
 10 | 
 11 | inline constexpr double Deg2Rad(double deg) { return deg * M_PI / 180.0; }
 12 | 
 13 | /// @brief image data in scan
 14 | struct ImageData {
 15 |   float x{};
 16 |   float y{};
 17 |   float z{};
 18 |   uint16_t r16u{};
 19 |   uint16_t s16u{};
 20 | 
 21 |   void set_range(float range, double scale) noexcept {
 22 |     r16u = static_cast<uint16_t>(
 23 |         std::min(range * scale,
 24 |                  static_cast<double>(std::numeric_limits<uint16_t>::max())));
 25 |   }
 26 | 
 27 |   void set_bad() noexcept {
 28 |     x = y = z = std::numeric_limits<float>::quiet_NaN();
 29 |     r16u = s16u = 0;
 30 |   }
 31 | };
 32 | 
 33 | static_assert(sizeof(ImageData) == sizeof(float) * 4,
 34 |               "Size of ImageData must be 4 floats (16 bytes)");
 35 | 
 36 | /// @brief Stores SensorInfo from ouster with some other useful data
 37 | struct LidarModel {
 38 |   LidarModel() = default;
 39 |   explicit LidarModel(const std::string& metadata);
 40 | 
 41 |   /// @brief whether this model is ready
 42 |   bool Initialized() const { return !altitudes.empty(); }
 43 | 
 44 |   int rows{};                     // number of beams
 45 |   int cols{};                     // cols of a full scan
 46 |   int freq{};                     // frequency
 47 |   double dt_col{};                // delta time between two columns [s]
 48 |   double dt_packet{};             // delta time between two packets [s]
 49 |   double d_azimuth{};             // delta angle between two columns [rad]
 50 |   double beam_offset{};           // distance between beam to origin
 51 |   std::vector<double> altitudes;  // altitude angles, high to low [rad]
 52 |   std::vector<double> azimuths;   // azimuths offset angles [rad]
 53 |   ouster_ros::sensor::sensor_info info;                  // sensor info
 54 |   ouster_ros::sensor::packet_format const* pf{nullptr};  // packet format
 55 | 
 56 |   const auto& pixel_shifts() const noexcept {
 57 |     return info.format.pixel_shift_by_row;
 58 |   }
 59 | 
 60 |   /// @brief Convert lidar range data to xyz
 61 |   /// @details see software manual 3.1.2 Lidar Range to XYZ
 62 |   ///
 63 |   ///    y    r
 64 |   ///    ^   / -> rotate clockwise
 65 |   ///    |  /
 66 |   ///    | /
 67 |   ///    |/  theta
 68 |   ///    o ---------> x  (connector)
 69 |   ///
 70 |   Eigen::Vector3f ToPoint(float range, float theta_enc, int row) const;
 71 | 
 72 |   /// @brief Return a unique id for a measurement
 73 |   int Uid(int fid, int mid) const noexcept { return fid * cols + mid; }
 74 | 
 75 |   /// @brief Update camera info with this model
 76 |   void UpdateCameraInfo(sensor_msgs::CameraInfo& cinfo) const;
 77 | };
 78 | 
 79 | /// @brief Stores data for a (sub)scan
 80 | struct LidarScan {
 81 |   int icol{0};              // column index
 82 |   int iscan{0};             // subscan index
 83 |   int prev_uid{-1};         // previous uid
 84 |   double min_range{0.25};   // minimum range
 85 |   double max_range{256.0};  // maximum range
 86 |   double range_scale{};     // raw range is  uint_16, divide by scale to meter
 87 |   bool destagger{false};    // whether to destagger scan
 88 | 
 89 |   //  cv::Mat image;
 90 |   sensor_msgs::ImagePtr image_ptr;  // each pixel is ImageData
 91 |   sensor_msgs::PointCloud2 cloud;   // each point is (x,y,z,intensity)
 92 |   std::vector<uint64_t> times;      // all time stamps [nanosecond]
 93 | 
 94 |   float* CloudPtr(int r, int c) {
 95 |     const auto i = r * cols() + c;
 96 |     return reinterpret_cast<float*>(cloud.data.data() + i * cloud.point_step);
 97 |   }
 98 | 
 99 |   ImageData* ImagePtr(int r, int c) {
100 |     const auto i = r * cols() + c;
101 |     return reinterpret_cast<ImageData*>(image_ptr->data.data() +
102 |                                         i * sizeof(ImageData));
103 |   }
104 | 
105 |   int rows() const noexcept { return static_cast<int>(cloud.height); }
106 |   int cols() const noexcept { return static_cast<int>(cloud.width); }
107 | 
108 |   /// @brief whether this scan is full
109 |   bool IsFull() const noexcept { return icol >= cols(); }
110 | 
111 |   /// @brief Starting column of this scan
112 |   int StartingCol() const noexcept { return iscan * cols(); }
113 | 
114 |   /// @brief Detect if there is a jump in the lidar data
115 |   /// @return 0 - no jump, >0 - jump forward in time, <0 - jump backward in time
116 |   int DetectJump(int uid) noexcept;
117 | 
118 |   /// @brief Allocate storage for the scan
119 |   void Allocate(int rows, int cols);
120 | 
121 |   /// @brief Hard reset internal counters and prev_uid
122 |   void HardReset() noexcept;
123 | 
124 |   /// @brief Try to reset the internal counters if it is full
125 |   void SoftReset(int full_col) noexcept;
126 | 
127 |   /// @brief Invalidate an entire column
128 |   void InvalidateColumn(double dt_col) noexcept;
129 | 
130 |   /// @brief Decode column
131 |   void DecodeColumn(const uint8_t* const col_buf, const LidarModel& model);
132 | 
133 |   /// @brief Update camera info roi data with this scan
134 |   void UpdateCinfo(sensor_msgs::CameraInfo& cinfo) const noexcept;
135 | };
136 | 
137 | std::vector<sensor_msgs::PointField> MakePointFieldsXYZI() noexcept;
138 | 
139 | }  // namespace ouster_decoder
140 | 


--------------------------------------------------------------------------------
/src/lidar.cpp:
--------------------------------------------------------------------------------
  1 | #include "lidar.h"
  2 | 
  3 | #include <boost/make_shared.hpp>
  4 | 
  5 | namespace ouster_decoder {
  6 | 
  7 | namespace os = ouster_ros::sensor;
  8 | 
  9 | constexpr float kMmToM = 0.001;
 10 | constexpr double kTau = 2 * M_PI;
 11 | constexpr float kNaNF = std::numeric_limits<float>::quiet_NaN();
 12 | 
 13 | namespace {
 14 | 
 15 | /// @brief Convert a vector of double from deg to rad
 16 | std::vector<double> TransformDeg2Rad(const std::vector<double>& degs) {
 17 |   std::vector<double> rads;
 18 |   rads.reserve(degs.size());
 19 |   for (const auto& deg : degs) {
 20 |     rads.push_back(Deg2Rad(deg));
 21 |   }
 22 |   return rads;
 23 | }
 24 | 
 25 | }  // namespace
 26 | 
 27 | LidarModel::LidarModel(const std::string& metadata) {
 28 |   info = os::parse_metadata(metadata);
 29 |   pf = &os::get_format(info);
 30 | 
 31 |   rows = info.beam_altitude_angles.size();
 32 |   cols = os::n_cols_of_lidar_mode(info.mode);
 33 |   freq = os::frequency_of_lidar_mode(info.mode);
 34 | 
 35 |   dt_col = 1.0 / freq / cols;
 36 |   d_azimuth = kTau / cols;
 37 |   dt_packet = dt_col * pf->columns_per_packet;
 38 |   beam_offset = info.lidar_origin_to_beam_origin_mm * kMmToM;
 39 |   altitudes = TransformDeg2Rad(info.beam_altitude_angles);
 40 |   azimuths = TransformDeg2Rad(info.beam_azimuth_angles);
 41 | }
 42 | 
 43 | Eigen::Vector3f LidarModel::ToPoint(float range,
 44 |                                     float theta_enc,
 45 |                                     int row) const {
 46 |   const float n = beam_offset;
 47 |   const float d = range - n;
 48 |   const float phi = altitudes[row];
 49 |   const float cos_phi = std::cos(phi);
 50 |   const float theta = theta_enc - azimuths[row];
 51 | 
 52 |   return {d * std::cos(theta) * cos_phi + n * std::cos(theta_enc),
 53 |           d * std::sin(theta) * cos_phi + n * std::sin(theta_enc),
 54 |           d * std::sin(phi)};
 55 | }
 56 | 
 57 | void LidarModel::UpdateCameraInfo(sensor_msgs::CameraInfo& cinfo) const {
 58 |   cinfo.height = rows;
 59 |   cinfo.width = cols;
 60 |   cinfo.distortion_model = info.prod_line;
 61 | 
 62 |   // cinfo.D.reserve(altitudes.size() + azimuths.size());
 63 |   // cinfo.D.insert(cinfo.D.end(), altitudes.begin(), altitudes.end());
 64 |   // cinfo.D.insert(cinfo.D.end(), azimuths.begin(), azimuths.end());
 65 |   cinfo.D.reserve(pixel_shifts().size());
 66 |   cinfo.D.insert(cinfo.D.end(), pixel_shifts().begin(), pixel_shifts().end());
 67 | 
 68 |   cinfo.K[0] = dt_col;  // time between each column
 69 |   // cinfo.K[1] = d_azimuth;    // radian between each column
 70 |   // cinfo.K[2] = beam_offset;  // distance from center to beam
 71 | }
 72 | 
 73 | void LidarScan::Allocate(int rows, int cols) {
 74 |   // Don't do any work if rows and cols are the same
 75 |   //  image.create(rows, cols, CV_32FC4);
 76 |   if (!image_ptr) image_ptr = boost::make_shared<sensor_msgs::Image>();
 77 | 
 78 |   image_ptr->height = rows;
 79 |   image_ptr->width = cols;
 80 |   image_ptr->encoding = "32FC4";
 81 |   image_ptr->step = cols * sizeof(ImageData);
 82 |   image_ptr->data.resize(rows * cols * sizeof(ImageData));
 83 | 
 84 |   cloud.width = cols;
 85 |   cloud.height = rows;
 86 |   cloud.point_step = 16;  // xyzi
 87 |   cloud.row_step = cloud.point_step * cloud.width;
 88 |   cloud.fields = MakePointFieldsXYZI();
 89 |   cloud.is_dense = 1u;
 90 |   cloud.data.resize(rows * cols * cloud.point_step);
 91 | 
 92 |   times.clear();
 93 |   times.resize(cols, 0);
 94 | }
 95 | 
 96 | int LidarScan::DetectJump(int uid) noexcept {
 97 |   int jump = 0;
 98 | 
 99 |   if (prev_uid >= 0) {
100 |     // Ideally the increment should be 1 hence the jump should be 0
101 |     jump = uid - prev_uid - 1;
102 |   }
103 | 
104 |   prev_uid = uid;
105 |   return jump;
106 | }
107 | 
108 | void LidarScan::HardReset() noexcept {
109 |   icol = 0;
110 |   iscan = 0;
111 |   prev_uid = -1;
112 | }
113 | 
114 | void LidarScan::SoftReset(int full_col) noexcept {
115 |   // Reset col (usually to 0 but in the rare case that data jumps forward
116 |   // it will be non-zero)
117 |   icol = icol % cols();
118 | 
119 |   // Reset scan if we have a full sweep
120 |   if (iscan * cols() >= full_col) {
121 |     iscan = 0;
122 |   }
123 | }
124 | 
125 | void LidarScan::InvalidateColumn(double dt_col) noexcept {
126 |   for (int irow = 0; irow < static_cast<int>(cloud.height); ++irow) {
127 |     auto* ptr = CloudPtr(irow, icol);
128 |     ptr[0] = ptr[1] = ptr[2] = kNaNF;
129 |   }
130 | 
131 |   for (int irow = 0; irow < rows(); ++irow) {
132 |     auto* ptr = ImagePtr(irow, icol);
133 |     ptr->set_bad();
134 |   }
135 | 
136 |   // It is possible that the jump spans two subscans, this will cause the
137 |   // first timestamp to be wrong when we publish the data, therefore we need
138 |   // to extrapolate timestamp here
139 |   times.at(icol) = (icol == 0 ? times.back() : times.at(icol - 1)) +
140 |                    static_cast<uint64_t>(dt_col * 1e9);
141 | 
142 |   // Move on to next column
143 |   ++icol;
144 | }
145 | 
146 | void LidarScan::DecodeColumn(const uint8_t* const col_buf,
147 |                              const LidarModel& model) {
148 |   const auto& pf = *model.pf;
149 |   const uint64_t t_ns = pf.col_timestamp(col_buf);
150 |   const uint16_t mid = pf.col_measurement_id(col_buf);
151 |   const uint32_t status = pf.col_status(col_buf) & 0x0001;
152 |   // Legacy mode
153 |   // const bool col_valid = (status == 0xffffffff);
154 |   //  const bool col_valid = status;
155 | 
156 |   // Compute azimuth angle theta0, this should always be valid
157 |   // const auto theta_enc = kTau - mid * model_.d_azimuth;
158 |   // const float theta_enc = kTau * (1.0f - encoder / 90112.0f);
159 |   const float theta_enc = kTau - mid * model.d_azimuth;
160 |   times.at(icol) = t_ns;
161 | 
162 |   for (int ipx = 0; ipx < pf.pixels_per_column; ++ipx) {
163 |     // Data to fill
164 |     Eigen::Vector3f xyz;
165 |     xyz.setConstant(kNaNF);
166 |     float r{};
167 |     uint16_t s16u{};
168 | 
169 |     if (status) {
170 |       const uint8_t* const px_buf = pf.nth_px(ipx, col_buf);
171 |       const auto raw_range = pf.px_range(px_buf);
172 |       const float range = raw_range * kMmToM;  // used to compute xyz
173 | 
174 |       if (min_range <= range && range <= max_range) {
175 |         xyz = model.ToPoint(range, theta_enc, ipx);
176 |         r = xyz.norm();  // we compute range ourselves
177 |         s16u = pf.px_signal(px_buf);
178 |       }
179 |       // s16u += pf.px_ambient(px_buf);
180 |     }
181 | 
182 |     // Now we set cloud and image data
183 |     // There is no destagger for cloud, so we update point no matter what
184 |     auto* cptr = CloudPtr(ipx, icol);
185 |     cptr[0] = xyz.x();
186 |     cptr[1] = xyz.y();
187 |     cptr[2] = xyz.z();
188 |     cptr[3] = static_cast<float>(s16u);
189 | 
190 |     // However image can be destaggered, and pixel can go out of bound
191 |     // add pixel shift to get where the pixel should be
192 |     const auto col_shift = model.pixel_shifts()[ipx];
193 |     const auto im_col = destagger ? icol + col_shift : icol;
194 | 
195 |     if (0 <= im_col && im_col < cols()) {
196 |       auto* iptr = ImagePtr(ipx, im_col);
197 |       iptr->x = xyz.x();
198 |       iptr->y = xyz.y();
199 |       iptr->z = xyz.z();
200 |       iptr->set_range(r, range_scale);
201 |       iptr->s16u = s16u;
202 |     } else {
203 |       auto* iptr = ImagePtr(ipx, im_col % cols());
204 |       iptr->set_bad();
205 |     }
206 |   }
207 | 
208 |   // Move on to next column
209 |   ++icol;
210 | }
211 | 
212 | void LidarScan::UpdateCinfo(sensor_msgs::CameraInfo& cinfo) const noexcept {
213 |   cinfo.R[0] = range_scale;
214 |   cinfo.binning_x = iscan;  // index of subscan within a full scan
215 | 
216 |   // Update camera info roi with curr_scan
217 |   auto& roi = cinfo.roi;
218 |   roi.x_offset = StartingCol();
219 |   roi.y_offset = 0;  // this is always 0
220 |   roi.width = cols();
221 |   roi.height = rows();
222 |   roi.do_rectify = destagger;
223 | }
224 | 
225 | std::vector<sensor_msgs::PointField> MakePointFieldsXYZI() noexcept {
226 |   using sensor_msgs::PointField;
227 |   std::vector<PointField> fields;
228 |   fields.reserve(4);
229 | 
230 |   PointField field;
231 |   field.name = "x";
232 |   field.offset = 0;
233 |   field.datatype = PointField::FLOAT32;
234 |   field.count = 1;
235 |   fields.push_back(field);
236 | 
237 |   field.name = "y";
238 |   field.offset = 4;
239 |   field.datatype = PointField::FLOAT32;
240 |   field.count = 1;
241 |   fields.push_back(field);
242 | 
243 |   field.name = "z";
244 |   field.offset = 8;
245 |   field.datatype = PointField::FLOAT32;
246 |   field.count = 1;
247 |   fields.push_back(field);
248 | 
249 |   field.name = "intensity";
250 |   field.offset = 12;
251 |   field.datatype = PointField::FLOAT32;
252 |   field.count = 1;
253 |   fields.push_back(field);
254 | 
255 |   return fields;
256 | }
257 | 
258 | }  // namespace ouster_decoder
259 | 


--------------------------------------------------------------------------------
/src/driver.cpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * Copyright (c) 2018, Ouster, Inc.
  3 |  * All rights reserved.
  4 |  *
  5 |  * @file
  6 |  * @brief Example node to publish raw sensor output on ROS topics
  7 |  *
  8 |  * ROS Parameters
  9 |  * sensor_hostname: hostname or IP in dotted decimal form of the sensor
 10 |  * udp_dest: hostname or IP where the sensor will send data packets
 11 |  * lidar_port: port to which the sensor should send lidar data
 12 |  * imu_port: port to which the sensor should send imu data
 13 |  */
 14 | 
 15 | // This is a modified version of ouster_ros/os_node.cpp
 16 | // It is intended to have the same behavior as os_node.
 17 | // The only difference is that we also advertise the metadata message.
 18 | 
 19 | #include <ros/ros.h>
 20 | #include <std_msgs/String.h>
 21 | 
 22 | #include <fstream>
 23 | #include <sstream>
 24 | #include <string>
 25 | 
 26 | #include "ouster_ros/GetMetadata.h"
 27 | #include "ouster_ros/PacketMsg.h"
 28 | #include "ouster_ros/os_ros.h"
 29 | 
 30 | using PacketMsg = ouster_ros::PacketMsg;
 31 | using OsGetMetadata = ouster_ros::GetMetadata;
 32 | namespace sensor = ouster::sensor;
 33 | 
 34 | // fill in values that could not be parsed from metadata
 35 | void populate_metadata_defaults(sensor::sensor_info& info,
 36 |                                 sensor::lidar_mode specified_lidar_mode) {
 37 |   if (!info.name.size()) info.name = "UNKNOWN";
 38 | 
 39 |   if (!info.sn.size()) info.sn = "UNKNOWN";
 40 | 
 41 |   ouster::util::version v = ouster::util::version_of_string(info.fw_rev);
 42 |   if (v == ouster::util::invalid_version)
 43 |     ROS_WARN("Unknown sensor firmware version; output may not be reliable");
 44 |   else if (v < sensor::min_version)
 45 |     ROS_WARN("Firmware < %s not supported; output may not be reliable",
 46 |              to_string(sensor::min_version).c_str());
 47 | 
 48 |   if (!info.mode) {
 49 |     ROS_WARN("Lidar mode not found in metadata; output may not be reliable");
 50 |     info.mode = specified_lidar_mode;
 51 |   }
 52 | 
 53 |   if (!info.prod_line.size()) info.prod_line = "UNKNOWN";
 54 | 
 55 |   if (info.beam_azimuth_angles.empty() || info.beam_altitude_angles.empty()) {
 56 |     ROS_WARN("Beam angles not found in metadata; using design values");
 57 |     info.beam_azimuth_angles = sensor::gen1_azimuth_angles;
 58 |     info.beam_altitude_angles = sensor::gen1_altitude_angles;
 59 |   }
 60 | }
 61 | 
 62 | // try to write metadata file
 63 | bool write_metadata(const std::string& meta_file, const std::string& metadata) {
 64 |   std::ofstream ofs;
 65 |   ofs.open(meta_file);
 66 |   ofs << metadata << std::endl;
 67 |   ofs.close();
 68 |   if (ofs) {
 69 |     ROS_INFO("Wrote metadata to %s", meta_file.c_str());
 70 |   } else {
 71 |     ROS_WARN(
 72 |         "Failed to write metadata to %s; check that the path is valid. If "
 73 |         "you provided a relative path, please note that the working "
 74 |         "directory of all ROS nodes is set by default to $ROS_HOME",
 75 |         meta_file.c_str());
 76 |     return false;
 77 |   }
 78 |   return true;
 79 | }
 80 | 
 81 | int connection_loop(ros::NodeHandle& nh,
 82 |                     sensor::client& cli,
 83 |                     const sensor::sensor_info& info) {
 84 |   auto lidar_packet_pub = nh.advertise<PacketMsg>("lidar_packets", 1280);
 85 |   auto imu_packet_pub = nh.advertise<PacketMsg>("imu_packets", 100);
 86 | 
 87 |   auto pf = sensor::get_format(info);
 88 | 
 89 |   PacketMsg lidar_packet, imu_packet;
 90 |   lidar_packet.buf.resize(pf.lidar_packet_size + 1);
 91 |   imu_packet.buf.resize(pf.imu_packet_size + 1);
 92 | 
 93 |   while (ros::ok()) {
 94 |     auto state = sensor::poll_client(cli);
 95 |     if (state == sensor::EXIT) {
 96 |       ROS_INFO("poll_client: caught signal, exiting");
 97 |       return EXIT_SUCCESS;
 98 |     }
 99 |     if (state & sensor::CLIENT_ERROR) {
100 |       ROS_ERROR("poll_client: returned error");
101 |       return EXIT_FAILURE;
102 |     }
103 |     if (state & sensor::LIDAR_DATA) {
104 |       if (sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf))
105 |         lidar_packet_pub.publish(lidar_packet);
106 |     }
107 |     if (state & sensor::IMU_DATA) {
108 |       if (sensor::read_imu_packet(cli, imu_packet.buf.data(), pf))
109 |         imu_packet_pub.publish(imu_packet);
110 |     }
111 |     ros::spinOnce();
112 |   }
113 |   return EXIT_SUCCESS;
114 | }
115 | 
116 | void advertise_service(ros::NodeHandle& nh,
117 |                        ros::ServiceServer& srv,
118 |                        const sensor::sensor_info& info) {
119 |   auto metadata = sensor::to_string(info);
120 |   ROS_INFO("Using lidar_mode: %s", sensor::to_string(info.mode).c_str());
121 |   ROS_INFO("%s sn: %s firmware rev: %s",
122 |            info.prod_line.c_str(),
123 |            info.sn.c_str(),
124 |            info.fw_rev.c_str());
125 |   if (srv) {
126 |     // shutdown first and readvertise
127 |     ROS_INFO("Shutting down %s service and re-advertise",
128 |              srv.getService().c_str());
129 |     srv.shutdown();
130 |   }
131 |   srv = nh.advertiseService<OsGetMetadata::Request, OsGetMetadata::Response>(
132 |       "get_metadata",
133 |       [metadata](OsGetMetadata::Request&, OsGetMetadata::Response& res) {
134 |         if (metadata.empty()) return false;
135 |         res.metadata = metadata;
136 |         return true;
137 |       });
138 |   ROS_INFO("Advertise service to %s", srv.getService().c_str());
139 | }
140 | 
141 | int main(int argc, char** argv) {
142 |   ros::init(argc, argv, "os_node");
143 |   ros::NodeHandle nh("~");
144 |   // Extra stuff
145 |   ros::Publisher pub_meta;
146 |   ros::Subscriber meta_sub;
147 |   ros::ServiceServer srv;
148 | 
149 |   // empty indicates "not set" since roslaunch xml can't optionally set params
150 |   auto hostname = nh.param("sensor_hostname", std::string{});
151 |   auto udp_dest = nh.param("udp_dest", std::string{});
152 |   auto lidar_port = nh.param("lidar_port", 0);
153 |   auto imu_port = nh.param("imu_port", 0);
154 |   auto replay = nh.param("replay", false);
155 |   auto lidar_mode_arg = nh.param("lidar_mode", std::string{});
156 |   auto timestamp_mode_arg = nh.param("timestamp_mode", std::string{});
157 | 
158 |   std::string udp_profile_lidar_arg;
159 |   nh.param<std::string>("udp_profile_lidar", udp_profile_lidar_arg, "");
160 | 
161 |   //   optional<sensor::UDPProfileLidar> udp_profile_lidar;
162 |   //   if (udp_profile_lidar_arg.size()) {
163 |   //     if (replay)
164 |   //       ROS_WARN("UDP Profile Lidar set in replay mode. Will be ignored.");
165 | 
166 |   //     // set lidar profile from param
167 |   //     udp_profile_lidar =
168 |   //         sensor::udp_profile_lidar_of_string(udp_profile_lidar_arg);
169 |   //     if (!udp_profile_lidar) {
170 |   //       ROS_ERROR("Invalid udp profile lidar: %s",
171 |   //       udp_profile_lidar_arg.c_str()); return EXIT_FAILURE;
172 |   //     }
173 |   //   }
174 | 
175 |   // set lidar mode from param
176 |   sensor::lidar_mode lidar_mode = sensor::MODE_UNSPEC;
177 |   if (lidar_mode_arg.size()) {
178 |     if (replay) ROS_WARN("Lidar mode set in replay mode. Will be ignored");
179 | 
180 |     lidar_mode = sensor::lidar_mode_of_string(lidar_mode_arg);
181 |     if (!lidar_mode) {
182 |       ROS_ERROR("Invalid lidar mode %s", lidar_mode_arg.c_str());
183 |       return EXIT_FAILURE;
184 |     }
185 |   }
186 | 
187 |   // set timestamp mode from param
188 |   sensor::timestamp_mode timestamp_mode = sensor::TIME_FROM_UNSPEC;
189 |   if (timestamp_mode_arg.size()) {
190 |     if (replay) ROS_WARN("Timestamp mode set in replay mode. Will be ignored");
191 | 
192 |     timestamp_mode = sensor::timestamp_mode_of_string(timestamp_mode_arg);
193 |     if (!timestamp_mode) {
194 |       ROS_ERROR("Invalid timestamp mode %s", timestamp_mode_arg.c_str());
195 |       return EXIT_FAILURE;
196 |     }
197 |   }
198 | 
199 |   // fall back to metadata file name based on hostname, if available
200 |   auto meta_file = nh.param("metadata", std::string{});
201 |   // if (!meta_file.size() && hostname.size()) meta_file = hostname + ".json";
202 | 
203 |   if (!replay && (!hostname.size() || !udp_dest.size())) {
204 |     ROS_ERROR("Must specify both hostname and udp destination");
205 |     return EXIT_FAILURE;
206 |   }
207 | 
208 |   // ROS_INFO("Client version: %s", ouster::CLIENT_VERSION_FULL);
209 | 
210 |   if (replay) {
211 |     ROS_INFO("Running in replay mode");
212 | 
213 |     auto meta_cb = [&nh, &srv](const std_msgs::String& str_msg) {
214 |       auto info = sensor::parse_metadata(str_msg.data);
215 |       advertise_service(nh, srv, info);
216 |     };
217 | 
218 |     // populate info for config service
219 |     try {
220 |       if (meta_file.empty()) {
221 |         meta_sub = nh.subscribe<std_msgs::String, const std_msgs::String&>(
222 |             "metadata", 1, meta_cb);
223 |         ROS_INFO("metadata is empty, subscribing to %s",
224 |                  meta_sub.getTopic().c_str());
225 | 
226 |       } else {
227 |         ROS_INFO("metadata file is given, using %s", meta_file.c_str());
228 |         auto info = sensor::metadata_from_json(meta_file);
229 |         advertise_service(nh, srv, info);
230 |       }
231 | 
232 |       // just serve config service
233 |       ros::spin();
234 |       return EXIT_SUCCESS;
235 |     } catch (const std::runtime_error& e) {
236 |       ROS_ERROR("Error when running in replay mode: %s", e.what());
237 |     }
238 |   } else {
239 |     ROS_INFO("Waiting for sensor %s to initialize ...", hostname.c_str());
240 |     ROS_INFO("Sending data to %s", udp_dest.c_str());
241 | 
242 |     // use no-config version of init_client to allow for random ports
243 |     // auto cli = sensor::init_client(hostname, lidar_port, imu_port);
244 |     auto cli = sensor::init_client(
245 |         hostname, udp_dest, lidar_mode, timestamp_mode, lidar_port, imu_port);
246 | 
247 |     if (!cli) {
248 |       ROS_ERROR("Failed to initialize sensor at: %s", hostname.c_str());
249 |       return EXIT_FAILURE;
250 |     }
251 |     ROS_INFO("Sensor initialized successfully");
252 | 
253 |     // write metadata file to cwd (usually ~/.ros)
254 |     auto metadata = sensor::get_metadata(*cli);
255 |     if (meta_file.empty()) {
256 |       meta_file = hostname + ".json";  // hostname must be nonempty
257 |       ROS_INFO("meta_file not given, use: %s", meta_file.c_str());
258 |     }
259 | 
260 |     // write metadata file. If metadata_path is relative, will use cwd
261 |     // (usually ~/.ros)
262 |     if (!write_metadata(meta_file, metadata)) {
263 |       ROS_ERROR("Exiting because of failure to write metadata path to %s",
264 |                 meta_file.c_str());
265 |       return EXIT_FAILURE;
266 |     }
267 | 
268 |     // populate sensor info
269 |     auto info = sensor::parse_metadata(metadata);
270 |     populate_metadata_defaults(info, sensor::MODE_UNSPEC);
271 |     metadata = to_string(info);  // regenerate metadata
272 | 
273 |     // publish metadata
274 |     pub_meta = nh.advertise<std_msgs::String>("metadata", 1, true);
275 |     std_msgs::String meta_msg;
276 |     meta_msg.data = metadata;
277 |     pub_meta.publish(meta_msg);
278 |     ROS_INFO("Publish metadata to %s", pub_meta.getTopic().c_str());
279 | 
280 |     srv = nh.advertiseService<OsGetMetadata::Request, OsGetMetadata::Response>(
281 |         "get_metadata",
282 |         [metadata](OsGetMetadata::Request&, OsGetMetadata::Response& res) {
283 |           if (metadata.empty()) return false;
284 |           res.metadata = metadata;
285 |           return true;
286 |         });
287 | 
288 |     ROS_INFO("Using lidar_mode: %s", sensor::to_string(info.mode).c_str());
289 |     ROS_INFO("%s sn: %s firmware rev: %s",
290 |              info.prod_line.c_str(),
291 |              info.sn.c_str(),
292 |              info.fw_rev.c_str());
293 | 
294 |     // publish packet messages from the sensor
295 |     return connection_loop(nh, *cli, info);
296 |   }
297 | }
298 | 


--------------------------------------------------------------------------------
/src/decoder.cpp:
--------------------------------------------------------------------------------
  1 | #include <cv_bridge/cv_bridge.h>
  2 | #include <image_transport/image_transport.h>
  3 | #include <ros/ros.h>
  4 | #include <tf2_ros/static_transform_broadcaster.h>
  5 | 
  6 | #include "lidar.h"
  7 | #include "ouster_ros/GetMetadata.h"
  8 | #include "ouster_ros/PacketMsg.h"
  9 | 
 10 | namespace ouster_decoder {
 11 | 
 12 | namespace os = ouster_ros::sensor;
 13 | namespace sm = sensor_msgs;
 14 | 
 15 | constexpr double kDefaultGravity = 9.807;  // [m/s^2] earth gravity
 16 | 
 17 | /// @brief Decoder node
 18 | class Decoder {
 19 |  public:
 20 |   explicit Decoder(const ros::NodeHandle& pnh);
 21 | 
 22 |   // No copy no move
 23 |   Decoder(const Decoder&) = delete;
 24 |   Decoder& operator=(const Decoder&) = delete;
 25 |   Decoder(Decoder&&) = delete;
 26 |   Decoder& operator=(Decoder&&) = delete;
 27 | 
 28 |   /// Callbacks
 29 |   void LidarPacketCb(const ouster_ros::PacketMsg& lidar_msg);
 30 |   void ImuPacketCb(const ouster_ros::PacketMsg& imu_msg);
 31 | 
 32 |  private:
 33 |   /// Initialize ros related stuff (frame, publisher, subscriber)
 34 |   void InitRos();
 35 |   /// Initialize all parameters
 36 |   void InitParams();
 37 |   /// Initialize ouster related stuff
 38 |   void InitOuster();
 39 |   void InitModel(const std::string& metadata);
 40 |   void InitScan(const LidarModel& model);
 41 |   void SendTransform(const LidarModel& model);
 42 | 
 43 |   /// Whether we are still waiting for alignment to mid 0
 44 |   [[nodiscard]] bool NeedAlign(int mid);
 45 | 
 46 |   /// Publish messages
 47 |   void PublishAndReset();
 48 | 
 49 |   /// Record processing time of lidar callback, print warning if it exceeds time
 50 |   /// between two packets
 51 |   void Timing(const ros::Time& start) const;
 52 | 
 53 |   // ros
 54 |   ros::NodeHandle pnh_;
 55 |   image_transport::ImageTransport it_;
 56 |   ros::Subscriber lidar_sub_, imu_sub_, meta_sub_;
 57 |   ros::Publisher cloud_pub_, imu_pub_;
 58 |   ros::Publisher range_pub_, signal_pub_;
 59 |   image_transport::CameraPublisher camera_pub_;
 60 |   tf2_ros::StaticTransformBroadcaster static_tf_;
 61 |   std::string sensor_frame_, lidar_frame_, imu_frame_;
 62 | 
 63 |   // data
 64 |   LidarScan scan_;
 65 |   LidarModel model_;
 66 |   sm::CameraInfoPtr cinfo_msg_;
 67 | 
 68 |   // params
 69 |   double gravity_{};              // gravity
 70 |   bool replay_{false};            // replay mode will reinitialize on jump
 71 |   bool need_align_{true};         // whether to align scan
 72 |   double acc_noise_var_{};        // discrete time acc noise variance
 73 |   double gyr_noise_var_{};        // discrete time gyr noise variance
 74 |   double vis_signal_scale_{1.0};  // scale signal visualization
 75 | };
 76 | 
 77 | Decoder::Decoder(const ros::NodeHandle& pnh) : pnh_(pnh), it_(pnh) {
 78 |   InitParams();
 79 |   InitRos();
 80 |   InitOuster();
 81 | }
 82 | 
 83 | void Decoder::InitRos() {
 84 |   // Subscribers, queue size is 1 second
 85 |   lidar_sub_ =
 86 |       pnh_.subscribe("lidar_packets", 640, &Decoder::LidarPacketCb, this);
 87 |   imu_sub_ = pnh_.subscribe("imu_packets", 100, &Decoder::ImuPacketCb, this);
 88 |   ROS_INFO_STREAM("Subscribing lidar packets: " << lidar_sub_.getTopic());
 89 |   ROS_INFO_STREAM("Subscribing imu packets: " << imu_sub_.getTopic());
 90 | 
 91 |   // Publishers
 92 |   camera_pub_ = it_.advertiseCamera("image", 10);
 93 |   cloud_pub_ = pnh_.advertise<sm::PointCloud2>("cloud", 10);
 94 |   imu_pub_ = pnh_.advertise<sm::Imu>("imu", 100);
 95 |   range_pub_ = pnh_.advertise<sm::Image>("range", 5);
 96 |   signal_pub_ = pnh_.advertise<sm::Image>("signal", 5);
 97 | 
 98 |   // Frames
 99 |   sensor_frame_ = pnh_.param<std::string>("sensor_frame", "os_sensor");
100 |   lidar_frame_ = pnh_.param<std::string>("lidar_frame", "os_lidar");
101 |   imu_frame_ = pnh_.param<std::string>("imu_frame", "os_imu");
102 |   ROS_INFO_STREAM("Sensor frame: " << sensor_frame_);
103 |   ROS_INFO_STREAM("Lidar frame: " << lidar_frame_);
104 |   ROS_INFO_STREAM("Imu frame: " << imu_frame_);
105 | }
106 | 
107 | void Decoder::InitParams() {
108 |   replay_ = pnh_.param<bool>("replay", false);
109 |   ROS_INFO("Replay: %s", replay_ ? "true" : "false");
110 |   gravity_ = pnh_.param<double>("gravity", kDefaultGravity);
111 |   ROS_INFO("Gravity: %f", gravity_);
112 |   scan_.destagger = pnh_.param<bool>("destagger", false);
113 |   ROS_INFO("Destagger: %s", scan_.destagger ? "true" : "false");
114 |   scan_.min_range = pnh_.param<double>("min_range", 0.5);
115 |   scan_.max_range = pnh_.param<double>("max_range", 127.0);
116 |   scan_.range_scale = pnh_.param<double>("range_scale", 512.0);
117 |   ROS_INFO("Range: [%f, %f], scale: %f",
118 |            scan_.min_range,
119 |            scan_.max_range,
120 |            scan_.range_scale);
121 |   if (scan_.max_range * scan_.range_scale >
122 |       static_cast<double>(std::numeric_limits<uint16_t>::max())) {
123 |     throw std::domain_error("max range exceeds representation");
124 |   }
125 | 
126 |   acc_noise_var_ = pnh_.param<double>("acc_noise_std", 0.0023);
127 |   gyr_noise_var_ = pnh_.param<double>("gyr_noise_std", 0.00026);
128 |   // https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model
129 |   acc_noise_var_ = std::pow(acc_noise_var_, 2) * 100.0;
130 |   gyr_noise_var_ = std::pow(gyr_noise_var_, 2) * 100.0;
131 |   ROS_INFO("Discrete time acc noise var: %f, gyr nosie var: %f",
132 |            acc_noise_var_,
133 |            gyr_noise_var_);
134 |   vis_signal_scale_ = pnh_.param<double>("vis_signal_scale", 4.0);
135 |   ROS_INFO("Signal scale: %f", vis_signal_scale_);
136 | }
137 | 
138 | void Decoder::InitOuster() {
139 |   ROS_INFO_STREAM("=== Initializing Ouster Decoder ===");
140 |   // wait for service
141 |   auto client = pnh_.serviceClient<ouster_ros::GetMetadata>("get_metadata");
142 | 
143 |   // NOTE: it is possible that in replay mode, the service was shutdown and
144 |   // re-advertised. If the client call is too soon, then we risk getting the old
145 |   // metadata. It is also possible that the call would fail because the driver
146 |   // side hasn't finished re-advertising. Therefore in replay mode, we add a
147 |   // small delay before calling the service.
148 |   if (replay_) {
149 |     ros::Duration(0.1).sleep();
150 |   }
151 | 
152 |   client.waitForExistence();
153 | 
154 |   ouster_ros::GetMetadata srv{};
155 |   // Initialize everything if service call is successful
156 |   if (client.call(srv)) {
157 |     InitModel(srv.response.metadata);
158 |     InitScan(model_);
159 |     SendTransform(model_);
160 |   } else {
161 |     ROS_ERROR_STREAM(client.getService() << " call failed, abort.");
162 |     ros::shutdown();
163 |   }
164 | }
165 | 
166 | void Decoder::InitModel(const std::string& metadata) {
167 |   // parse metadata into lidar model
168 |   model_ = LidarModel{metadata};
169 |   ROS_INFO("Lidar mode %s: %d x %d @ %d hz, delta_azimuth %f",
170 |            os::to_string(model_.info.mode).c_str(),
171 |            model_.rows,
172 |            model_.cols,
173 |            model_.freq,
174 |            model_.d_azimuth);
175 |   ROS_INFO("Columns per packet: %d, Pixels per column: %d",
176 |            model_.pf->columns_per_packet,
177 |            model_.pf->pixels_per_column);
178 | 
179 |   // Generate partial camera info message
180 |   cinfo_msg_ = boost::make_shared<sm::CameraInfo>();
181 |   model_.UpdateCameraInfo(*cinfo_msg_);
182 | }
183 | 
184 | void Decoder::InitScan(const LidarModel& model) {
185 |   int num_subscans = pnh_.param<int>("divide", 1);
186 |   // Make sure cols is divisible by num_subscans
187 |   if (num_subscans < 1 || model.cols % num_subscans != 0) {
188 |     throw std::domain_error(
189 |         "num subscans is not divisible by cols: " + std::to_string(model.cols) +
190 |         " / " + std::to_string(num_subscans));
191 |   }
192 | 
193 |   // Each block has 16 cols, make sure we dont divide into anything smaller
194 |   num_subscans = std::min(num_subscans, model.cols / 16);
195 | 
196 |   const int subscan_cols = model.cols / num_subscans;
197 |   ROS_INFO("Subscan %d x %d, total %d", model.rows, subscan_cols, num_subscans);
198 |   scan_.Allocate(model.rows, subscan_cols);
199 | }
200 | 
201 | void Decoder::SendTransform(const LidarModel& model) {
202 |   static_tf_.sendTransform(ouster_ros::transform_to_tf_msg(
203 |       model.info.imu_to_sensor_transform, sensor_frame_, imu_frame_));
204 |   static_tf_.sendTransform(ouster_ros::transform_to_tf_msg(
205 |       model.info.lidar_to_sensor_transform, sensor_frame_, lidar_frame_));
206 | }
207 | 
208 | void Decoder::PublishAndReset() {
209 |   std_msgs::Header header;
210 |   header.frame_id = lidar_frame_;
211 |   header.stamp.fromNSec(scan_.times.back());  // use time of the last column
212 | 
213 |   // Publish image and camera_info
214 |   // cinfo stores information about the full sweep, while roi stores information
215 |   // about the subscan
216 |   if (camera_pub_.getNumSubscribers() > 0) {
217 |     // const auto image_msg =
218 |     //     cv_bridge::CvImage(header, "32FC4", scan_.image).toImageMsg();
219 |     cinfo_msg_->header = header;
220 |     // Update camera info roi with scan
221 |     scan_.UpdateCinfo(*cinfo_msg_);
222 |     scan_.image_ptr->header = header;
223 |     camera_pub_.publish(scan_.image_ptr, cinfo_msg_);
224 |   }
225 | 
226 |   // Publish range image on demand
227 |   if (range_pub_.getNumSubscribers() > 0 ||
228 |       signal_pub_.getNumSubscribers() > 0) {
229 |     // cast image as 8 channel short so that we can extract the last 2 as range
230 |     // and signal
231 |     const auto image = cv_bridge::toCvShare(scan_.image_ptr)->image;
232 |     const cv::Mat image16u(scan_.rows(), scan_.cols(), CV_16UC(8), image.data);
233 | 
234 |     if (range_pub_.getNumSubscribers() > 0) {
235 |       cv::Mat range;
236 |       cv::extractChannel(image16u, range, 6);
237 |       range_pub_.publish(
238 |           cv_bridge::CvImage(header, "16UC1", range).toImageMsg());
239 |     }
240 | 
241 |     if (signal_pub_.getNumSubscribers() > 0) {
242 |       cv::Mat signal;
243 |       cv::extractChannel(image16u, signal, 7);
244 |       // multiply by 32 for visualization purposes
245 |       signal_pub_.publish(
246 |           cv_bridge::CvImage(header, "16UC1", signal * vis_signal_scale_)
247 |               .toImageMsg());
248 |     }
249 |   }
250 | 
251 |   // Publish cloud
252 |   if (cloud_pub_.getNumSubscribers() > 0) {
253 |     scan_.cloud.header = header;
254 |     cloud_pub_.publish(scan_.cloud);
255 |   }
256 | 
257 |   // Increment subscan counter
258 |   ++scan_.iscan;
259 |   // Reset cached data after publish
260 |   scan_.SoftReset(model_.cols);
261 | }
262 | 
263 | void Decoder::Timing(const ros::Time& t_start) const {
264 |   const auto t_end = ros::Time::now();
265 |   const auto t_proc = (t_end - t_start).toSec();
266 |   const auto ratio = t_proc / model_.dt_packet;
267 |   const auto t_proc_ms = t_proc * 1e3;
268 |   const auto t_block_ms = model_.dt_packet * 1e3;
269 | 
270 |   ROS_DEBUG_THROTTLE(
271 |       1, "time [ms] %.4f / %.4f (%.1f%%)", t_proc_ms, t_block_ms, ratio * 100);
272 | }
273 | 
274 | bool Decoder::NeedAlign(int mid) {
275 |   if (need_align_ && mid == 0) {
276 |     need_align_ = false;
277 |     ROS_WARN("Align start of scan to mid %d, icol in scan %d", mid, scan_.icol);
278 |   }
279 |   return need_align_;
280 | }
281 | 
282 | void Decoder::LidarPacketCb(const ouster_ros::PacketMsg& lidar_msg) {
283 |   const auto t0 = ros::Time::now();
284 |   const auto* packet_buf = lidar_msg.buf.data();
285 |   const auto& pf = *model_.pf;
286 |   const int fid = pf.frame_id(packet_buf);
287 | 
288 |   for (int col = 0; col < pf.columns_per_packet; ++col) {
289 |     // Get column buffer
290 |     const uint8_t* const col_buf = pf.nth_col(col, packet_buf);
291 |     // const int fid = pf.col_frame_id(col_buf);
292 |     const int mid = pf.col_measurement_id(col_buf);
293 | 
294 |     // If we set need_align to true then this will wait for mid = 0 to
295 |     // start a scan
296 |     if (NeedAlign(mid)) {
297 |       continue;
298 |     }
299 | 
300 |     // The invariant here is that scan_.icol will always point at the current
301 |     // column to be filled at the beginning of the loop
302 | 
303 |     // Sometimes the lidar packet will jump forward by a large chunk, we handle
304 |     // this case here
305 |     const auto uid = model_.Uid(fid, mid);
306 |     const auto jump = scan_.DetectJump(uid);
307 |     if (jump == 0) {
308 |       // Data arrived as expected, decode and forward
309 |       scan_.DecodeColumn(col_buf, model_);
310 |       if (scan_.IsFull()) {
311 |         PublishAndReset();
312 |         Timing(t0);
313 |       }
314 |     } else if (0 < jump && jump <= model_.cols) {
315 |       // A jump smaller than a full sweep
316 |       ROS_WARN("Packet jumped to f%d:m%d by %d columns (%.2f sweeps).",
317 |                fid,
318 |                mid,
319 |                jump,
320 |                static_cast<double>(jump) / model_.cols);
321 |       // Detect a jump, we need to forward scan icol by the same amount as jump
322 |       // We could directly increment icol and publish if necessary, but
323 |       // this will require us to zero the whole cloud at publish time which is
324 |       // very time-consuming. Therefore, we choose to advance icol one by one
325 |       // and zero out each column in the point cloud
326 |       for (int i = 0; i < jump; ++i) {
327 |         // zero cloud column at current col and then increment
328 |         scan_.InvalidateColumn(model_.dt_col);
329 |         // It is possible that this jump will span two scans, so if that is
330 |         // the case, we need to publish the previous scan before moving forward
331 |         if (scan_.IsFull()) {
332 |           ROS_WARN("Jumped into a new scan, need to publish the previous one");
333 |           PublishAndReset();
334 |           Timing(t0);
335 |         }
336 |       }
337 |     } else {
338 |       // Handle backward jump or large forward jump
339 |       ROS_ERROR("Packet jumped to f%d:m%d by %d columns (%.2f sweeps).",
340 |                 fid,
341 |                 mid,
342 |                 jump,
343 |                 static_cast<double>(jump) / model_.cols);
344 |       if (replay_) {
345 |         ROS_WARN("Large jump detected in replay mode, re-initialize...");
346 |         need_align_ = true;
347 |         scan_.HardReset();
348 |         // Also need to reinitialize everything since it is possible that it is
349 |         // a different dataset
350 |         InitOuster();
351 |       } else {
352 |         ROS_FATAL("Large jump detected in normal mode, shutting down...");
353 |         ros::shutdown();
354 |       }
355 |       return;
356 |     }
357 |   }
358 | }
359 | 
360 | void Decoder::ImuPacketCb(const ouster_ros::PacketMsg& imu_msg) {
361 |   const auto* buf = imu_msg.buf.data();
362 |   const auto& pf = *model_.pf;
363 | 
364 |   sm::Imu m;
365 |   m.header.stamp.fromNSec(pf.imu_gyro_ts(buf));
366 |   m.header.frame_id = imu_frame_;
367 | 
368 |   // Invalidate orientation data since we don't have it
369 |   // http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Imu.html
370 |   auto& q = m.orientation;
371 |   q.x = q.y = q.z = q.w = 0;
372 |   m.orientation_covariance[0] = -1;
373 | 
374 |   auto& a = m.linear_acceleration;
375 |   a.x = pf.imu_la_x(buf) * gravity_;
376 |   a.y = pf.imu_la_y(buf) * gravity_;
377 |   a.z = pf.imu_la_z(buf) * gravity_;
378 | 
379 |   auto& w = m.angular_velocity;
380 |   w.x = Deg2Rad(pf.imu_av_x(buf));
381 |   w.y = Deg2Rad(pf.imu_av_y(buf));
382 |   w.z = Deg2Rad(pf.imu_av_z(buf));
383 | 
384 |   for (int i = 0; i < 9; i += 4) {
385 |     m.linear_acceleration_covariance[i] = acc_noise_var_;
386 |     m.angular_velocity_covariance[i] = gyr_noise_var_;
387 |   }
388 | 
389 |   imu_pub_.publish(m);
390 | }
391 | 
392 | }  // namespace ouster_decoder
393 | 
394 | int main(int argc, char** argv) {
395 |   ros::init(argc, argv, "os_decoder");
396 | 
397 |   ouster_decoder::Decoder node(ros::NodeHandle("~"));
398 |   ros::spin();
399 | 
400 |   return 0;
401 | }
402 | 


--------------------------------------------------------------------------------