Refactor/v0.10

.gitignore

@@ -34,3 +34,4 @@
 build/
 .vscode/
 ouster_decoder.*
+metadata.json

CMakeLists.txt

@@ -1,19 +1,35 @@
-cmake_minimum_required(VERSION 3.15)
+cmake_minimum_required(VERSION 3.0.2)
 project(ouster_decoder)
 
-set(CMAKE_CXX_STANDARD 17)
-list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
-include(CompilerWarnings)
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
 
-find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport pcl_ros
-                                        sensor_msgs ouster_ros)
-catkin_package()
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  sensor_msgs
+  cv_bridge
+  image_transport
+  pcl_ros
+  ouster_ros
+  )
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES ouster_decoder2
+#  CATKIN_DEPENDS roscpp sensor_msgs
+#  DEPENDS system_lib
+)
 
-add_executable(ouster_decoder src/lidar.cpp src/decoder.cpp)
-target_include_directories(ouster_decoder PRIVATE src ${catkin_INCLUDE_DIRS})
-target_link_libraries(ouster_decoder PRIVATE ${catkin_LIBRARIES})
-enable_warnings(ouster_decoder)
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(decoder_node src/decoder.cpp src/decoder_node.cpp src/lidar.cpp)
+target_link_libraries(decoder_node PRIVATE ${catkin_LIBRARIES})
+
+add_executable(driver_node src/driver.cpp src/driver_node.cpp)
+target_link_libraries(driver_node PRIVATE ${catkin_LIBRARIES})
 
-add_executable(ouster_driver src/driver.cpp)
-target_include_directories(ouster_driver PRIVATE ${catkin_INCLUDE_DIRS})
-target_link_libraries(ouster_driver PRIVATE ${catkin_LIBRARIES})

README.md

@@ -1,8 +1,6 @@
 # ouster_decoder
 
-This decoder is intended to extend the ouster_ros package from https://github.com/ouster-lidar/ouster_example
-
-It has very low latency (<0.2ms) compared to ouster_example (>3ms), tested on Intel i7 11th gen cpu.
+This decoder is intended as an alternative the [ouster-ros](https://github.com/ouster-lidar/ouster-ros) decoder. It publishes things like LidarScans in a different formats which may be better for things like Lidar Odometry, while things like point clouds, signal, range and IMU remain in the same format. It also has very low latency (<0.2ms) compared to ouster_example (>3ms), tested on Intel i7 11th gen cpu. This will also notify you when packets are dropped. The decoder is up to date with the v0.10 of the ouster SDK.
 
 The decoder only supports LEGACY and single return profile. Currently there's no plan for dual return profile.
 
@@ -11,19 +9,40 @@ The decoder only supports LEGACY and single return profile. Currently there's no
 The timestamp of both the cloud and image message is the time of the last column, not the first column.
 This is different from the official driver, which uses timestamp of the first column. 
 
+## Setup
+Clone this repo in you catkin workspace along with [ouster-ros](https://github.com/ouster-lidar/ouster-ros) and `catkin build`. We use thier custom service and messages and thier ouster sdk submodule. 
+
+## Parameters
+The following parameters may differ from the defaults that we use. They can be set in the launch file or passed as an argument.
+- `replay` set to `true` if you are using a bag, default: `false`.
+- `sensor_hostname` hostname or IP in dotted decimal format of the ouster, default: `192.168.100.12`
+- `udp_dest` hostname or IP in dotted decimal format of where the sensor will send data. Most likely the computer the ouster is connected to, default: `192.168.100.1`
+- `lidar_port` the port to which the ouster will send lidar packets, default: `7502`
+- `imu_port` the port to which the ouster will send imu packets, default: `7503`
+- `lidar_mode` resolution and rate of the lidar: either `512x10`, `512x20`, `1024x10`, `1024x20`, or `2048x10`, defualt comes from lidar.
+- `timestamp_mode` method used to timestamp measurements: either `IME_FROM_INTERNAL_OSC`, `TIME_FROM_SYNC_PULSE_IN`, `TIME_FROM_PTP_1588`, default comes from lidar.
+- `metadata` specifiy a metadata file to write to, default: `ouster_decoder/metadata.json`. This must be specified if you are using a bag without the `/metadata` topic. 
+- `tf_prefic` namespace for tf transforms.
+- `driver_ns` namespace for driver node.
+
 ## Usage
+To start everything at once use:
+```
+roslaunch ouster_decoder driver.launch
+```
 
-Run the ouster driver 
+Run just the driver (if you want to bag the packets for later decoding) 
 ```
-roslaunch ouster_decoder driver.launch replay:=true/false
+roslaunch ouster_decoder driver.launch
 ```
 
-Then run the decoder
+To run with a bag file run:
 ```
-roslaunch ouster_decoder decoder.launch
+roslaunch ouster_decoder ouster.launch replay:=true
 ```
+and start your bag in another terminal. If your bag does not have the `/metadata` topic you'll need to specify a json file with `metadata:=/path/to/json`.
 
-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.
+The driver node is the same (logically) as the one from `ouster_example`, but cleaned up and 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.
 
 ## Decoder
 
@@ -53,3 +72,4 @@ Our decoder also checks for missing data. This is very useful for debugging purp
 The decoder then makes sure that missing data is zeroed out in the message. 
 
 Therefore, one can safely stop and replay a recorded rosbag without restarting the driver and decoder.
+

include/ouster_decoder/decoder.h

@@ -0,0 +1,144 @@
+/*!
+ * Kumar Robotics
+ * Januart 2024 refactor
+ * @breif: decodes incoming lidar and imu packets and publishes them 
+ * on appropriate ROS topics
+ * Authors: Chao Qu and Jason Hughes
+ */
+
+#ifndef DECODER_H
+#define DECODER_H
+
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <ros/ros.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+
+#include "ouster_ros/GetMetadata.h"
+#include "ouster_ros/PacketMsg.h"
+
+#include "lidar.h"
+
+constexpr double kDefaultGravity = 9.807;
+
+class Decoder 
+{
+    public:
+        /*!
+        * @breif: call to set ros params, initial ros and ouster
+        * @param: ros nodehandle
+        */
+        explicit Decoder(const ros::NodeHandle& pnh);
+
+        // No copy no move
+        Decoder(const Decoder&) = delete;
+        Decoder& operator=(const Decoder&) = delete;
+        Decoder(Decoder&&) = delete;
+        Decoder& operator=(Decoder&&) = delete;
+
+        /*!
+        * @breif: lidar packet callback
+        * @param: PacketMsg, ros msg containing lidar packet
+        * data.
+        */
+        void LidarPacketCb(const ouster_ros::PacketMsg& lidar_msg);
+        /*!
+        * @brief: imu packet callback
+        * @param: PacketMsg, ros msg containing imu packet
+        */
+        void ImuPacketCb(const ouster_ros::PacketMsg& imu_msg);
+
+    private:
+        /*!
+        * @brief: initialize ros publishers/ subscribers and frames
+        */
+        void InitRos();
+        /*!
+        * @breif: initialize all ros params.
+        */
+        void InitParams();
+        /*!
+        * @breif: get metadata from ros client from driver, 
+        * call lidar initializers
+        */
+        void InitOuster();
+        /*!
+        * @breif: initialize LidarModel object, 
+        * generate camera info for topic
+        * @param: string of metadata recieved from ros client.
+        */
+        void InitModel(const std::string& metadata);
+        /*!
+        * @breif: Allocate the memory for pointcloud 
+        * based on the number of subscans gotten by the "divide" param.
+        * @param: lidar model object, containing all the lidar information.
+        */
+        void InitScan(const LidarModel& model);
+        /*!
+        * @breif: send imu and lidar transforms to tf.
+        * @param: lidar model object, containing all the lidar information.
+        */
+        void SendTransform(const LidarModel& model);
+
+        /*!
+        * @breif: check if decoder is still waiting for alignment to mid 0.
+        * @param: mid, column measurment id from the column buffer.
+        * @return: true if mid == 0
+        */
+        [[nodiscard]] bool NeedAlign(int mid);
+
+        /*!
+        * @breif: publish the pointcloud, range and signal images, and 
+        * camera info. Reset the lidar scan.
+        */
+        void PublishAndReset();
+
+        /*!
+        * @breif: record processing time of lidar callback, print warning if it 
+        * exceeds window between two packets
+        * @param: ros time when the lidar packet was recieved.
+        */
+        void Timing(const ros::Time& start) const;
+
+        // ROS
+        // @brief: ros noehandler.
+        ros::NodeHandle pnh_;
+        // @brief: tos image transporter.
+        image_transport::ImageTransport it_;
+        // @breif: lidar imu and metadata subscribers.
+        ros::Subscriber lidar_sub_, imu_sub_, meta_sub_;
+        // @breif: point cloud and imu publisher.
+        ros::Publisher cloud_pub_, imu_pub_;
+        // @breif: range and signal image publishers.
+        ros::Publisher range_pub_, signal_pub_;
+        // @breif: camera info publisher.
+        image_transport::CameraPublisher camera_pub_;
+        // @brief: tf2 static transform broadcaster
+        tf2_ros::StaticTransformBroadcaster static_tf_;
+        // @brief: frames, defined in launch file and gotten
+        // as ros params.
+        std::string sensor_frame_, lidar_frame_, imu_frame_;
+
+        // DATA
+        // @breif: object to hold incoming lidar data
+        LidarScan scan_;
+        // @breif: object to hold lidar metadata
+        LidarModel model_;
+        // @brief: ros msg for camera info
+        sensor_msgs::CameraInfoPtr cinfo_msg_;
+
+        // PARAMS
+        // @breif: gravity
+        double gravity_{};
+        // @brief: replay mode will reinitialize on jump
+        bool replay_{false};            
+        // @breif: whether to align scan
+        bool need_align_{true};         
+        // @breif: discrete time accelerometer noise variance
+        double acc_noise_var_{};        
+        // @breif: discrete time gyro noise varaince
+        double gyr_noise_var_{};        
+        // @breif: scal signal visualization
+        double vis_signal_scale_{1.0};  
+}; 
+#endif