From 3775b489bd4f0213ecef7055584b22e2cf084b96 Mon Sep 17 00:00:00 2001 From: Jason Date: Mon, 22 Jan 2024 17:11:17 -0500 Subject: [PATCH 01/12] started refactor for v0.10 compatability --- CMakeLists.txt | 43 +++++-- cmake/CompilerWarnings.cmake | 52 -------- include/ouster_decoder/decoder.h | 79 ++++++++++++ {src => include/ouster_decoder}/lidar.h | 18 ++- include/ouster_decoder/viz.h | 37 ++++++ launch/decoder.launch | 2 +- launch/driver.launch | 2 +- package.xml | 27 ++-- src/decoder.cpp | 163 +++++++++--------------- src/decoder_node.cpp | 17 +++ src/driver.cpp | 2 + src/lidar.cpp | 101 +++++++++------ src/viz.cpp | 64 +++------- 13 files changed, 325 insertions(+), 282 deletions(-) delete mode 100644 cmake/CompilerWarnings.cmake create mode 100644 include/ouster_decoder/decoder.h rename {src => include/ouster_decoder}/lidar.h (95%) create mode 100644 include/ouster_decoder/viz.h create mode 100644 src/decoder_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 352d080..4365923 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,19 +1,34 @@ -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(ouster_driver src/driver.cpp) -target_include_directories(ouster_driver PRIVATE ${catkin_INCLUDE_DIRS}) -target_link_libraries(ouster_driver PRIVATE ${catkin_LIBRARIES}) +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) +target_link_libraries(driver_node PRIVATE ${catkin_LIBRARIES}) diff --git a/cmake/CompilerWarnings.cmake b/cmake/CompilerWarnings.cmake deleted file mode 100644 index 03e1fc1..0000000 --- a/cmake/CompilerWarnings.cmake +++ /dev/null @@ -1,52 +0,0 @@ -# from here: -# -# https://github.com/lefticus/cppbestpractices/blob/master/02-Use_the_Tools_Available.md - -function(enable_warnings target) - - set(CLANG_WARNINGS - -Wall - -Wextra # reasonable and standard - # -Wshadow # warn the user if a variable declaration shadows one from a - # parent context - -Wnon-virtual-dtor # warn the user if a class with virtual functions has a - # non-virtual destructor. This helps catch hard to - # track down memory errors - # -Wold-style-cast # warn for c-style casts - -Wcast-align # warn for potential performance problem casts - -Wunused # warn on anything being unused - -Woverloaded-virtual # warn if you overload (not override) a virtual - # function - # -Wpedantic # warn if non-standard C++ is used - # -Wconversion # warn on type conversions that may lose data - # -Wsign-conversion # warn on sign conversions - -Wnull-dereference # warn if a null dereference is detected - # -Wdouble-promotion # warn if float is implicit promoted to double - -Wformat=2 # warn on security issues around functions that format output - # (ie printf) - ) - - set(GCC_WARNINGS - ${CLANG_WARNINGS} - -Wmisleading-indentation # warn if indentation implies blocks where blocks - # do not exist - -Wduplicated-cond # warn if if / else chain has duplicated conditions - # -Wduplicated-branches # warn if if / else branches have duplicated code - -Wlogical-op # warn about logical operations being used where bitwise were - # probably wanted - # -Wuseless-cast # warn if you perform a cast to the same type - ) - - if(CMAKE_CXX_COMPILER_ID MATCHES ".*Clang") - set(PROJECT_WARNINGS ${CLANG_WARNINGS}) - elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - set(PROJECT_WARNINGS ${GCC_WARNINGS}) - else() - message( - AUTHOR_WARNING - "No compiler warnings set for '${CMAKE_CXX_COMPILER_ID}' compiler.") - endif() - - target_compile_options(${target} INTERFACE ${PROJECT_WARNINGS}) - -endfunction() diff --git a/include/ouster_decoder/decoder.h b/include/ouster_decoder/decoder.h new file mode 100644 index 0000000..c1f9b7a --- /dev/null +++ b/include/ouster_decoder/decoder.h @@ -0,0 +1,79 @@ +/* +* Header file for decoder.cpp +*/ + +#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: + 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_; + sensor_msgs::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 +}; +#endif diff --git a/src/lidar.h b/include/ouster_decoder/lidar.h similarity index 95% rename from src/lidar.h rename to include/ouster_decoder/lidar.h index 6a161ad..4cbb242 100644 --- a/src/lidar.h +++ b/include/ouster_decoder/lidar.h @@ -1,17 +1,22 @@ +/* January 2024 +* Basic structs for data organization around lidar scans and lidar metadata +*/ + #pragma once +#include + #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 { +struct ImageData +{ float x{}; float y{}; float z{}; @@ -34,7 +39,8 @@ 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 { +struct LidarModel +{ LidarModel() = default; explicit LidarModel(const std::string& metadata); @@ -77,7 +83,8 @@ struct LidarModel { }; /// @brief Stores data for a (sub)scan -struct LidarScan { +struct LidarScan +{ int icol{0}; // column index int iscan{0}; // subscan index int prev_uid{-1}; // previous uid @@ -136,4 +143,3 @@ struct LidarScan { std::vector MakePointFieldsXYZI() noexcept; -} // namespace ouster_decoder 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..db9a018 100644 --- a/launch/decoder.launch +++ b/launch/decoder.launch @@ -42,7 +42,7 @@ - + diff --git a/launch/driver.launch b/launch/driver.launch index 432fc63..f0ef221 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -11,7 +11,7 @@ - + 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..908b28b 100644 --- a/src/decoder.cpp +++ b/src/decoder.cpp @@ -1,86 +1,18 @@ -#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) { +/* +* functions for decoder node +*/ + +#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); @@ -90,10 +22,10 @@ void Decoder::InitRos() { // 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"); @@ -104,13 +36,17 @@ void Decoder::InitRos() { 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 +61,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 +102,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 +118,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 +131,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 +140,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 +205,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 +217,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 +226,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; @@ -357,11 +305,12 @@ void Decoder::LidarPacketCb(const ouster_ros::PacketMsg& lidar_msg) { } } -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 +338,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..52579df --- /dev/null +++ b/src/decoder_node.cpp @@ -0,0 +1,17 @@ +/* January 2024 +* As part of the refactor for upgrades to ouster_ros v0.10+ +* Node start script for ouster decoder +*/ + +#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..3826ca5 100644 --- a/src/driver.cpp +++ b/src/driver.cpp @@ -15,6 +15,8 @@ // 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. +// The logic here was kept the same in the 2024 refactor. + #include #include diff --git a/src/lidar.cpp b/src/lidar.cpp index 0348565..f3e3738 100644 --- a/src/lidar.cpp +++ b/src/lidar.cpp @@ -1,36 +1,35 @@ -#include "lidar.h" +/* January 2024 +* cpp file for lidar data structs +*/ -#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 +39,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 +52,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 +89,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 +102,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 +121,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 +143,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 +159,41 @@ 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 auto raw_range = pf.px_range(px_buf); //TODO: this needs to be replaced!! + 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]; //pf.px_signal(px_buf); //TODO: this needs to be replaced!! } // 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 +201,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 +214,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 +225,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 +239,8 @@ void LidarScan::UpdateCinfo(sensor_msgs::CameraInfo& cinfo) const noexcept { roi.do_rectify = destagger; } -std::vector MakePointFieldsXYZI() noexcept { +std::vector MakePointFieldsXYZI() noexcept +{ using sensor_msgs::PointField; std::vector fields; fields.reserve(4); @@ -255,4 +273,3 @@ std::vector MakePointFieldsXYZI() noexcept { return fields; } -} // 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; } From 009fd7cb73178c7365d4de7705f0d720ae977d9b Mon Sep 17 00:00:00 2001 From: Jason Date: Thu, 25 Jan 2024 22:46:27 -0500 Subject: [PATCH 02/12] reworked driver --- CMakeLists.txt | 5 +- README.md | 12 +- include/ouster_decoder/driver.h | 52 ++++ src/decoder.cpp | 15 +- src/driver.cpp | 480 ++++++++++++++------------------ src/driver_node.cpp | 11 + src/driver_old.cpp | 304 ++++++++++++++++++++ src/lidar.cpp | 4 +- 8 files changed, 595 insertions(+), 288 deletions(-) create mode 100644 include/ouster_decoder/driver.h create mode 100644 src/driver_node.cpp create mode 100644 src/driver_old.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4365923..f7ecda8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,5 +30,8 @@ include_directories( 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) +add_executable(driver_node src/driver.cpp src/driver_node.cpp) target_link_libraries(driver_node PRIVATE ${catkin_LIBRARIES}) + +add_executable(driver_old_node src/driver_old.cpp) +target_link_libraries(driver_old_node PRIVATE ${catkin_LIBRARIES}) diff --git a/README.md b/README.md index 01c259a..a4cf3ff 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,10 @@ Then run the decoder ``` roslaunch ouster_decoder decoder.launch ``` - +Running on hardware requires setting a few more parameters: +``` +roslaunch ouster_decoder driver.launch sensor_hostname:=192.168.100.12 lidar_port:=7502 imu_port:=7503 udp_dest:=192.168.100.1 replay:=false +``` 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. ## Decoder @@ -36,8 +39,8 @@ struct Data { float x; float y; float z; - uint16_t r16u; // range - uint16_t s16u; // signal + uint32_t range; // range + uint32_t signal; // signal }; ``` @@ -53,3 +56,6 @@ 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. + +## Notes +CPU usage is 9.0 percent with ouster_ros. diff --git a/include/ouster_decoder/driver.h b/include/ouster_decoder/driver.h new file mode 100644 index 0000000..651dd15 --- /dev/null +++ b/include/ouster_decoder/driver.h @@ -0,0 +1,52 @@ +#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: + Driver(const ros::NodeHandle& nh); + + private: + + void initParams(); + void initRos(); + + void populateMetadataDefaults(ouster::sensor::sensor_info& info, + ouster::sensor::lidar_mode specified_lidar_mode); + bool writeMetadata(const std::string& metadata); + int connectionLoop(const ouster::sensor::sensor_info info); + void advertiseService(const ouster::sensor::sensor_info info); + + std::string hostname_; + std::string udp_dest_; + uint32_t lidar_port_; + uint32_t imu_port_; + bool replay_; + std::string lidar_mode_arg_; + std::string timestamp_mode_arg_; + std::string udp_profile_lidar_arg_; + std::string meta_file_; + + ros::NodeHandle nh_; + ros::ServiceServer srv_; + ros::Publisher lidar_pub_; + ros::Publisher imu_pub_; + ros::Publisher meta_pub_; + ros::Subscriber meta_sub_; + + std::shared_ptr cli_; + +}; +#endif diff --git a/src/decoder.cpp b/src/decoder.cpp index 908b28b..5558789 100644 --- a/src/decoder.cpp +++ b/src/decoder.cpp @@ -1,5 +1,5 @@ -/* -* functions for decoder node +/* January 2024 Refactor +* Functions for decoder node */ #include "ouster_decoder/decoder.h" @@ -14,9 +14,9 @@ Decoder::Decoder(const ros::NodeHandle& pnh) : pnh_(pnh), it_(pnh) 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()); @@ -31,6 +31,7 @@ void Decoder::InitRos() 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_); @@ -106,7 +107,11 @@ void Decoder::InitOuster() void Decoder::InitModel(const std::string& metadata) { // parse metadata into lidar model - model_ = LidarModel{metadata}; + std::cout << "metadata: "<< metadata << std::endl; + std::string legacy_metadata = ouster_ros::sensor::convert_to_legacy(metadata); + std::cout << "legacy: " << metadata < -#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"; - - if (!info.sn.size()) info.sn = "UNKNOWN"; - - 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()); - - if (!info.mode) { - ROS_WARN("Lidar mode not found in metadata; output may not be reliable"); - info.mode = specified_lidar_mode; - } - - if (!info.prod_line.size()) info.prod_line = "UNKNOWN"; - - 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; - } -} - -// try to write metadata file -bool write_metadata(const std::string& meta_file, const std::string& metadata) { - std::ofstream ofs; - ofs.open(meta_file); - ofs << metadata << std::endl; - ofs.close(); - 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()); - 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); - - PacketMsg lidar_packet, imu_packet; - lidar_packet.buf.resize(pf.lidar_packet_size + 1); - imu_packet.buf.resize(pf.imu_packet_size + 1); +#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(); + } + } - while (ros::ok()) { - auto state = sensor::poll_client(cli); - if (state == sensor::EXIT) { - ROS_INFO("poll_client: caught signal, exiting"); - return EXIT_SUCCESS; + 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"); + + 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 (state & sensor::CLIENT_ERROR) { - ROS_ERROR("poll_client: returned error"); - return EXIT_FAILURE; + + if (!replay_ && (!hostname_.size() || !udp_dest_.size())) + { + ROS_ERROR("Must specify both hostname and udp destination when not in replay mode"); + 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 (replay_) + { + try + { + ROS_INFO("Running in replay mode"); + info = ouster::sensor::metadata_from_json(meta_file_); + advertiseService(info); + + } catch (const std::runtime_error& e) + { + ROS_ERROR("Error running in replay mode: %s", e.what()); + } } - if (state & sensor::IMU_DATA) { - if (sensor::read_imu_packet(cli, imu_packet.buf.data(), pf)) - imu_packet_pub.publish(imu_packet); + else + { + 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_ = hostname_+".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); + populateMetadataDefaults(info, lidar_mode); + + advertiseService(info); } - ros::spinOnce(); - } - return EXIT_SUCCESS; -} + 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()); -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 success = connectionLoop(info); } -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{}); - - 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; +void Driver::populateMetadataDefaults(ouster::sensor::sensor_info& info, ouster::sensor::lidar_mode specified_lidar_mode) +{ + if (!info.name.size()) info.name = "UNKNOWN"; + if (!info.sn.size()) info.sn = "UNKNOWN"; + + 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 < ouster::sensor::min_version) + ROS_WARN("Firmware < %s not supported; output may not be reliable", + to_string(ouster::sensor::min_version).c_str()); + + if (!info.mode) + { + ROS_WARN("Lidar mode not found in metadata; output may not be reliable"); + info.mode = specified_lidar_mode; } - } - - // 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"); + if (!info.prod_line.size()) info.prod_line = "UNKNOWN"; - 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 (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 = ouster::sensor::gen1_azimuth_angles; + info.beam_altitude_angles = ouster::sensor::gen1_altitude_angles; } - } - - // 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()); +bool Driver::writeMetadata(const std::string& metadata) +{ + std::ofstream ofs; + ofs.open(meta_file_); + ofs << metadata << std::endl; + ofs.close(); + 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()); + return false; } - } 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); + return true; +} - if (!cli) { - ROS_ERROR("Failed to initialize sensor at: %s", hostname.c_str()); - return EXIT_FAILURE; +int Driver::connectionLoop(const ouster::sensor::sensor_info info) +{ + ouster::sensor::packet_format pf = ouster::sensor::get_format(info); + + 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()) + { + 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 & ouster::sensor::CLIENT_ERROR) + { + ROS_ERROR("Client: returned error"); + ros::shutdown(); + } + 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 & ouster::sensor::IMU_DATA) + { + if (ouster::sensor::read_imu_packet(*cli_, imu_packet.buf.data(), pf)) + { + imu_pub_.publish(imu_packet); + } + } + ros::spinOnce(); } - ROS_INFO("Sensor initialized successfully"); + return EXIT_SUCCESS; +} - // 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()); - } +void Driver::advertiseService(const ouster::sensor::sensor_info info) +{ + std::string metadata = ouster::sensor::to_string(info); - // 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; + 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; + }); + //srv.request.metadata = metadata; + //srv_ = nh_.advertiseService("get_metadata", &Driver::validateMetadata, &srv); + ROS_INFO("Advertising Metadata on service %s.", srv_.getService().c_str()); +} - // 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); +} - // publish packet messages from the sensor - return connection_loop(nh, *cli, info); - } +void Driver::initParams() +{ + 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); + replay_ = nh_.param("replay", false); + lidar_mode_arg_ = nh_.param("lidar_mode", std::string{}); + timestamp_mode_arg_ = nh_.param("timestamp_mode", std::string{}); + nh_.param("udp_profile_lidar", udp_profile_lidar_arg_, ""); + nh_.param("metadata", meta_file_, ""); } diff --git a/src/driver_node.cpp b/src/driver_node.cpp new file mode 100644 index 0000000..16d33fc --- /dev/null +++ b/src/driver_node.cpp @@ -0,0 +1,11 @@ +#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/driver_old.cpp b/src/driver_old.cpp new file mode 100644 index 0000000..caf7830 --- /dev/null +++ b/src/driver_old.cpp @@ -0,0 +1,304 @@ +/** + * 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. +// The logic here was kept the same in the 2024 refactor. + + +#include +#include + +#include +#include +#include + +#include "ouster_ros/GetMetadata.h" +#include "ouster_ros/PacketMsg.h" +#include "ouster/types.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"; + + if (!info.sn.size()) info.sn = "UNKNOWN"; + + 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()); + + if (!info.mode) { + ROS_WARN("Lidar mode not found in metadata; output may not be reliable"); + info.mode = specified_lidar_mode; + } + + if (!info.prod_line.size()) info.prod_line = "UNKNOWN"; + + 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; + } +} + +// try to write metadata file +bool write_metadata(const std::string& meta_file, const std::string& metadata) { + std::ofstream ofs; + ofs.open(meta_file); + ofs << metadata << std::endl; + ofs.close(); + 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()); + 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); + + 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; + } + if (state & sensor::CLIENT_ERROR) { + ROS_ERROR("poll_client: returned error"); + return EXIT_FAILURE; + } + if (state & sensor::LIDAR_DATA) { + if (sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf)) + lidar_packet_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); + } + 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); + + std::cout << "Metadata: " << metadata << std::endl; + //std::string legacy_metadata = sensor::convert_to_legacy(metadata); + + 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{}); + + 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; + } + } + + // 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()); + + // publish packet messages from the sensor + return connection_loop(nh, *cli, info); + } +} diff --git a/src/lidar.cpp b/src/lidar.cpp index f3e3738..b97acf3 100644 --- a/src/lidar.cpp +++ b/src/lidar.cpp @@ -24,9 +24,11 @@ std::vector TransformDeg2Rad(const std::vector& degs) // LidarModel LidarModel::LidarModel(const std::string& metadata) { + std::cout << "Metadata: "<< metadata << std::endl; info = ouster_ros::sensor::parse_metadata(metadata); + std::cout << "parsed metadata" << std::endl; pf = &ouster_ros::sensor::get_format(info); - + rows = info.beam_altitude_angles.size(); cols = ouster_ros::sensor::n_cols_of_lidar_mode(info.mode); freq = ouster_ros::sensor::frequency_of_lidar_mode(info.mode); From aa7c17864e74b5464c5effedb4cdb4f3126ade0d Mon Sep 17 00:00:00 2001 From: Jason Date: Fri, 26 Jan 2024 13:49:14 -0500 Subject: [PATCH 03/12] working on ouster --- include/ouster_decoder/driver.h | 5 +-- src/decoder.cpp | 6 +--- src/driver.cpp | 63 ++++++++++++++++++--------------- src/lidar.cpp | 2 ++ 4 files changed, 40 insertions(+), 36 deletions(-) diff --git a/include/ouster_decoder/driver.h b/include/ouster_decoder/driver.h index 651dd15..14cbb0b 100644 --- a/include/ouster_decoder/driver.h +++ b/include/ouster_decoder/driver.h @@ -23,8 +23,9 @@ class Driver void initParams(); void initRos(); - void populateMetadataDefaults(ouster::sensor::sensor_info& info, - ouster::sensor::lidar_mode specified_lidar_mode); + ouster::sensor::sensor_info populateMetadataDefaults( + ouster::sensor::sensor_info& info, + ouster::sensor::lidar_mode specified_lidar_mode); bool writeMetadata(const std::string& metadata); int connectionLoop(const ouster::sensor::sensor_info info); void advertiseService(const ouster::sensor::sensor_info info); diff --git a/src/decoder.cpp b/src/decoder.cpp index 5558789..df44538 100644 --- a/src/decoder.cpp +++ b/src/decoder.cpp @@ -107,11 +107,7 @@ void Decoder::InitOuster() void Decoder::InitModel(const std::string& metadata) { // parse metadata into lidar model - std::cout << "metadata: "<< metadata << std::endl; - std::string legacy_metadata = ouster_ros::sensor::convert_to_legacy(metadata); - std::cout << "legacy: " << metadata < TransformDeg2Rad(const std::vector& degs) LidarModel::LidarModel(const std::string& metadata) { std::cout << "Metadata: "<< metadata << std::endl; + //std::string legacy_metadata = ouster::sensor::convert_to_legacy(metadata); + //std::cout << "converted..." << std::endl; info = ouster_ros::sensor::parse_metadata(metadata); std::cout << "parsed metadata" << std::endl; pf = &ouster_ros::sensor::get_format(info); From 4af3a6a902dc13d631ed7e328241d9cc31bf46e4 Mon Sep 17 00:00:00 2001 From: Jason Date: Fri, 26 Jan 2024 16:23:37 -0500 Subject: [PATCH 04/12] working with bags and ouster --- include/ouster_decoder/driver.h | 3 --- src/driver.cpp | 35 +++------------------------------ src/lidar.cpp | 4 ---- 3 files changed, 3 insertions(+), 39 deletions(-) diff --git a/include/ouster_decoder/driver.h b/include/ouster_decoder/driver.h index 14cbb0b..40ad16a 100644 --- a/include/ouster_decoder/driver.h +++ b/include/ouster_decoder/driver.h @@ -23,9 +23,6 @@ class Driver void initParams(); void initRos(); - ouster::sensor::sensor_info populateMetadataDefaults( - ouster::sensor::sensor_info& info, - ouster::sensor::lidar_mode specified_lidar_mode); bool writeMetadata(const std::string& metadata); int connectionLoop(const ouster::sensor::sensor_info info); void advertiseService(const ouster::sensor::sensor_info info); diff --git a/src/driver.cpp b/src/driver.cpp index d5ef37e..e744618 100644 --- a/src/driver.cpp +++ b/src/driver.cpp @@ -83,7 +83,6 @@ Driver::Driver(const ros::NodeHandle& nh) : nh_(nh) ROS_ERROR("Couldn't write metadata to json, continueing"); } info = ouster::sensor::parse_metadata(metadata); - //info = populateMetadataDefaults(info, lidar_mode); advertiseService(info); } catch(const std::exception& e) @@ -96,36 +95,7 @@ Driver::Driver(const ros::NodeHandle& nh) : nh_(nh) info.prod_line.c_str(), info.sn.c_str(), info.fw_rev.c_str()); - - int success = connectionLoop(info); -} - -ouster::sensor::sensor_info Driver::populateMetadataDefaults(ouster::sensor::sensor_info& info, ouster::sensor::lidar_mode specified_lidar_mode) -{ - if (!info.name.size()) info.name = "UNKNOWN"; - if (!info.sn.size()) info.sn = "UNKNOWN"; - - 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 < ouster::sensor::min_version) - ROS_WARN("Firmware < %s not supported; output may not be reliable", - to_string(ouster::sensor::min_version).c_str()); - - if (!info.mode) - { - ROS_WARN("Lidar mode not found in metadata; output may not be reliable"); - info.mode = specified_lidar_mode; - } - if (!info.prod_line.size()) info.prod_line = "UNKNOWN"; - - 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 = ouster::sensor::gen1_azimuth_angles; - info.beam_altitude_angles = ouster::sensor::gen1_altitude_angles; - } - return info; + if (!replay_) int success = connectionLoop(info); } bool Driver::writeMetadata(const std::string& metadata) @@ -157,7 +127,8 @@ int Driver::connectionLoop(const ouster::sensor::sensor_info info) 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); - + + ROS_INFO("Publishing Packets..."); while (ros::ok()) { ouster::sensor::client_state state = ouster::sensor::poll_client(*cli_); diff --git a/src/lidar.cpp b/src/lidar.cpp index 0ad9b52..48e20a5 100644 --- a/src/lidar.cpp +++ b/src/lidar.cpp @@ -24,11 +24,7 @@ std::vector TransformDeg2Rad(const std::vector& degs) // LidarModel LidarModel::LidarModel(const std::string& metadata) { - std::cout << "Metadata: "<< metadata << std::endl; - //std::string legacy_metadata = ouster::sensor::convert_to_legacy(metadata); - //std::cout << "converted..." << std::endl; info = ouster_ros::sensor::parse_metadata(metadata); - std::cout << "parsed metadata" << std::endl; pf = &ouster_ros::sensor::get_format(info); rows = info.beam_altitude_angles.size(); From f5a86b369d2401da0ee396dc6435afbebaba6389 Mon Sep 17 00:00:00 2001 From: Jason Date: Fri, 26 Jan 2024 17:11:52 -0500 Subject: [PATCH 05/12] added launch file for singular command --- launch/ouster.launch | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) create mode 100644 launch/ouster.launch diff --git a/launch/ouster.launch b/launch/ouster.launch new file mode 100644 index 0000000..d4a1fd2 --- /dev/null +++ b/launch/ouster.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + From 9ae5f0c064a5a9b76c19244cd02bd8b08222b526 Mon Sep 17 00:00:00 2001 From: Jason Date: Wed, 31 Jan 2024 16:45:30 -0500 Subject: [PATCH 06/12] New launch file, namespace handling, and disaster handling --- config/ouster.yaml | 6 ++++++ include/ouster_decoder/driver.h | 1 + launch/decoder.launch | 5 +---- launch/driver.launch | 10 ++++----- launch/ouster.launch | 4 ++-- src/decoder.cpp | 8 ++++--- src/driver.cpp | 37 +++++++++++++++++++++++++-------- 7 files changed, 48 insertions(+), 23 deletions(-) create mode 100644 config/ouster.yaml diff --git a/config/ouster.yaml b/config/ouster.yaml new file mode 100644 index 0000000..14f42ef --- /dev/null +++ b/config/ouster.yaml @@ -0,0 +1,6 @@ +sensor_hostname: 192.168.100.12 +udp_dest: 192.168.100.1 +lidar_port: 7502 +imu_port: 7503 + +replay: false diff --git a/include/ouster_decoder/driver.h b/include/ouster_decoder/driver.h index 40ad16a..eff4cba 100644 --- a/include/ouster_decoder/driver.h +++ b/include/ouster_decoder/driver.h @@ -24,6 +24,7 @@ class Driver void initRos(); bool writeMetadata(const std::string& metadata); + void metadataCallback(const std_msgs::String msg); int connectionLoop(const ouster::sensor::sensor_info info); void advertiseService(const ouster::sensor::sensor_info info); diff --git a/launch/decoder.launch b/launch/decoder.launch index db9a018..575a567 100644 --- a/launch/decoder.launch +++ b/launch/decoder.launch @@ -15,8 +15,6 @@ - - @@ -42,10 +40,9 @@ - + - diff --git a/launch/driver.launch b/launch/driver.launch index f0ef221..cfefd26 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -1,9 +1,9 @@ - - - - - + + + + + diff --git a/launch/ouster.launch b/launch/ouster.launch index d4a1fd2..c86d0e7 100644 --- a/launch/ouster.launch +++ b/launch/ouster.launch @@ -3,18 +3,18 @@ - + + - diff --git a/src/decoder.cpp b/src/decoder.cpp index df44538..2ec1055 100644 --- a/src/decoder.cpp +++ b/src/decoder.cpp @@ -41,7 +41,7 @@ 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_); @@ -298,8 +298,10 @@ 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; } diff --git a/src/driver.cpp b/src/driver.cpp index e744618..5948d8e 100644 --- a/src/driver.cpp +++ b/src/driver.cpp @@ -35,20 +35,28 @@ Driver::Driver(const ros::NodeHandle& nh) : nh_(nh) } } - if (!replay_ && (!hostname_.size() || !udp_dest_.size())) + if (!replay_) { - ROS_ERROR("Must specify both hostname and udp destination when not in replay mode"); - ros::shutdown(); + 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 (replay_) { try { - ROS_INFO("Running in replay mode"); - info = ouster::sensor::metadata_from_json(meta_file_); - advertiseService(info); - + 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()); @@ -95,6 +103,11 @@ Driver::Driver(const ros::NodeHandle& nh) : nh_(nh) 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); } @@ -163,7 +176,7 @@ int Driver::connectionLoop(const ouster::sensor::sensor_info info) void Driver::advertiseService(const ouster::sensor::sensor_info info) { - std::string metadata = info.original_string();//ouster::sensor::to_string(info); + std::string metadata = info.original_string(); if (srv_) { @@ -183,17 +196,23 @@ void Driver::initRos() { lidar_pub_ = nh_.advertise("lidar_packets", 1280); imu_pub_ = nh_.advertise("imu_packets", 100); + meta_pub_ = nh_.advertise("metadata", 1); } 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); - replay_ = nh_.param("replay", false); lidar_mode_arg_ = nh_.param("lidar_mode", std::string{}); timestamp_mode_arg_ = nh_.param("timestamp_mode", std::string{}); nh_.param("udp_profile_lidar", udp_profile_lidar_arg_, ""); nh_.param("metadata", meta_file_, ""); } + +void Driver::metadataCallback(const std_msgs::String msg) +{ + advertiseService(ouster::sensor::parse_metadata(msg.data)); +} From 3a7218fabdfbb1638c116f58c366189173ff8165 Mon Sep 17 00:00:00 2001 From: Jason Date: Fri, 2 Feb 2024 12:28:55 -0500 Subject: [PATCH 07/12] added documentation and tested --- .gitignore | 1 + README.md | 44 ++-- config/ouster.yaml | 6 - include/ouster_decoder/decoder.h | 165 ++++++++++----- include/ouster_decoder/driver.h | 69 ++++++- include/ouster_decoder/lidar.h | 343 ++++++++++++++++++++----------- launch/decoder.launch | 1 + launch/ouster.launch | 10 +- src/driver.cpp | 3 +- src/lidar.cpp | 5 +- 10 files changed, 447 insertions(+), 200 deletions(-) delete mode 100644 config/ouster.yaml 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/README.md b/README.md index a4cf3ff..e4c5517 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_example (>3ms), tested on Intel i7 11th gen cpu. This will also notify you when packets are dropped. The decoder only supports LEGACY and single return profile. Currently there's no plan for dual return profile. @@ -11,22 +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. -## Usage +## 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. -Run the ouster driver +## Usage +To start everything at once use: ``` -roslaunch ouster_decoder driver.launch replay:=true/false +roslaunch ouster_decoder driver.launch ``` -Then run the decoder +Run just the driver (if you want to bag the packets for later decoding) ``` -roslaunch ouster_decoder decoder.launch +roslaunch ouster_decoder driver.launch ``` -Running on hardware requires setting a few more parameters: + +To run with a bag file run: ``` -roslaunch ouster_decoder driver.launch sensor_hostname:=192.168.100.12 lidar_port:=7502 imu_port:=7503 udp_dest:=192.168.100.1 replay:=false +roslaunch ouster_decoder ouster.launch replay:=true ``` -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. +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 (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 @@ -39,8 +55,8 @@ struct Data { float x; float y; float z; - uint32_t range; // range - uint32_t signal; // signal + uint16_t r16u; // range + uint16_t s16u; // signal }; ``` @@ -57,5 +73,3 @@ 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. -## Notes -CPU usage is 9.0 percent with ouster_ros. diff --git a/config/ouster.yaml b/config/ouster.yaml deleted file mode 100644 index 14f42ef..0000000 --- a/config/ouster.yaml +++ /dev/null @@ -1,6 +0,0 @@ -sensor_hostname: 192.168.100.12 -udp_dest: 192.168.100.1 -lidar_port: 7502 -imu_port: 7503 - -replay: false diff --git a/include/ouster_decoder/decoder.h b/include/ouster_decoder/decoder.h index c1f9b7a..92868c0 100644 --- a/include/ouster_decoder/decoder.h +++ b/include/ouster_decoder/decoder.h @@ -1,6 +1,10 @@ -/* -* Header file for decoder.cpp -*/ +/*! + * Kumar Robotics + * Januart 2024 + * @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 @@ -19,61 +23,122 @@ constexpr double kDefaultGravity = 9.807; class Decoder { - public: - explicit Decoder(const ros::NodeHandle& pnh); + 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; + // 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); + /*! + * @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: - /// 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); + 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); - /// Whether we are still waiting for alignment to mid 0 - [[nodiscard]] bool NeedAlign(int mid); + /*! + * @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); - /// Publish messages - void PublishAndReset(); + /*! + * @breif: publish the pointcloud, range and signal images, and + * camera info. Reset the lidar scan. + */ + void PublishAndReset(); - /// Record processing time of lidar callback, print warning if it exceeds time - /// between two packets - void Timing(const ros::Time& start) const; + /*! + * @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 - 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_; + // 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 - LidarScan scan_; - LidarModel model_; - sensor_msgs::CameraInfoPtr cinfo_msg_; + // 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 - 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 + // 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 index eff4cba..5256b92 100644 --- a/include/ouster_decoder/driver.h +++ b/include/ouster_decoder/driver.h @@ -1,7 +1,15 @@ +/*! + * Kumar Robotics + * January 2024 + * @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 @@ -16,35 +24,92 @@ 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_; - std::string udp_profile_lidar_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_; }; diff --git a/include/ouster_decoder/lidar.h b/include/ouster_decoder/lidar.h index 4cbb242..e9469f4 100644 --- a/include/ouster_decoder/lidar.h +++ b/include/ouster_decoder/lidar.h @@ -1,6 +1,8 @@ -/* January 2024 -* Basic structs for data organization around lidar scans and lidar metadata -*/ +/*! Kumar Robotics + * January 2024 + * Structs for data organization around lidar scans, lidar metadata and range/signal images + * Authors: Chao Qu and Jason Hughes + */ #pragma once @@ -14,132 +16,239 @@ inline constexpr double Deg2Rad(double deg) { return deg * M_PI / 180.0; } -/// @brief image data in scan +/*! + * @brief struct for organize image data in a 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; - } + // @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 +/*! +* @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; + 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 +/*! +* @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() + + // @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)); - } - - 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; + } + + /*! + * @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; }; - -std::vector MakePointFieldsXYZI() noexcept; - diff --git a/launch/decoder.launch b/launch/decoder.launch index 575a567..8a131ac 100644 --- a/launch/decoder.launch +++ b/launch/decoder.launch @@ -4,6 +4,7 @@ + diff --git a/launch/ouster.launch b/launch/ouster.launch index c86d0e7..ec38a4e 100644 --- a/launch/ouster.launch +++ b/launch/ouster.launch @@ -1,12 +1,12 @@ - - - - + + + + - + diff --git a/src/driver.cpp b/src/driver.cpp index 5948d8e..5553b93 100644 --- a/src/driver.cpp +++ b/src/driver.cpp @@ -83,7 +83,7 @@ Driver::Driver(const ros::NodeHandle& nh) : nh_(nh) metadata = ouster::sensor::get_metadata (*cli_, 20, true); if (meta_file_.empty()) { - meta_file_ = hostname_+".json"; + meta_file_ = "metadata.json"; ROS_INFO("No metadata json specified, using %s", meta_file_.c_str()); } if (!writeMetadata(metadata)) @@ -208,7 +208,6 @@ void Driver::initParams() 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("udp_profile_lidar", udp_profile_lidar_arg_, ""); nh_.param("metadata", meta_file_, ""); } diff --git a/src/lidar.cpp b/src/lidar.cpp index 48e20a5..692c225 100644 --- a/src/lidar.cpp +++ b/src/lidar.cpp @@ -182,13 +182,12 @@ void LidarScan::DecodeColumn(const uint8_t* const col_buf, const LidarModel& mod if (status) { const uint8_t* const px_buf = pf.nth_px(ipx, col_buf); - //const auto raw_range = pf.px_range(px_buf); //TODO: this needs to be replaced!! 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 - signal = raw_signal[ipx]; //pf.px_signal(px_buf); //TODO: this needs to be replaced!! + signal = raw_signal[ipx]; } // s16u += pf.px_ambient(px_buf); } @@ -239,7 +238,7 @@ 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; From d0a951b695656e156d2d3aa23c71f386896dddd0 Mon Sep 17 00:00:00 2001 From: Jason Date: Fri, 2 Feb 2024 12:40:56 -0500 Subject: [PATCH 08/12] minor documentation changes --- include/ouster_decoder/decoder.h | 2 +- include/ouster_decoder/driver.h | 2 +- include/ouster_decoder/lidar.h | 2 +- src/decoder.cpp | 11 ++++++++--- src/decoder_node.cpp | 10 ++++++---- src/driver.cpp | 9 +++++++++ src/driver_node.cpp | 7 +++++++ src/lidar.cpp | 10 +++++++--- 8 files changed, 40 insertions(+), 13 deletions(-) diff --git a/include/ouster_decoder/decoder.h b/include/ouster_decoder/decoder.h index 92868c0..0f75686 100644 --- a/include/ouster_decoder/decoder.h +++ b/include/ouster_decoder/decoder.h @@ -1,6 +1,6 @@ /*! * Kumar Robotics - * Januart 2024 + * Januart 2024 refactor * @breif: decodes incoming lidar and imu packets and publishes them * on appropriate ROS topics * Authors: Chao Qu and Jason Hughes diff --git a/include/ouster_decoder/driver.h b/include/ouster_decoder/driver.h index 5256b92..ca7a8fc 100644 --- a/include/ouster_decoder/driver.h +++ b/include/ouster_decoder/driver.h @@ -1,6 +1,6 @@ /*! * Kumar Robotics - * January 2024 + * 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. diff --git a/include/ouster_decoder/lidar.h b/include/ouster_decoder/lidar.h index e9469f4..863a781 100644 --- a/include/ouster_decoder/lidar.h +++ b/include/ouster_decoder/lidar.h @@ -1,5 +1,5 @@ /*! Kumar Robotics - * January 2024 + * January 2024 refactor * Structs for data organization around lidar scans, lidar metadata and range/signal images * Authors: Chao Qu and Jason Hughes */ diff --git a/src/decoder.cpp b/src/decoder.cpp index 2ec1055..15d3727 100644 --- a/src/decoder.cpp +++ b/src/decoder.cpp @@ -1,6 +1,11 @@ -/* January 2024 Refactor -* Functions for decoder node -*/ +/*! + * 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" diff --git a/src/decoder_node.cpp b/src/decoder_node.cpp index 52579df..9027d56 100644 --- a/src/decoder_node.cpp +++ b/src/decoder_node.cpp @@ -1,7 +1,9 @@ -/* January 2024 -* As part of the refactor for upgrades to ouster_ros v0.10+ -* Node start script for ouster decoder -*/ +/*! + * Kumar Robotics + * January 2024 Refactor + * @brief: starter for the ros decoder node + * Authors: Jason Hughes + */ #include "ouster_decoder/decoder.h" diff --git a/src/driver.cpp b/src/driver.cpp index 5553b93..ee69044 100644 --- a/src/driver.cpp +++ b/src/driver.cpp @@ -1,3 +1,12 @@ +/*! +* 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) diff --git a/src/driver_node.cpp b/src/driver_node.cpp index 16d33fc..d4f5227 100644 --- a/src/driver_node.cpp +++ b/src/driver_node.cpp @@ -1,3 +1,10 @@ +/*! +* 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) diff --git a/src/lidar.cpp b/src/lidar.cpp index 692c225..f277c42 100644 --- a/src/lidar.cpp +++ b/src/lidar.cpp @@ -1,6 +1,10 @@ -/* January 2024 -* cpp file for lidar data structs -*/ +/*! + * Kumar Robotics + * January 2024 refactor + * @breif logic ofr handling lidar metadata, image formatting and + * incoming lidar buffers from the packets + * Author: Chao Qu + */ #include "ouster_decoder/lidar.h" From 179dcd35e4cfd4ae51b82f61f24054a2113a4b62 Mon Sep 17 00:00:00 2001 From: Jason Date: Fri, 2 Feb 2024 12:42:24 -0500 Subject: [PATCH 09/12] add sdk version --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index e4c5517..0eeb595 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # ouster_decoder -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. +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. From eab0a1f654440199924cdcfd2930ba647fafa822 Mon Sep 17 00:00:00 2001 From: Jason Date: Fri, 2 Feb 2024 15:13:48 -0500 Subject: [PATCH 10/12] removed depricated driver node --- CMakeLists.txt | 2 - src/driver_old.cpp | 304 --------------------------------------------- 2 files changed, 306 deletions(-) delete mode 100644 src/driver_old.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index f7ecda8..0bee6bb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -33,5 +33,3 @@ 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(driver_old_node src/driver_old.cpp) -target_link_libraries(driver_old_node PRIVATE ${catkin_LIBRARIES}) diff --git a/src/driver_old.cpp b/src/driver_old.cpp deleted file mode 100644 index caf7830..0000000 --- a/src/driver_old.cpp +++ /dev/null @@ -1,304 +0,0 @@ -/** - * 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. -// The logic here was kept the same in the 2024 refactor. - - -#include -#include - -#include -#include -#include - -#include "ouster_ros/GetMetadata.h" -#include "ouster_ros/PacketMsg.h" -#include "ouster/types.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"; - - if (!info.sn.size()) info.sn = "UNKNOWN"; - - 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()); - - if (!info.mode) { - ROS_WARN("Lidar mode not found in metadata; output may not be reliable"); - info.mode = specified_lidar_mode; - } - - if (!info.prod_line.size()) info.prod_line = "UNKNOWN"; - - 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; - } -} - -// try to write metadata file -bool write_metadata(const std::string& meta_file, const std::string& metadata) { - std::ofstream ofs; - ofs.open(meta_file); - ofs << metadata << std::endl; - ofs.close(); - 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()); - 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); - - 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; - } - if (state & sensor::CLIENT_ERROR) { - ROS_ERROR("poll_client: returned error"); - return EXIT_FAILURE; - } - if (state & sensor::LIDAR_DATA) { - if (sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf)) - lidar_packet_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); - } - 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); - - std::cout << "Metadata: " << metadata << std::endl; - //std::string legacy_metadata = sensor::convert_to_legacy(metadata); - - 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{}); - - 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; - } - } - - // 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()); - - // publish packet messages from the sensor - return connection_loop(nh, *cli, info); - } -} From 13418231ecea43560c8610c28ac1b9bb9fd7d238 Mon Sep 17 00:00:00 2001 From: Jason Date: Fri, 9 Feb 2024 11:51:55 -0500 Subject: [PATCH 11/12] fixed docs, defaults and matched formatting --- CMakeLists.txt | 21 +- README.md | 14 +- cmake/CompilerWarnings.cmake | 52 +++++ include/ouster_decoder/decoder.h | 2 +- include/ouster_decoder/driver.h | 12 +- launch/driver.launch | 5 +- launch/ouster.launch | 6 +- src/decoder_node.cpp | 8 +- src/driver.cpp | 373 +++++++++++++++---------------- src/lidar.cpp | 2 +- 10 files changed, 270 insertions(+), 225 deletions(-) create mode 100644 cmake/CompilerWarnings.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 0bee6bb..6b1510a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,12 +1,10 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.15) project(ouster_decoder) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +set(CMAKE_CXX_STANDARD 17) +list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") +include(CompilerWarnings) -## 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 @@ -15,12 +13,8 @@ find_package(catkin REQUIRED COMPONENTS pcl_ros ouster_ros ) -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES ouster_decoder2 -# CATKIN_DEPENDS roscpp sensor_msgs -# DEPENDS system_lib -) + +catkin_package() include_directories( include @@ -29,7 +23,8 @@ include_directories( 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(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 0eeb595..8810be5 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # ouster_decoder -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. +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. @@ -15,8 +15,8 @@ Clone this repo in you catkin workspace along with [ouster-ros](https://github.c ## 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` +- `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. @@ -26,9 +26,9 @@ The following parameters may differ from the defaults that we use. They can be s - `driver_ns` namespace for driver node. ## Usage -To start everything at once use: +To start everything at once on hardware: ``` -roslaunch ouster_decoder driver.launch +roslaunch ouster_decoder ouster.launch sensor_hostname:= udp_dest:= ``` Run just the driver (if you want to bag the packets for later decoding) @@ -40,7 +40,7 @@ To run with a bag file run: ``` 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`. +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 (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. @@ -64,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 diff --git a/cmake/CompilerWarnings.cmake b/cmake/CompilerWarnings.cmake new file mode 100644 index 0000000..03e1fc1 --- /dev/null +++ b/cmake/CompilerWarnings.cmake @@ -0,0 +1,52 @@ +# from here: +# +# https://github.com/lefticus/cppbestpractices/blob/master/02-Use_the_Tools_Available.md + +function(enable_warnings target) + + set(CLANG_WARNINGS + -Wall + -Wextra # reasonable and standard + # -Wshadow # warn the user if a variable declaration shadows one from a + # parent context + -Wnon-virtual-dtor # warn the user if a class with virtual functions has a + # non-virtual destructor. This helps catch hard to + # track down memory errors + # -Wold-style-cast # warn for c-style casts + -Wcast-align # warn for potential performance problem casts + -Wunused # warn on anything being unused + -Woverloaded-virtual # warn if you overload (not override) a virtual + # function + # -Wpedantic # warn if non-standard C++ is used + # -Wconversion # warn on type conversions that may lose data + # -Wsign-conversion # warn on sign conversions + -Wnull-dereference # warn if a null dereference is detected + # -Wdouble-promotion # warn if float is implicit promoted to double + -Wformat=2 # warn on security issues around functions that format output + # (ie printf) + ) + + set(GCC_WARNINGS + ${CLANG_WARNINGS} + -Wmisleading-indentation # warn if indentation implies blocks where blocks + # do not exist + -Wduplicated-cond # warn if if / else chain has duplicated conditions + # -Wduplicated-branches # warn if if / else branches have duplicated code + -Wlogical-op # warn about logical operations being used where bitwise were + # probably wanted + # -Wuseless-cast # warn if you perform a cast to the same type + ) + + if(CMAKE_CXX_COMPILER_ID MATCHES ".*Clang") + set(PROJECT_WARNINGS ${CLANG_WARNINGS}) + elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(PROJECT_WARNINGS ${GCC_WARNINGS}) + else() + message( + AUTHOR_WARNING + "No compiler warnings set for '${CMAKE_CXX_COMPILER_ID}' compiler.") + endif() + + target_compile_options(${target} INTERFACE ${PROJECT_WARNINGS}) + +endfunction() diff --git a/include/ouster_decoder/decoder.h b/include/ouster_decoder/decoder.h index 0f75686..6af8b98 100644 --- a/include/ouster_decoder/decoder.h +++ b/include/ouster_decoder/decoder.h @@ -1,6 +1,6 @@ /*! * Kumar Robotics - * Januart 2024 refactor + * January 2024 refactor * @breif: decodes incoming lidar and imu packets and publishes them * on appropriate ROS topics * Authors: Chao Qu and Jason Hughes diff --git a/include/ouster_decoder/driver.h b/include/ouster_decoder/driver.h index ca7a8fc..7f2d286 100644 --- a/include/ouster_decoder/driver.h +++ b/include/ouster_decoder/driver.h @@ -37,12 +37,12 @@ class Driver * @brief: get and set all ROS params * as private class varaibles. */ - void initParams(); + void InitParams(); /*! * @brief: initialize lidar packet publisher, imu packet publisher * and metadata string publisher. */ - void initRos(); + void InitRos(); /*! * @brief: write the metadata string from the ouster to a @@ -50,26 +50,26 @@ class Driver * @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); + 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); + 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); + 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); + void AdvertiseService(const ouster::sensor::sensor_info info); // ROS Params // @brief: hostname of the ouster, usually the ip address diff --git a/launch/driver.launch b/launch/driver.launch index cfefd26..9c5307a 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -1,6 +1,6 @@ - - + + @@ -10,7 +10,6 @@ - diff --git a/launch/ouster.launch b/launch/ouster.launch index ec38a4e..9dae3fd 100644 --- a/launch/ouster.launch +++ b/launch/ouster.launch @@ -1,6 +1,6 @@ - - + + @@ -11,7 +11,7 @@ - + diff --git a/src/decoder_node.cpp b/src/decoder_node.cpp index 9027d56..4fce425 100644 --- a/src/decoder_node.cpp +++ b/src/decoder_node.cpp @@ -9,11 +9,11 @@ int main(int argc, char** argv) { - ros::init(argc, argv, "os_decoder_node"); + ros::init(argc, argv, "os_decoder_node"); - Decoder node(ros::NodeHandle("~")); - ros::spin(); + Decoder node(ros::NodeHandle("~")); + ros::spin(); - return 0; + return 0; } diff --git a/src/driver.cpp b/src/driver.cpp index ee69044..bbfe976 100644 --- a/src/driver.cpp +++ b/src/driver.cpp @@ -11,216 +11,215 @@ 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()) + 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(); + } + } + + 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"); + + timestamp_mode = ouster::sensor::timestamp_mode_of_string(timestamp_mode_arg_); + if(!timestamp_mode) { - 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(); - } + ROS_ERROR("Invalid timestamp mode %s, exiting", timestamp_mode_arg_.c_str()); + ros::shutdown(); } - - ouster::sensor::timestamp_mode timestamp_mode = ouster::sensor::TIME_FROM_UNSPEC; - if (timestamp_mode_arg_.size()) + } + + 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 (replay_) + { + try { - if (replay_) ROS_WARN("Timestamp mode set in replay mode, will be ignored"); - - 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 (!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 (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) - { + 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 + } + } + else + { + try { - 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); - - 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); + 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); + + 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); } -bool Driver::writeMetadata(const std::string& metadata) +bool Driver::WriteMetadata(const std::string& metadata) { - std::ofstream ofs; - ofs.open(meta_file_); - ofs << metadata << std::endl; - ofs.close(); - 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()); - return false; - } - return true; + std::ofstream ofs; + ofs.open(meta_file_); + ofs << metadata << std::endl; + ofs.close(); + 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()); + return false; + } + return true; } -int Driver::connectionLoop(const ouster::sensor::sensor_info info) +int Driver::ConnectionLoop(const ouster::sensor::sensor_info info) { - ouster::sensor::packet_format pf = ouster::sensor::get_format(info); - - 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); - - ROS_INFO("Publishing Packets..."); - while (ros::ok()) + ouster::sensor::packet_format pf = ouster::sensor::get_format(info); + + 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); + + 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 & ouster::sensor::CLIENT_ERROR) + { + ROS_ERROR("Client: returned error"); + ros::shutdown(); + } + 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 & ouster::sensor::IMU_DATA) { - 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 & ouster::sensor::CLIENT_ERROR) - { - ROS_ERROR("Client: returned error"); - ros::shutdown(); - } - 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 & ouster::sensor::IMU_DATA) - { - if (ouster::sensor::read_imu_packet(*cli_, imu_packet.buf.data(), pf)) - { - imu_pub_.publish(imu_packet); - } - } - ros::spinOnce(); + if (ouster::sensor::read_imu_packet(*cli_, imu_packet.buf.data(), pf)) + { + imu_pub_.publish(imu_packet); + } } - return EXIT_SUCCESS; + ros::spinOnce(); + } + return EXIT_SUCCESS; } -void Driver::advertiseService(const ouster::sensor::sensor_info info) +void Driver::AdvertiseService(const ouster::sensor::sensor_info info) { - std::string metadata = info.original_string(); - - 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()); + std::string metadata = info.original_string(); + + 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()); } -void Driver::initRos() +void Driver::InitRos() { - lidar_pub_ = nh_.advertise("lidar_packets", 1280); - imu_pub_ = nh_.advertise("imu_packets", 100); - meta_pub_ = nh_.advertise("metadata", 1); + lidar_pub_ = nh_.advertise("lidar_packets", 1280); + imu_pub_ = nh_.advertise("imu_packets", 100); + meta_pub_ = nh_.advertise("metadata", 1); } -void Driver::initParams() +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_, ""); + 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) +void Driver::MetadataCallback(const std_msgs::String msg) { - advertiseService(ouster::sensor::parse_metadata(msg.data)); + AdvertiseService(ouster::sensor::parse_metadata(msg.data)); } diff --git a/src/lidar.cpp b/src/lidar.cpp index f277c42..40c0dd0 100644 --- a/src/lidar.cpp +++ b/src/lidar.cpp @@ -1,7 +1,7 @@ /*! * Kumar Robotics * January 2024 refactor - * @breif logic ofr handling lidar metadata, image formatting and + * @breif logic for handling lidar metadata, image formatting and * incoming lidar buffers from the packets * Author: Chao Qu */ From 564b3283c24c4f4a07ea8151dc9c5efbeab6d9e9 Mon Sep 17 00:00:00 2001 From: Jason Date: Fri, 9 Feb 2024 11:59:47 -0500 Subject: [PATCH 12/12] reformatted header files --- include/ouster_decoder/decoder.h | 216 +++++++++++++++---------------- include/ouster_decoder/driver.h | 164 +++++++++++------------ include/ouster_decoder/lidar.h | 189 ++++++++++++++------------- 3 files changed, 284 insertions(+), 285 deletions(-) diff --git a/include/ouster_decoder/decoder.h b/include/ouster_decoder/decoder.h index 6af8b98..abf42df 100644 --- a/include/ouster_decoder/decoder.h +++ b/include/ouster_decoder/decoder.h @@ -23,122 +23,122 @@ 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); + 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; + // 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); + /*! + * @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); + 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: 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: 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; + /*! + * @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_; + // 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_; + // 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}; + // 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 index 7f2d286..e53add0 100644 --- a/include/ouster_decoder/driver.h +++ b/include/ouster_decoder/driver.h @@ -23,94 +23,94 @@ 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); + 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: + 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: 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); + /*! + * @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 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_; + // 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_; + // 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 index 863a781..402b8cd 100644 --- a/include/ouster_decoder/lidar.h +++ b/include/ouster_decoder/lidar.h @@ -21,34 +21,34 @@ inline constexpr double Deg2Rad(double deg) { return deg * M_PI / 180.0; } */ 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; - } + // @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, @@ -59,73 +59,72 @@ static_assert(sizeof(ImageData) == sizeof(float) * 4, */ 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; + 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; }; /*!