diff --git a/.gitignore b/.gitignore index 0d703f4..2fc83f2 100644 --- a/.gitignore +++ b/.gitignore @@ -34,3 +34,4 @@ build/ .vscode/ ouster_decoder.* +metadata.json diff --git a/CMakeLists.txt b/CMakeLists.txt index 352d080..6b1510a 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/README.md b/README.md index 01c259a..8810be5 100644 --- a/README.md +++ b/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:= udp_dest:= +``` -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,7 +64,7 @@ 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 @@ -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. + diff --git a/include/ouster_decoder/decoder.h b/include/ouster_decoder/decoder.h new file mode 100644 index 0000000..abf42df --- /dev/null +++ b/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 +#include +#include +#include + +#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 diff --git a/include/ouster_decoder/driver.h b/include/ouster_decoder/driver.h new file mode 100644 index 0000000..e53add0 --- /dev/null +++ b/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 +#include +#include + +#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 cli_; + +}; +#endif diff --git a/include/ouster_decoder/lidar.h b/include/ouster_decoder/lidar.h new file mode 100644 index 0000000..402b8cd --- /dev/null +++ b/include/ouster_decoder/lidar.h @@ -0,0 +1,253 @@ +/*! Kumar Robotics + * January 2024 refactor + * Structs for data organization around lidar scans, lidar metadata and range/signal images + * Authors: Chao Qu and Jason Hughes + */ + +#pragma once + +#include + +#include +#include +#include + +#include "ouster_ros/os_ros.h" + +inline constexpr double Deg2Rad(double deg) { return deg * M_PI / 180.0; } + +/*! + * @brief struct for organize image data in a scan + */ +struct ImageData +{ + // @brief: x,y,z values + float x{}; + float y{}; + float z{}; + // @brief: range + uint16_t r16u{}; + // @brief:signal + uint16_t s16u{}; + + /*! + * @breif: set the raw range value in image struct + * @param: range in meters + * @param: scale thet range is in + */ + void set_range(float range, double scale) noexcept + { + r16u = static_cast( + std::min(range * scale, + static_cast(std::numeric_limits::max()))); + } + + /*! + * @breif: set range and signal to zero approaching inf. + */ + void set_bad() noexcept { + x = y = z = std::numeric_limits::quiet_NaN(); + r16u = s16u = 0; + } +}; + +static_assert(sizeof(ImageData) == sizeof(float) * 4, + "Size of ImageData must be 4 floats (16 bytes)"); + +/*! +* @brief: Stores SensorInfo from ouster with some other useful data +*/ +struct LidarModel +{ + LidarModel() = default; + /*! + * @breif: parse metadata and save important info from it. + * @param: string in json format containing ouster metadata. + */ + explicit LidarModel(const std::string& metadata); + + /*! + * @brief: whether this model is ready + */ + bool Initialized() const { return !altitudes.empty(); } + + // @breif: number of beams + int rows{}; + // @breif: number of columns in a full scan + int cols{}; + // @breif: frequency + int freq{}; + // @breif: delta time between two columns in secs + double dt_col{}; + // @breif: delta time between two packets in secs + double dt_packet{}; + // @breif: delta angle between two columns in radians + double d_azimuth{}; + // @brief: distance between beam and origin + double beam_offset{}; + // @brief: alitude angles, high to low in radians + std::vector altitudes; + // @breif: azimuth affset angles in radians + std::vector azimuths; + // @breif: metadata in sensor info formet + ouster_ros::sensor::sensor_info info; + // @breif: packet format from ouster + ouster_ros::sensor::packet_format const* pf{nullptr}; + + const auto& pixel_shifts() const noexcept + { + return info.format.pixel_shift_by_row; + } + + /*! + * @brief Convert lidar range data to xyz + * @details see software manual 3.1.2 Lidar Range to XYZ + * + * y r + * ^ / -> rotate clockwise + * | / + * | / + * |/ theta + * o ---------> x (connector) + * @param: range of point + * @param: calcualted angle of column + * @param: id of row + */ + Eigen::Vector3f ToPoint(float range, float theta_enc, int row) const; + /*! + * @brief: calculate unique id for a measurement + * @return: calculated uid + */ + int Uid(int fid, int mid) const noexcept { return fid * cols + mid; } + + /*! + * @brief: update camera info with current info + * @param: recently recieved camera info ros msg + */ + void UpdateCameraInfo(sensor_msgs::CameraInfo& cinfo) const; +}; + +/*! +* @brief: Stores data for a (sub)scan +*/ +struct LidarScan +{ + // @breif: column index + int icol{0}; + // @breif: subscan index + int iscan{0}; + // @brief: previous uid + int prev_uid{-1}; + // @breif: minimum range + double min_range{0.25}; + // @breif: maximum range + double max_range{256.0}; + // @breif: raw range is uint32_t, divide by scale to meter + double range_scale{}; + // @breif: whether to destagger scan + bool destagger{false}; + + // @breif: each pixel is ImageData + sensor_msgs::ImagePtr image_ptr; + // @breif: each point is x,y,z,itensity + sensor_msgs::PointCloud2 cloud; + // @breif: all time stamps in nanoseconds + std::vector times; + + /*! + * @brief: update point cloud ptr + * @param: r: pixel index + * @param: c: column of cloud + * @return: updated point + */ + float* CloudPtr(int r, int c) + { + const auto i = r * cols() + c; + return reinterpret_cast(cloud.data.data() + i * cloud.point_step); + } + + /*! + * @breif: update image prt data + * @param: r: pixel index + * @param: c: column of image + * @return: updated image data struct + */ + ImageData* ImagePtr(int r, int c) + { + const auto i = r * cols() + c; + return reinterpret_cast(image_ptr->data.data() + + i * sizeof(ImageData)); + } + + /*! + * @breif: getter for number of rows in cloud + * @return: int height of cloud + */ + int rows() const noexcept { return static_cast(cloud.height); } + /*! + * @brief: getter for number of columns in cloud + * @return: width of cloud + */ + int cols() const noexcept { return static_cast(cloud.width); } + + /*! + * @brief: whether this scan is full + * @return: true if scan is full + */ + bool IsFull() const noexcept { return icol >= cols(); } + + /*! + * @brief: Starting column of this scan + * @return: index of starting column of scan + */ + int StartingCol() const noexcept { return iscan * cols(); } + + /*! + * @brief: Detect if there is a jump in the lidar data + * @return: 0 - no jump, >0 - jump forward in time, <0 - jump backward in time + */ + int DetectJump(int uid) noexcept; + + /*! + * @brief: Allocate storage for the scan + * @param: rows: number of rows in scan + * @param: cols: number of columns in scan + */ + void Allocate(int rows, int cols); + + /*! + * @brief: Hard reset internal counters and prev_uid + */ + void HardReset() noexcept; + + /*! + * @brief: Try to reset the internal counters if it is full + * @params: integer indicating column is full + */ + void SoftReset(int full_col) noexcept; + + /*! + * @brief: Invalidate an entire column + * @param: delta time between two columns + */ + void InvalidateColumn(double dt_col) noexcept; + + /*! + * @brief: Decode column + * @param: column buffer from raw lidar packet + * @param: lidar model containing lidar info + */ + void DecodeColumn(const uint8_t* const col_buf, const LidarModel& model); + + /*! + * @brief: Update camera info roi data with this scan + * @param: msg contianing camera info + */ + void UpdateCinfo(sensor_msgs::CameraInfo& cinfo) const noexcept; + + /*! + * @breif: prepares the PointField msg + * @returns: vector of PointField msg. + */ + std::vector MakePointFieldsXYZI() noexcept; +}; diff --git a/include/ouster_decoder/viz.h b/include/ouster_decoder/viz.h new file mode 100644 index 0000000..df2c7c5 --- /dev/null +++ b/include/ouster_decoder/viz.h @@ -0,0 +1,37 @@ +/* January 2024 +* header file for vizualization class +*/ + +#ifndef VIZ_H +#define VIZ_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +class Viz +{ + public: + explicit Viz(const ros::NodeHandle& pnh); + + void cameraCb(const sensor_msgs::ImageConstPtr& image_ptr, + const sensor_msgs::CameraInfoConstPtr& cinfo_ptr); + + private: + cv::Mat applyCmap(const cv::Mat& input, + int cmap, + uint8_t bad_color); + + ros::NodeHandle pnh_; + image_transport::ImageTransport it_; + image_transport::CameraSubscriber sub_camera_; + std::string cmap_{"gray"}; +}; +#endif diff --git a/launch/decoder.launch b/launch/decoder.launch index e512f3e..8a131ac 100644 --- a/launch/decoder.launch +++ b/launch/decoder.launch @@ -4,6 +4,7 @@ + @@ -15,8 +16,6 @@ - - @@ -42,10 +41,9 @@ - + - diff --git a/launch/driver.launch b/launch/driver.launch index 432fc63..9c5307a 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -1,17 +1,16 @@ - - - + + + - - + diff --git a/launch/ouster.launch b/launch/ouster.launch new file mode 100644 index 0000000..9dae3fd --- /dev/null +++ b/launch/ouster.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index c12a3cc..8636823 100644 --- a/package.xml +++ b/package.xml @@ -1,18 +1,21 @@ - ouster_decoder - 0.1.0 - Ouster decoder - Chao Qu - BSD - catkin + ouster_decoder + 0.0.0 + The ouster_decoder package - cv_bridge - image_transport - pcl_ros - sensor_msgs + Jason Hughes + BSD - ouster_ros + catkin - + cv_bridge + image_transport + pcl_ros + sensor_msgs + + ouster_ros + + + diff --git a/src/decoder.cpp b/src/decoder.cpp index fb23a56..15d3727 100644 --- a/src/decoder.cpp +++ b/src/decoder.cpp @@ -1,116 +1,58 @@ -#include -#include -#include -#include - -#include "lidar.h" -#include "ouster_ros/GetMetadata.h" -#include "ouster_ros/PacketMsg.h" - -namespace ouster_decoder { - -namespace os = ouster_ros::sensor; -namespace sm = sensor_msgs; - -constexpr double kDefaultGravity = 9.807; // [m/s^2] earth gravity - -/// @brief Decoder node -class Decoder { - public: - 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; - - /// Callbacks - void LidarPacketCb(const ouster_ros::PacketMsg& lidar_msg); - void ImuPacketCb(const ouster_ros::PacketMsg& imu_msg); - - private: - /// Initialize ros related stuff (frame, publisher, subscriber) - void InitRos(); - /// Initialize all parameters - void InitParams(); - /// Initialize ouster related stuff - void InitOuster(); - void InitModel(const std::string& metadata); - void InitScan(const LidarModel& model); - void SendTransform(const LidarModel& model); - - /// Whether we are still waiting for alignment to mid 0 - [[nodiscard]] bool NeedAlign(int mid); - - /// Publish messages - void PublishAndReset(); - - /// Record processing time of lidar callback, print warning if it exceeds time - /// between two packets - void Timing(const ros::Time& start) const; - - // ros - ros::NodeHandle pnh_; - image_transport::ImageTransport it_; - ros::Subscriber lidar_sub_, imu_sub_, meta_sub_; - ros::Publisher cloud_pub_, imu_pub_; - ros::Publisher range_pub_, signal_pub_; - image_transport::CameraPublisher camera_pub_; - tf2_ros::StaticTransformBroadcaster static_tf_; - std::string sensor_frame_, lidar_frame_, imu_frame_; - - // data - LidarScan scan_; - LidarModel model_; - sm::CameraInfoPtr cinfo_msg_; - - // params - double gravity_{}; // gravity - bool replay_{false}; // replay mode will reinitialize on jump - bool need_align_{true}; // whether to align scan - double acc_noise_var_{}; // discrete time acc noise variance - double gyr_noise_var_{}; // discrete time gyr noise variance - double vis_signal_scale_{1.0}; // scale signal visualization -}; - -Decoder::Decoder(const ros::NodeHandle& pnh) : pnh_(pnh), it_(pnh) { +/*! + * Kumar Robotics + * January 2024 Refactor + * Logic for handling incoming lidar and imu packets and + * passing them appropriate struct to be decoded. Once the buffers are + * ready they are published on the appropriate topics. + * Authors: Chao Qu, Jason Hughes + */ + +#include "ouster_decoder/decoder.h" + +Decoder::Decoder(const ros::NodeHandle& pnh) : pnh_(pnh), it_(pnh) +{ InitParams(); InitRos(); InitOuster(); } -void Decoder::InitRos() { +void Decoder::InitRos() +{ // Subscribers, queue size is 1 second - lidar_sub_ = - pnh_.subscribe("lidar_packets", 640, &Decoder::LidarPacketCb, this); + lidar_sub_ = pnh_.subscribe("lidar_packets", 640, &Decoder::LidarPacketCb, this); imu_sub_ = pnh_.subscribe("imu_packets", 100, &Decoder::ImuPacketCb, this); + ROS_INFO_STREAM("Subscribing lidar packets: " << lidar_sub_.getTopic()); ROS_INFO_STREAM("Subscribing imu packets: " << imu_sub_.getTopic()); // Publishers camera_pub_ = it_.advertiseCamera("image", 10); - cloud_pub_ = pnh_.advertise("cloud", 10); - imu_pub_ = pnh_.advertise("imu", 100); - range_pub_ = pnh_.advertise("range", 5); - signal_pub_ = pnh_.advertise("signal", 5); + cloud_pub_ = pnh_.advertise("cloud", 10); + imu_pub_ = pnh_.advertise("imu", 100); + range_pub_ = pnh_.advertise("range", 5); + signal_pub_ = pnh_.advertise("signal", 5); // Frames sensor_frame_ = pnh_.param("sensor_frame", "os_sensor"); lidar_frame_ = pnh_.param("lidar_frame", "os_lidar"); imu_frame_ = pnh_.param("imu_frame", "os_imu"); + ROS_INFO_STREAM("Sensor frame: " << sensor_frame_); ROS_INFO_STREAM("Lidar frame: " << lidar_frame_); ROS_INFO_STREAM("Imu frame: " << imu_frame_); } -void Decoder::InitParams() { +void Decoder::InitParams() +{ replay_ = pnh_.param("replay", false); ROS_INFO("Replay: %s", replay_ ? "true" : "false"); + gravity_ = pnh_.param("gravity", kDefaultGravity); ROS_INFO("Gravity: %f", gravity_); + scan_.destagger = pnh_.param("destagger", false); ROS_INFO("Destagger: %s", scan_.destagger ? "true" : "false"); + scan_.min_range = pnh_.param("min_range", 0.5); scan_.max_range = pnh_.param("max_range", 127.0); scan_.range_scale = pnh_.param("range_scale", 512.0); @@ -125,17 +67,20 @@ void Decoder::InitParams() { acc_noise_var_ = pnh_.param("acc_noise_std", 0.0023); gyr_noise_var_ = pnh_.param("gyr_noise_std", 0.00026); + // https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model acc_noise_var_ = std::pow(acc_noise_var_, 2) * 100.0; gyr_noise_var_ = std::pow(gyr_noise_var_, 2) * 100.0; ROS_INFO("Discrete time acc noise var: %f, gyr nosie var: %f", acc_noise_var_, gyr_noise_var_); + vis_signal_scale_ = pnh_.param("vis_signal_scale", 4.0); ROS_INFO("Signal scale: %f", vis_signal_scale_); } -void Decoder::InitOuster() { +void Decoder::InitOuster() +{ ROS_INFO_STREAM("=== Initializing Ouster Decoder ==="); // wait for service auto client = pnh_.serviceClient("get_metadata"); @@ -163,11 +108,13 @@ void Decoder::InitOuster() { } } -void Decoder::InitModel(const std::string& metadata) { +// TODO this may need to change depending on the changes of Lidar Model +void Decoder::InitModel(const std::string& metadata) +{ // parse metadata into lidar model model_ = LidarModel{metadata}; ROS_INFO("Lidar mode %s: %d x %d @ %d hz, delta_azimuth %f", - os::to_string(model_.info.mode).c_str(), + ouster_ros::sensor::to_string(model_.info.mode).c_str(), model_.rows, model_.cols, model_.freq, @@ -177,11 +124,12 @@ void Decoder::InitModel(const std::string& metadata) { model_.pf->pixels_per_column); // Generate partial camera info message - cinfo_msg_ = boost::make_shared(); + cinfo_msg_ = boost::make_shared(); model_.UpdateCameraInfo(*cinfo_msg_); } -void Decoder::InitScan(const LidarModel& model) { +void Decoder::InitScan(const LidarModel& model) +{ int num_subscans = pnh_.param("divide", 1); // Make sure cols is divisible by num_subscans if (num_subscans < 1 || model.cols % num_subscans != 0) { @@ -189,7 +137,7 @@ void Decoder::InitScan(const LidarModel& model) { "num subscans is not divisible by cols: " + std::to_string(model.cols) + " / " + std::to_string(num_subscans)); } - + // Each block has 16 cols, make sure we dont divide into anything smaller num_subscans = std::min(num_subscans, model.cols / 16); @@ -198,14 +146,17 @@ void Decoder::InitScan(const LidarModel& model) { scan_.Allocate(model.rows, subscan_cols); } -void Decoder::SendTransform(const LidarModel& model) { + +void Decoder::SendTransform(const LidarModel& model) +{ static_tf_.sendTransform(ouster_ros::transform_to_tf_msg( - model.info.imu_to_sensor_transform, sensor_frame_, imu_frame_)); + model.info.imu_to_sensor_transform, sensor_frame_, imu_frame_, ros::Time::now())); static_tf_.sendTransform(ouster_ros::transform_to_tf_msg( - model.info.lidar_to_sensor_transform, sensor_frame_, lidar_frame_)); + model.info.lidar_to_sensor_transform, sensor_frame_, lidar_frame_, ros::Time::now())); } -void Decoder::PublishAndReset() { +void Decoder::PublishAndReset() +{ std_msgs::Header header; header.frame_id = lidar_frame_; header.stamp.fromNSec(scan_.times.back()); // use time of the last column @@ -260,7 +211,8 @@ void Decoder::PublishAndReset() { scan_.SoftReset(model_.cols); } -void Decoder::Timing(const ros::Time& t_start) const { +void Decoder::Timing(const ros::Time& t_start) const +{ const auto t_end = ros::Time::now(); const auto t_proc = (t_end - t_start).toSec(); const auto ratio = t_proc / model_.dt_packet; @@ -271,7 +223,8 @@ void Decoder::Timing(const ros::Time& t_start) const { 1, "time [ms] %.4f / %.4f (%.1f%%)", t_proc_ms, t_block_ms, ratio * 100); } -bool Decoder::NeedAlign(int mid) { +bool Decoder::NeedAlign(int mid) +{ if (need_align_ && mid == 0) { need_align_ = false; ROS_WARN("Align start of scan to mid %d, icol in scan %d", mid, scan_.icol); @@ -279,7 +232,8 @@ bool Decoder::NeedAlign(int mid) { return need_align_; } -void Decoder::LidarPacketCb(const ouster_ros::PacketMsg& lidar_msg) { +void Decoder::LidarPacketCb(const ouster_ros::PacketMsg& lidar_msg) +{ const auto t0 = ros::Time::now(); const auto* packet_buf = lidar_msg.buf.data(); const auto& pf = *model_.pf; @@ -349,19 +303,22 @@ void Decoder::LidarPacketCb(const ouster_ros::PacketMsg& lidar_msg) { // a different dataset InitOuster(); } else { - ROS_FATAL("Large jump detected in normal mode, shutting down..."); - ros::shutdown(); + ROS_FATAL("Large jump detected in normal mode, restarting..."); + need_align_ = true; + scan_.HardReset(); + InitOuster(); } return; } } } -void Decoder::ImuPacketCb(const ouster_ros::PacketMsg& imu_msg) { +void Decoder::ImuPacketCb(const ouster_ros::PacketMsg& imu_msg) +{ const auto* buf = imu_msg.buf.data(); const auto& pf = *model_.pf; - sm::Imu m; + sensor_msgs::Imu m; m.header.stamp.fromNSec(pf.imu_gyro_ts(buf)); m.header.frame_id = imu_frame_; @@ -389,13 +346,17 @@ void Decoder::ImuPacketCb(const ouster_ros::PacketMsg& imu_msg) { imu_pub_.publish(m); } -} // namespace ouster_decoder -int main(int argc, char** argv) { - ros::init(argc, argv, "os_decoder"); - ouster_decoder::Decoder node(ros::NodeHandle("~")); - ros::spin(); - return 0; -} + + + + + + + + + + + diff --git a/src/decoder_node.cpp b/src/decoder_node.cpp new file mode 100644 index 0000000..4fce425 --- /dev/null +++ b/src/decoder_node.cpp @@ -0,0 +1,19 @@ +/*! + * Kumar Robotics + * January 2024 Refactor + * @brief: starter for the ros decoder node + * Authors: Jason Hughes + */ + +#include "ouster_decoder/decoder.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "os_decoder_node"); + + Decoder node(ros::NodeHandle("~")); + ros::spin(); + + return 0; +} + diff --git a/src/driver.cpp b/src/driver.cpp index a85a168..bbfe976 100644 --- a/src/driver.cpp +++ b/src/driver.cpp @@ -1,297 +1,225 @@ -/** - * Copyright (c) 2018, Ouster, Inc. - * All rights reserved. - * - * @file - * @brief Example node to publish raw sensor output on ROS topics - * - * ROS Parameters - * sensor_hostname: hostname or IP in dotted decimal form of the sensor - * udp_dest: hostname or IP where the sensor will send data packets - * lidar_port: port to which the sensor should send lidar data - * imu_port: port to which the sensor should send imu data - */ - -// This is a modified version of ouster_ros/os_node.cpp -// It is intended to have the same behavior as os_node. -// The only difference is that we also advertise the metadata message. - -#include -#include - -#include -#include -#include - -#include "ouster_ros/GetMetadata.h" -#include "ouster_ros/PacketMsg.h" -#include "ouster_ros/os_ros.h" - -using PacketMsg = ouster_ros::PacketMsg; -using OsGetMetadata = ouster_ros::GetMetadata; -namespace sensor = ouster::sensor; - -// fill in values that could not be parsed from metadata -void populate_metadata_defaults(sensor::sensor_info& info, - sensor::lidar_mode specified_lidar_mode) { - if (!info.name.size()) info.name = "UNKNOWN"; +/*! +* Kumar Robotics +* January 2024 Refactor +* @breif: talks to the ouster via the ouster client +* and get metadata, lidar packets and imu packets and passes them along +* on ros topics. +* Authors: Chao Qu, Jason Hughes and Ouster +*/ + +#include "ouster_decoder/driver.h" + +Driver::Driver(const ros::NodeHandle& nh) : nh_(nh) +{ + InitParams(); + InitRos(); + + ouster::sensor::sensor_info info; + std::string metadata; + + // set lidar mode + ouster::sensor::lidar_mode lidar_mode = ouster::sensor::MODE_UNSPEC; + if (lidar_mode_arg_.size()) + { + if (replay_) ROS_WARN("Lidar mode set in replay mode, will be ignored"); + lidar_mode = ouster::sensor::lidar_mode_of_string(lidar_mode_arg_); + if (!lidar_mode) + { + ROS_ERROR("Invalid lidar mode %s", lidar_mode_arg_.c_str()); + ros::shutdown(); + } + } - if (!info.sn.size()) info.sn = "UNKNOWN"; + ouster::sensor::timestamp_mode timestamp_mode = ouster::sensor::TIME_FROM_UNSPEC; + if (timestamp_mode_arg_.size()) + { + if (replay_) ROS_WARN("Timestamp mode set in replay mode, will be ignored"); - ouster::util::version v = ouster::util::version_of_string(info.fw_rev); - if (v == ouster::util::invalid_version) - ROS_WARN("Unknown sensor firmware version; output may not be reliable"); - else if (v < sensor::min_version) - ROS_WARN("Firmware < %s not supported; output may not be reliable", - to_string(sensor::min_version).c_str()); + timestamp_mode = ouster::sensor::timestamp_mode_of_string(timestamp_mode_arg_); + if(!timestamp_mode) + { + ROS_ERROR("Invalid timestamp mode %s, exiting", timestamp_mode_arg_.c_str()); + ros::shutdown(); + } + } - if (!info.mode) { - ROS_WARN("Lidar mode not found in metadata; output may not be reliable"); - info.mode = specified_lidar_mode; + if (!replay_) + { + ROS_INFO("Running in hardware mode"); + ROS_INFO("Looking for sensor at: %s", hostname_.c_str()); + ROS_INFO("Sending data to udp destination: %s", udp_dest_.c_str()); } - if (!info.prod_line.size()) info.prod_line = "UNKNOWN"; + if (replay_) + { + try + { + if (!meta_file_.empty()) + { + ROS_INFO("Running in replay mode"); + info = ouster::sensor::metadata_from_json(meta_file_); + AdvertiseService(info); + } + else + { + meta_sub_ = nh_.subscribe("metadata", 1, &Driver::MetadataCallback, this); + ROS_WARN("No metadata file specified, looking for metadata on topic"); + } + } catch (const std::runtime_error& e) + { + ROS_ERROR("Error running in replay mode: %s", e.what()); + } + } + else + { + try + { + ROS_INFO("Connecting to sensor with hostname: %s", hostname_.c_str()); + ROS_INFO("Sending data to udp destination: %s", udp_dest_.c_str()); + + cli_ = ouster::sensor::init_client(hostname_, + udp_dest_, + lidar_mode, + timestamp_mode, + lidar_port_, + imu_port_); + if (!cli_) + { + ROS_ERROR("Failed to initialize sensor at %s", hostname_.c_str()); + ros::shutdown(); + } + metadata = ouster::sensor::get_metadata (*cli_, 20, true); + if (meta_file_.empty()) + { + meta_file_ = "metadata.json"; + ROS_INFO("No metadata json specified, using %s", meta_file_.c_str()); + } + if (!WriteMetadata(metadata)) + { + ROS_ERROR("Couldn't write metadata to json, continueing"); + } + info = ouster::sensor::parse_metadata(metadata); - if (info.beam_azimuth_angles.empty() || info.beam_altitude_angles.empty()) { - ROS_WARN("Beam angles not found in metadata; using design values"); - info.beam_azimuth_angles = sensor::gen1_azimuth_angles; - info.beam_altitude_angles = sensor::gen1_altitude_angles; + AdvertiseService(info); + } catch(const std::exception& e) + { + ROS_ERROR("Error connection to sensor: %s", e.what()); + } } + ROS_INFO("Using lidar_mode: %s", ouster::sensor::to_string(info.mode).c_str()); + ROS_INFO("%s sn: %s firmware rev: %s", + info.prod_line.c_str(), + info.sn.c_str(), + info.fw_rev.c_str()); + //publish metadata on topic (mostly for bagging) + std_msgs::String meta_msg; + meta_msg.data = metadata; + meta_pub_.publish(meta_msg); + + if (!replay_) int success = ConnectionLoop(info); } -// try to write metadata file -bool write_metadata(const std::string& meta_file, const std::string& metadata) { +bool Driver::WriteMetadata(const std::string& metadata) +{ std::ofstream ofs; - ofs.open(meta_file); + ofs.open(meta_file_); ofs << metadata << std::endl; ofs.close(); - if (ofs) { - ROS_INFO("Wrote metadata to %s", meta_file.c_str()); - } else { + if (ofs) + { + ROS_INFO("Wrote metadata to %s", meta_file_.c_str()); + } + else + { ROS_WARN( - "Failed to write metadata to %s; check that the path is valid. If " - "you provided a relative path, please note that the working " - "directory of all ROS nodes is set by default to $ROS_HOME", - meta_file.c_str()); + "Failed to write metadata to %s; check that the path is valid. If " + "you provided a relative path, please note that the working " + "directory of all ROS nodes is set by default to $ROS_HOME", + meta_file_.c_str()); return false; } return true; } -int connection_loop(ros::NodeHandle& nh, - sensor::client& cli, - const sensor::sensor_info& info) { - auto lidar_packet_pub = nh.advertise("lidar_packets", 1280); - auto imu_packet_pub = nh.advertise("imu_packets", 100); - - auto pf = sensor::get_format(info); +int Driver::ConnectionLoop(const ouster::sensor::sensor_info info) +{ + ouster::sensor::packet_format pf = ouster::sensor::get_format(info); - PacketMsg lidar_packet, imu_packet; + ouster_ros::PacketMsg lidar_packet, imu_packet; lidar_packet.buf.resize(pf.lidar_packet_size + 1); imu_packet.buf.resize(pf.imu_packet_size + 1); - while (ros::ok()) { - auto state = sensor::poll_client(cli); - if (state == sensor::EXIT) { - ROS_INFO("poll_client: caught signal, exiting"); - return EXIT_SUCCESS; + ROS_INFO("Publishing Packets..."); + while (ros::ok()) + { + ouster::sensor::client_state state = ouster::sensor::poll_client(*cli_); + if (state == ouster::sensor::EXIT) + { + ROS_INFO("Client: caught signal, exiting"); + ros::shutdown(); } - if (state & sensor::CLIENT_ERROR) { - ROS_ERROR("poll_client: returned error"); - return EXIT_FAILURE; + if (state & ouster::sensor::CLIENT_ERROR) + { + ROS_ERROR("Client: returned error"); + ros::shutdown(); } - if (state & sensor::LIDAR_DATA) { - if (sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf)) - lidar_packet_pub.publish(lidar_packet); + if (state & ouster::sensor::LIDAR_DATA) + { + if (ouster::sensor::read_lidar_packet(*cli_, lidar_packet.buf.data(), pf)) + { + lidar_pub_.publish(lidar_packet); + } } - if (state & sensor::IMU_DATA) { - if (sensor::read_imu_packet(cli, imu_packet.buf.data(), pf)) - imu_packet_pub.publish(imu_packet); + if (state & ouster::sensor::IMU_DATA) + { + if (ouster::sensor::read_imu_packet(*cli_, imu_packet.buf.data(), pf)) + { + imu_pub_.publish(imu_packet); + } } ros::spinOnce(); } return EXIT_SUCCESS; } -void advertise_service(ros::NodeHandle& nh, - ros::ServiceServer& srv, - const sensor::sensor_info& info) { - auto metadata = sensor::to_string(info); - ROS_INFO("Using lidar_mode: %s", sensor::to_string(info.mode).c_str()); - ROS_INFO("%s sn: %s firmware rev: %s", - info.prod_line.c_str(), - info.sn.c_str(), - info.fw_rev.c_str()); - if (srv) { - // shutdown first and readvertise - ROS_INFO("Shutting down %s service and re-advertise", - srv.getService().c_str()); - srv.shutdown(); - } - srv = nh.advertiseService( - "get_metadata", - [metadata](OsGetMetadata::Request&, OsGetMetadata::Response& res) { - if (metadata.empty()) return false; - res.metadata = metadata; - return true; - }); - ROS_INFO("Advertise service to %s", srv.getService().c_str()); -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "os_node"); - ros::NodeHandle nh("~"); - // Extra stuff - ros::Publisher pub_meta; - ros::Subscriber meta_sub; - ros::ServiceServer srv; - - // empty indicates "not set" since roslaunch xml can't optionally set params - auto hostname = nh.param("sensor_hostname", std::string{}); - auto udp_dest = nh.param("udp_dest", std::string{}); - auto lidar_port = nh.param("lidar_port", 0); - auto imu_port = nh.param("imu_port", 0); - auto replay = nh.param("replay", false); - auto lidar_mode_arg = nh.param("lidar_mode", std::string{}); - auto timestamp_mode_arg = nh.param("timestamp_mode", std::string{}); +void Driver::AdvertiseService(const ouster::sensor::sensor_info info) +{ + std::string metadata = info.original_string(); - std::string udp_profile_lidar_arg; - nh.param("udp_profile_lidar", udp_profile_lidar_arg, ""); - - // optional udp_profile_lidar; - // if (udp_profile_lidar_arg.size()) { - // if (replay) - // ROS_WARN("UDP Profile Lidar set in replay mode. Will be ignored."); - - // // set lidar profile from param - // udp_profile_lidar = - // sensor::udp_profile_lidar_of_string(udp_profile_lidar_arg); - // if (!udp_profile_lidar) { - // ROS_ERROR("Invalid udp profile lidar: %s", - // udp_profile_lidar_arg.c_str()); return EXIT_FAILURE; - // } - // } - - // set lidar mode from param - sensor::lidar_mode lidar_mode = sensor::MODE_UNSPEC; - if (lidar_mode_arg.size()) { - if (replay) ROS_WARN("Lidar mode set in replay mode. Will be ignored"); - - lidar_mode = sensor::lidar_mode_of_string(lidar_mode_arg); - if (!lidar_mode) { - ROS_ERROR("Invalid lidar mode %s", lidar_mode_arg.c_str()); - return EXIT_FAILURE; - } - } - - // set timestamp mode from param - sensor::timestamp_mode timestamp_mode = sensor::TIME_FROM_UNSPEC; - if (timestamp_mode_arg.size()) { - if (replay) ROS_WARN("Timestamp mode set in replay mode. Will be ignored"); - - timestamp_mode = sensor::timestamp_mode_of_string(timestamp_mode_arg); - if (!timestamp_mode) { - ROS_ERROR("Invalid timestamp mode %s", timestamp_mode_arg.c_str()); - return EXIT_FAILURE; - } + if (srv_) + { + srv_.shutdown(); } + srv_ = nh_.advertiseService( + "get_metadata", + [metadata](ouster_ros::GetMetadata::Request&, ouster_ros::GetMetadata::Response& res) { + if (metadata.empty()) return false; + res.metadata = metadata; + return true; + }); + ROS_INFO("Advertising Metadata on service %s.", srv_.getService().c_str()); +} - // fall back to metadata file name based on hostname, if available - auto meta_file = nh.param("metadata", std::string{}); - // if (!meta_file.size() && hostname.size()) meta_file = hostname + ".json"; - - if (!replay && (!hostname.size() || !udp_dest.size())) { - ROS_ERROR("Must specify both hostname and udp destination"); - return EXIT_FAILURE; - } - - // ROS_INFO("Client version: %s", ouster::CLIENT_VERSION_FULL); - - if (replay) { - ROS_INFO("Running in replay mode"); - - auto meta_cb = [&nh, &srv](const std_msgs::String& str_msg) { - auto info = sensor::parse_metadata(str_msg.data); - advertise_service(nh, srv, info); - }; - - // populate info for config service - try { - if (meta_file.empty()) { - meta_sub = nh.subscribe( - "metadata", 1, meta_cb); - ROS_INFO("metadata is empty, subscribing to %s", - meta_sub.getTopic().c_str()); - - } else { - ROS_INFO("metadata file is given, using %s", meta_file.c_str()); - auto info = sensor::metadata_from_json(meta_file); - advertise_service(nh, srv, info); - } - - // just serve config service - ros::spin(); - return EXIT_SUCCESS; - } catch (const std::runtime_error& e) { - ROS_ERROR("Error when running in replay mode: %s", e.what()); - } - } else { - ROS_INFO("Waiting for sensor %s to initialize ...", hostname.c_str()); - ROS_INFO("Sending data to %s", udp_dest.c_str()); - - // use no-config version of init_client to allow for random ports - // auto cli = sensor::init_client(hostname, lidar_port, imu_port); - auto cli = sensor::init_client( - hostname, udp_dest, lidar_mode, timestamp_mode, lidar_port, imu_port); - - if (!cli) { - ROS_ERROR("Failed to initialize sensor at: %s", hostname.c_str()); - return EXIT_FAILURE; - } - ROS_INFO("Sensor initialized successfully"); - - // write metadata file to cwd (usually ~/.ros) - auto metadata = sensor::get_metadata(*cli); - if (meta_file.empty()) { - meta_file = hostname + ".json"; // hostname must be nonempty - ROS_INFO("meta_file not given, use: %s", meta_file.c_str()); - } - - // write metadata file. If metadata_path is relative, will use cwd - // (usually ~/.ros) - if (!write_metadata(meta_file, metadata)) { - ROS_ERROR("Exiting because of failure to write metadata path to %s", - meta_file.c_str()); - return EXIT_FAILURE; - } - - // populate sensor info - auto info = sensor::parse_metadata(metadata); - populate_metadata_defaults(info, sensor::MODE_UNSPEC); - metadata = to_string(info); // regenerate metadata - - // publish metadata - pub_meta = nh.advertise("metadata", 1, true); - std_msgs::String meta_msg; - meta_msg.data = metadata; - pub_meta.publish(meta_msg); - ROS_INFO("Publish metadata to %s", pub_meta.getTopic().c_str()); - - srv = nh.advertiseService( - "get_metadata", - [metadata](OsGetMetadata::Request&, OsGetMetadata::Response& res) { - if (metadata.empty()) return false; - res.metadata = metadata; - return true; - }); - - ROS_INFO("Using lidar_mode: %s", sensor::to_string(info.mode).c_str()); - ROS_INFO("%s sn: %s firmware rev: %s", - info.prod_line.c_str(), - info.sn.c_str(), - info.fw_rev.c_str()); +void Driver::InitRos() +{ + lidar_pub_ = nh_.advertise("lidar_packets", 1280); + imu_pub_ = nh_.advertise("imu_packets", 100); + meta_pub_ = nh_.advertise("metadata", 1); +} - // publish packet messages from the sensor - return connection_loop(nh, *cli, info); - } +void Driver::InitParams() +{ + nh_.getParam("replay", replay_); + hostname_ = nh_.param("sensor_hostname", std::string{}); + udp_dest_ = nh_.param("udp_dest", std::string{}); + lidar_port_ = nh_.param("lidar_port", 0); + imu_port_ = nh_.param("imu_port", 0); + lidar_mode_arg_ = nh_.param("lidar_mode", std::string{}); + timestamp_mode_arg_ = nh_.param("timestamp_mode", std::string{}); + nh_.param("metadata", meta_file_, ""); } + +void Driver::MetadataCallback(const std_msgs::String msg) +{ + AdvertiseService(ouster::sensor::parse_metadata(msg.data)); +} diff --git a/src/driver_node.cpp b/src/driver_node.cpp new file mode 100644 index 0000000..d4f5227 --- /dev/null +++ b/src/driver_node.cpp @@ -0,0 +1,18 @@ +/*! +* Kumar Robotics +* January 2024 Refactor +* @brief: starter for ouster driver ros node +* Authors: Jason Hughes +*/ + +#include "ouster_decoder/driver.h" + +int main(int argc , char** argv) +{ + ros::init(argc, argv, "os_driver_node"); + + Driver node(ros::NodeHandle("~")); + ros::spin(); + + return 0; +} diff --git a/src/lidar.cpp b/src/lidar.cpp index 0348565..40c0dd0 100644 --- a/src/lidar.cpp +++ b/src/lidar.cpp @@ -1,36 +1,39 @@ -#include "lidar.h" +/*! + * Kumar Robotics + * January 2024 refactor + * @breif logic for handling lidar metadata, image formatting and + * incoming lidar buffers from the packets + * Author: Chao Qu + */ -#include - -namespace ouster_decoder { - -namespace os = ouster_ros::sensor; +#include "ouster_decoder/lidar.h" constexpr float kMmToM = 0.001; constexpr double kTau = 2 * M_PI; constexpr float kNaNF = std::numeric_limits::quiet_NaN(); -namespace { - -/// @brief Convert a vector of double from deg to rad -std::vector TransformDeg2Rad(const std::vector& degs) { +// @brief Convert a vector of double from degrees to radians +std::vector TransformDeg2Rad(const std::vector& degs) +{ std::vector rads; rads.reserve(degs.size()); + for (const auto& deg : degs) { rads.push_back(Deg2Rad(deg)); } + return rads; } -} // namespace - -LidarModel::LidarModel(const std::string& metadata) { - info = os::parse_metadata(metadata); - pf = &os::get_format(info); - +// LidarModel +LidarModel::LidarModel(const std::string& metadata) +{ + info = ouster_ros::sensor::parse_metadata(metadata); + pf = &ouster_ros::sensor::get_format(info); + rows = info.beam_altitude_angles.size(); - cols = os::n_cols_of_lidar_mode(info.mode); - freq = os::frequency_of_lidar_mode(info.mode); + cols = ouster_ros::sensor::n_cols_of_lidar_mode(info.mode); + freq = ouster_ros::sensor::frequency_of_lidar_mode(info.mode); dt_col = 1.0 / freq / cols; d_azimuth = kTau / cols; @@ -40,9 +43,8 @@ LidarModel::LidarModel(const std::string& metadata) { azimuths = TransformDeg2Rad(info.beam_azimuth_angles); } -Eigen::Vector3f LidarModel::ToPoint(float range, - float theta_enc, - int row) const { +Eigen::Vector3f LidarModel::ToPoint(float range, float theta_enc, int row) const +{ const float n = beam_offset; const float d = range - n; const float phi = altitudes[row]; @@ -54,23 +56,21 @@ Eigen::Vector3f LidarModel::ToPoint(float range, d * std::sin(phi)}; } -void LidarModel::UpdateCameraInfo(sensor_msgs::CameraInfo& cinfo) const { +void LidarModel::UpdateCameraInfo(sensor_msgs::CameraInfo& cinfo) const +{ cinfo.height = rows; cinfo.width = cols; cinfo.distortion_model = info.prod_line; - // cinfo.D.reserve(altitudes.size() + azimuths.size()); - // cinfo.D.insert(cinfo.D.end(), altitudes.begin(), altitudes.end()); - // cinfo.D.insert(cinfo.D.end(), azimuths.begin(), azimuths.end()); cinfo.D.reserve(pixel_shifts().size()); cinfo.D.insert(cinfo.D.end(), pixel_shifts().begin(), pixel_shifts().end()); cinfo.K[0] = dt_col; // time between each column - // cinfo.K[1] = d_azimuth; // radian between each column - // cinfo.K[2] = beam_offset; // distance from center to beam } -void LidarScan::Allocate(int rows, int cols) { +// LidarScan +void LidarScan::Allocate(int rows, int cols) +{ // Don't do any work if rows and cols are the same // image.create(rows, cols, CV_32FC4); if (!image_ptr) image_ptr = boost::make_shared(); @@ -93,7 +93,8 @@ void LidarScan::Allocate(int rows, int cols) { times.resize(cols, 0); } -int LidarScan::DetectJump(int uid) noexcept { +int LidarScan::DetectJump(int uid) noexcept +{ int jump = 0; if (prev_uid >= 0) { @@ -105,13 +106,15 @@ int LidarScan::DetectJump(int uid) noexcept { return jump; } -void LidarScan::HardReset() noexcept { +void LidarScan::HardReset() noexcept +{ icol = 0; iscan = 0; prev_uid = -1; } -void LidarScan::SoftReset(int full_col) noexcept { +void LidarScan::SoftReset(int full_col) noexcept +{ // Reset col (usually to 0 but in the rare case that data jumps forward // it will be non-zero) icol = icol % cols(); @@ -122,7 +125,8 @@ void LidarScan::SoftReset(int full_col) noexcept { } } -void LidarScan::InvalidateColumn(double dt_col) noexcept { +void LidarScan::InvalidateColumn(double dt_col) noexcept +{ for (int irow = 0; irow < static_cast(cloud.height); ++irow) { auto* ptr = CloudPtr(irow, icol); ptr[0] = ptr[1] = ptr[2] = kNaNF; @@ -143,8 +147,9 @@ void LidarScan::InvalidateColumn(double dt_col) noexcept { ++icol; } -void LidarScan::DecodeColumn(const uint8_t* const col_buf, - const LidarModel& model) { + +void LidarScan::DecodeColumn(const uint8_t* const col_buf, const LidarModel& model) +{ const auto& pf = *model.pf; const uint64_t t_ns = pf.col_timestamp(col_buf); const uint16_t mid = pf.col_measurement_id(col_buf); @@ -158,26 +163,40 @@ void LidarScan::DecodeColumn(const uint8_t* const col_buf, // const float theta_enc = kTau * (1.0f - encoder / 90112.0f); const float theta_enc = kTau - mid * model.d_azimuth; times.at(icol) = t_ns; + uint32_t raw_ranges[pf.pixels_per_column]; + uint32_t raw_signal[pf.pixels_per_column]; + + pf.col_field(col_buf, + ouster_ros::sensor::ChanField::RANGE, + raw_ranges, + 1); + + pf.col_field(col_buf, + ouster_ros::sensor::ChanField::SIGNAL, + raw_signal, + 1); for (int ipx = 0; ipx < pf.pixels_per_column; ++ipx) { // Data to fill Eigen::Vector3f xyz; xyz.setConstant(kNaNF); float r{}; - uint16_t s16u{}; + //uint16_t s16u{}; + uint32_t signal; if (status) { const uint8_t* const px_buf = pf.nth_px(ipx, col_buf); - const auto raw_range = pf.px_range(px_buf); - const float range = raw_range * kMmToM; // used to compute xyz + const float range = raw_ranges[ipx] * kMmToM; // used to compute xyz if (min_range <= range && range <= max_range) { xyz = model.ToPoint(range, theta_enc, ipx); r = xyz.norm(); // we compute range ourselves - s16u = pf.px_signal(px_buf); + signal = raw_signal[ipx]; } // s16u += pf.px_ambient(px_buf); } + // TODO: what if we don't enter the above if-statement + // We're still setting signal without ever getting it. // Now we set cloud and image data // There is no destagger for cloud, so we update point no matter what @@ -185,7 +204,7 @@ void LidarScan::DecodeColumn(const uint8_t* const col_buf, cptr[0] = xyz.x(); cptr[1] = xyz.y(); cptr[2] = xyz.z(); - cptr[3] = static_cast(s16u); + cptr[3] = static_cast(signal); // However image can be destaggered, and pixel can go out of bound // add pixel shift to get where the pixel should be @@ -198,7 +217,7 @@ void LidarScan::DecodeColumn(const uint8_t* const col_buf, iptr->y = xyz.y(); iptr->z = xyz.z(); iptr->set_range(r, range_scale); - iptr->s16u = s16u; + iptr->s16u = signal; } else { auto* iptr = ImagePtr(ipx, im_col % cols()); iptr->set_bad(); @@ -209,7 +228,8 @@ void LidarScan::DecodeColumn(const uint8_t* const col_buf, ++icol; } -void LidarScan::UpdateCinfo(sensor_msgs::CameraInfo& cinfo) const noexcept { +void LidarScan::UpdateCinfo(sensor_msgs::CameraInfo& cinfo) const noexcept +{ cinfo.R[0] = range_scale; cinfo.binning_x = iscan; // index of subscan within a full scan @@ -222,7 +242,8 @@ void LidarScan::UpdateCinfo(sensor_msgs::CameraInfo& cinfo) const noexcept { roi.do_rectify = destagger; } -std::vector MakePointFieldsXYZI() noexcept { +std::vector LidarScan::MakePointFieldsXYZI() noexcept +{ using sensor_msgs::PointField; std::vector fields; fields.reserve(4); @@ -255,4 +276,3 @@ std::vector MakePointFieldsXYZI() noexcept { return fields; } -} // namespace ouster_decoder diff --git a/src/lidar.h b/src/lidar.h deleted file mode 100644 index 6a161ad..0000000 --- a/src/lidar.h +++ /dev/null @@ -1,139 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "ouster_ros/os_ros.h" - -namespace ouster_decoder { - -inline constexpr double Deg2Rad(double deg) { return deg * M_PI / 180.0; } - -/// @brief image data in scan -struct ImageData { - float x{}; - float y{}; - float z{}; - uint16_t r16u{}; - uint16_t s16u{}; - - void set_range(float range, double scale) noexcept { - r16u = static_cast( - std::min(range * scale, - static_cast(std::numeric_limits::max()))); - } - - void set_bad() noexcept { - x = y = z = std::numeric_limits::quiet_NaN(); - r16u = s16u = 0; - } -}; - -static_assert(sizeof(ImageData) == sizeof(float) * 4, - "Size of ImageData must be 4 floats (16 bytes)"); - -/// @brief Stores SensorInfo from ouster with some other useful data -struct LidarModel { - LidarModel() = default; - explicit LidarModel(const std::string& metadata); - - /// @brief whether this model is ready - bool Initialized() const { return !altitudes.empty(); } - - int rows{}; // number of beams - int cols{}; // cols of a full scan - int freq{}; // frequency - double dt_col{}; // delta time between two columns [s] - double dt_packet{}; // delta time between two packets [s] - double d_azimuth{}; // delta angle between two columns [rad] - double beam_offset{}; // distance between beam to origin - std::vector altitudes; // altitude angles, high to low [rad] - std::vector azimuths; // azimuths offset angles [rad] - ouster_ros::sensor::sensor_info info; // sensor info - ouster_ros::sensor::packet_format const* pf{nullptr}; // packet format - - const auto& pixel_shifts() const noexcept { - return info.format.pixel_shift_by_row; - } - - /// @brief Convert lidar range data to xyz - /// @details see software manual 3.1.2 Lidar Range to XYZ - /// - /// y r - /// ^ / -> rotate clockwise - /// | / - /// | / - /// |/ theta - /// o ---------> x (connector) - /// - Eigen::Vector3f ToPoint(float range, float theta_enc, int row) const; - - /// @brief Return a unique id for a measurement - int Uid(int fid, int mid) const noexcept { return fid * cols + mid; } - - /// @brief Update camera info with this model - void UpdateCameraInfo(sensor_msgs::CameraInfo& cinfo) const; -}; - -/// @brief Stores data for a (sub)scan -struct LidarScan { - int icol{0}; // column index - int iscan{0}; // subscan index - int prev_uid{-1}; // previous uid - double min_range{0.25}; // minimum range - double max_range{256.0}; // maximum range - double range_scale{}; // raw range is uint_16, divide by scale to meter - bool destagger{false}; // whether to destagger scan - - // cv::Mat image; - sensor_msgs::ImagePtr image_ptr; // each pixel is ImageData - sensor_msgs::PointCloud2 cloud; // each point is (x,y,z,intensity) - std::vector times; // all time stamps [nanosecond] - - float* CloudPtr(int r, int c) { - const auto i = r * cols() + c; - return reinterpret_cast(cloud.data.data() + i * cloud.point_step); - } - - ImageData* ImagePtr(int r, int c) { - const auto i = r * cols() + c; - return reinterpret_cast(image_ptr->data.data() + - i * sizeof(ImageData)); - } - - int rows() const noexcept { return static_cast(cloud.height); } - int cols() const noexcept { return static_cast(cloud.width); } - - /// @brief whether this scan is full - bool IsFull() const noexcept { return icol >= cols(); } - - /// @brief Starting column of this scan - int StartingCol() const noexcept { return iscan * cols(); } - - /// @brief Detect if there is a jump in the lidar data - /// @return 0 - no jump, >0 - jump forward in time, <0 - jump backward in time - int DetectJump(int uid) noexcept; - - /// @brief Allocate storage for the scan - void Allocate(int rows, int cols); - - /// @brief Hard reset internal counters and prev_uid - void HardReset() noexcept; - - /// @brief Try to reset the internal counters if it is full - void SoftReset(int full_col) noexcept; - - /// @brief Invalidate an entire column - void InvalidateColumn(double dt_col) noexcept; - - /// @brief Decode column - void DecodeColumn(const uint8_t* const col_buf, const LidarModel& model); - - /// @brief Update camera info roi data with this scan - void UpdateCinfo(sensor_msgs::CameraInfo& cinfo) const noexcept; -}; - -std::vector MakePointFieldsXYZI() noexcept; - -} // namespace ouster_decoder diff --git a/src/viz.cpp b/src/viz.cpp index 7d736b2..6108f5f 100644 --- a/src/viz.cpp +++ b/src/viz.cpp @@ -1,47 +1,15 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "ouster_decoder/viz.h" -namespace ouster_decoder { - -namespace it = image_transport; -namespace sm = sensor_msgs; - -cv::Mat ApplyCmap(const cv::Mat& input, int cmap, uint8_t bad_color) { - cv::Mat disp; - input.convertTo(disp, CV_8UC1); - cv::applyColorMap(disp, disp, cmap); - return disp; -} - -class Viz { - public: - explicit Viz(const ros::NodeHandle& pnh); - - void CameraCb(const sm::ImageConstPtr& image_ptr, - const sm::CameraInfoConstPtr& cinfo_ptr); - - private: - ros::NodeHandle pnh_; - it::ImageTransport it_; - it::CameraSubscriber sub_camera_; - std::string cmap_{"gray"}; -}; - -Viz::Viz(const ros::NodeHandle& pnh) : pnh_{pnh}, it_{pnh} { - sub_camera_ = it_.subscribeCamera("image", 1, &Viz::CameraCb, this); +Viz::Viz(const ros::NodeHandle& pnh) : pnh_{pnh}, it_{pnh} +{ + sub_camera_ = it_.subscribeCamera("image", 1, &Viz::cameraCb, this); ROS_INFO_STREAM("Subscribing to: " << sub_camera_.getTopic()); } -void Viz::CameraCb(const sm::ImageConstPtr& image_ptr, - const sm::CameraInfoConstPtr& cinfo_ptr) { +void Viz::cameraCb(const sensor_msgs::ImageConstPtr& image_ptr, + const sensor_msgs::CameraInfoConstPtr& cinfo_ptr) +{ const auto mat = cv_bridge::toCvShare(image_ptr)->image; const auto h = mat.rows; const auto w = mat.cols; @@ -52,8 +20,8 @@ void Viz::CameraCb(const sm::ImageConstPtr& image_ptr, cv::extractChannel(mat_map, range_raw, 6); cv::extractChannel(mat_map, signal_raw, 7); - auto range_color = ApplyCmap(range_raw / 100, cv::COLORMAP_PINK, 0); - auto signal_color = ApplyCmap(signal_raw / 4, cv::COLORMAP_PINK, 0); + auto range_color = applyCmap(range_raw / 100, cv::COLORMAP_PINK, 0); + auto signal_color = applyCmap(signal_raw / 4, cv::COLORMAP_PINK, 0); // set invalid range (0) to black range_color.setTo(0, range_raw == 0); @@ -98,13 +66,11 @@ void Viz::CameraCb(const sm::ImageConstPtr& image_ptr, cv::waitKey(1); } -} // namespace ouster_decoder - -int main(int argc, char** argv) { - ros::init(argc, argv, "os_viz"); - - ouster_decoder::Viz node(ros::NodeHandle("~")); - ros::spin(); +Viz::applyCmap(const cv::Mat& input, int cmap, uint8_t bad_color) +{ + cv::Mat disp; + input.convertTo(disp, CV_8UC1); + cv::applyColorMap(disp, disp, cmap); - return 0; + return disp; }