Refactor/v0.10

.gitignore

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

CMakeLists.txt

@@ -5,15 +5,26 @@ set(CMAKE_CXX_STANDARD 17)
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
 include(CompilerWarnings)
 
-find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport pcl_ros
-                                        sensor_msgs ouster_ros)
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  sensor_msgs
+  cv_bridge
+  image_transport
+  pcl_ros
+  ouster_ros
+  )
+
 catkin_package()
 
-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})
+enable_warnings(decoder_node)
 
-add_executable(ouster_driver src/driver.cpp)
-target_include_directories(ouster_driver PRIVATE ${catkin_INCLUDE_DIRS})
-target_link_libraries(ouster_driver PRIVATE ${catkin_LIBRARIES})
+add_executable(driver_node src/driver.cpp src/driver_node.cpp)
+target_link_libraries(driver_node PRIVATE ${catkin_LIBRARIES})
+enable_warnings(driver_node)

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_ros (>1.5ms), tested on Intel i9 13th 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.
+- `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.
+- `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 on hardware:
+```
+roslaunch ouster_decoder ouster.launch sensor_hostname:=<sensor-ip> udp_dest:=<destination-ip>
+```
 
-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` at launch.
 
-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
 
@@ -45,11 +64,12 @@ The corresponding point cloud has point type XYZI.
 
 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.
 
-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.
+Therefore, the publish latency of our decoder is typically less than 0.2ms, while the ouster `os_cloud_node` takes more than 1.5ms to publish a point cloud.
 
 ## Data Drops
 
 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.
 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
+ * January 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

include/ouster_decoder/driver.h

@@ -0,0 +1,116 @@
+/*!
+ * Kumar Robotics
+ * January 2024 refactor
+ * @breif: a cleaned up version of ousters' old driver. It gets
+ * lidar and imu packets from the ouster via the ouster client and 
+ * publishes them on a PacketMsg topic.
+ * Authors: Jason Hughes
+ */
+
+#ifndef DRIVER_H
+#define DRIVER_H
+
+#include <fstream>
+#include <sstream>
+#include <string>
+
+#include "ros/ros.h"
+#include "ouster/types.h"
+#include "ouster/client.h"
+#include "ouster_ros/GetMetadata.h"
+#include "ouster_ros/PacketMsg.h"
+#include "std_msgs/String.h"
+
+class Driver
+{
+  public:
+    /*!
+    * @brief: get and set lidar mode, timestamp mode, ouster metadata
+    * and call initRos and initParams 
+    * @param: ros nodehandle
+    */
+    Driver(const ros::NodeHandle& nh);
+
+  private:
+
+    /*!
+     * @brief: get and set all ROS params
+     * as private class varaibles.
+    */
+    void InitParams();
+    /*!
+    * @brief: initialize lidar packet publisher, imu packet publisher
+    * and metadata string publisher.
+    */
+    void InitRos();
+
+    /*!
+    * @brief: write the metadata string from the ouster to a 
+    * .json file. Default is ...
+    * @param: String metadata: a string in json format to be written.
+    * @return: bool: true if write was successful, false if it was not.
+    */
+    bool WriteMetadata(const std::string& metadata);
+    /*!
+    * @brief: callback for the metadata subscription, used for
+    * bags containing metadata on a topic rather than in json format.
+    * Once metadata is recived pass it to the decoder with the advertiseService.
+    * @param: std_msgs::String msg: ros String message containing the metadata.
+    */
+    void MetadataCallback(const std_msgs::String msg);
+    /*!
+    * @brief: get the lidar and imu packets from the ouster via the
+    * ouster client and publish them on their respective topics.
+    * @param: sensor_info info: sensor_info struct containing all the metadata.
+    */
+    int ConnectionLoop(const ouster::sensor::sensor_info info);
+    /*!
+    * @brief: convert the metadata to a string and advertise 
+    * the metadata on a service for the decoder.
+    * @param: sensor_info info: sensor_info struct containing the metadata.
+    */
+    void AdvertiseService(const ouster::sensor::sensor_info info);
+
+    // ROS Params
+    // @brief: hostname of the ouster, usually the ip address
+    std::string hostname_;
+    // @brief: where the ouster should send data, an ip address
+    // usually the host computer.
+    std::string udp_dest_;
+    // @brief: which port to get lidar packets on. 
+    uint32_t lidar_port_;
+    // @brief: which port to get imu packets on.
+    uint32_t imu_port_;
+    // @brief: set to true if in using a bag.
+    bool replay_;
+    // @brief: resolution and rate,
+    // either 512x10, 512x20, 1024x10, 1024x20, 2048x10
+    std::string lidar_mode_arg_;
+    // @brief: method used to timestamp measurements,
+    // either TIME_FROM_INTERNAL_OSC, TIME_FROM_SYNC_PULSE_IN, TIME_FROM_PTP_1588
+    std::string timestamp_mode_arg_;
+    // @brief: metadata json file, if in replay mode we will read this file
+    // if not we will write metadata to this file.
+    std::string meta_file_;
+
+    // ROS
+    // @brief: ros nodehandler.
+    ros::NodeHandle nh_;
+    // @brief: service to send metadata.
+    ros::ServiceServer srv_;
+    // @brief: lidar packet publisher.
+    ros::Publisher lidar_pub_;
+    // @brief: imu packet publisher.
+    ros::Publisher imu_pub_;
+    // @brief: metadata publisher.
+    ros::Publisher meta_pub_;
+    // @brief: metadata subscriber for bag files.
+    ros::Subscriber meta_sub_;
+
+    // Ouster Client
+    // @brief: ouster client object seud to talk to the ouster
+    // and recieve/send data to it.
+    std::shared_ptr<ouster::sensor::client> cli_;
+
+};
+#endif