From 362cf2f2041cd1e57e97c38b999918d97927300e Mon Sep 17 00:00:00 2001 From: Valts-M Date: Mon, 24 Jan 2022 15:56:58 +0200 Subject: [PATCH 1/3] Made a paramater for reducing the channel count used for the point cloud output --- README.rst | 1 + ouster_ros/include/ouster_ros/ros.h | 3 ++- ouster_ros/ouster.launch | 2 ++ ouster_ros/src/os_cloud_node.cpp | 17 +++++++++++++++-- ouster_ros/src/ros.cpp | 16 ++++++++++++---- 5 files changed, 32 insertions(+), 7 deletions(-) diff --git a/README.rst b/README.rst index 33ff582f..d95bb337 100644 --- a/README.rst +++ b/README.rst @@ -241,6 +241,7 @@ You can also optionally specify: * ``lidar_mode:=`` where mode is one of ``512x10``, ``512x20``, ``1024x10``, ``1024x20``, or ``2048x10``, and * ``viz:=true`` to visualize the sensor output, if you have the rviz ROS package installed +* ``channel_reduction_ratio:=`` to reduce the amount of channels used for the output cloud. So a 128 beam lidar with a channel_reduction_ratio of 4 would result in a simulated 32 beam lidar. (Value has to be 2^n) Recording Data diff --git a/ouster_ros/include/ouster_ros/ros.h b/ouster_ros/include/ouster_ros/ros.h index ca99536c..a36c1016 100644 --- a/ouster_ros/include/ouster_ros/ros.h +++ b/ouster_ros/include/ouster_ros/ros.h @@ -63,10 +63,11 @@ sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, * @param ls input lidar data * @param return_index index of return desired starting at 0 * @param cloud output pcl pointcloud to populate + * @param channel_reduction_ratio ratio at which to reduce channel count */ void scan_to_cloud(const ouster::XYZLut& xyz_lut, ouster::LidarScan::ts_t scan_ts, const ouster::LidarScan& ls, - ouster_ros::Cloud& cloud, int return_index = 0); + ouster_ros::Cloud& cloud, uint32_t channel_reduction_ratio, int return_index = 0); /** * Serialize a PCL point cloud to a ROS message diff --git a/ouster_ros/ouster.launch b/ouster_ros/ouster.launch index 0510f439..04ea55a5 100644 --- a/ouster_ros/ouster.launch +++ b/ouster_ros/ouster.launch @@ -12,6 +12,7 @@ + @@ -30,6 +31,7 @@ + diff --git a/ouster_ros/src/os_cloud_node.cpp b/ouster_ros/src/os_cloud_node.cpp index 89e2275d..d3c9791b 100644 --- a/ouster_ros/src/os_cloud_node.cpp +++ b/ouster_ros/src/os_cloud_node.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -13,6 +14,7 @@ #include #include #include +#include #include "ouster/lidar_scan.h" #include "ouster/types.h" @@ -29,6 +31,8 @@ int main(int argc, char** argv) { ros::init(argc, argv, "os_cloud_node"); ros::NodeHandle nh("~"); + uint32_t channel_reduction_ratio = nh.param("channel_reduction_ratio", 1); + auto tf_prefix = nh.param("tf_prefix", std::string{}); if (!tf_prefix.empty() && tf_prefix.back() != '/') tf_prefix.append("/"); auto sensor_frame = tf_prefix + "os_sensor"; @@ -48,6 +52,15 @@ int main(int argc, char** argv) { uint32_t W = info.format.columns_per_frame; auto udp_profile_lidar = info.format.udp_profile_lidar; + if(channel_reduction_ratio != 1){ + if(channel_reduction_ratio <= 0) + throw std::invalid_argument("channel_reduction_ratio must be greater than zero"); + if(H <= channel_reduction_ratio) + throw std::invalid_argument("channel_reduction_ratio must be less than or equal to the lidar channel count"); + if(ceil(log2(channel_reduction_ratio)) != floor(log2(channel_reduction_ratio))) + throw std::invalid_argument("channel_reduction_ratio must be 2^n"); + } + const int n_returns = (udp_profile_lidar == sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY) ? 1 @@ -71,7 +84,7 @@ int main(int argc, char** argv) { auto xyz_lut = ouster::make_xyz_lut(info); ouster::LidarScan ls{W, H, udp_profile_lidar}; - Cloud cloud{W, H}; + Cloud cloud{W, H / channel_reduction_ratio}; ouster::ScanBatcher batch(W, pf); @@ -83,7 +96,7 @@ int main(int argc, char** argv) { }); if (h != ls.headers.end()) { for (int i = 0; i < n_returns; i++) { - scan_to_cloud(xyz_lut, h->timestamp, ls, cloud, i); + scan_to_cloud(xyz_lut, h->timestamp, ls, cloud, channel_reduction_ratio, i); lidar_pubs[i].publish(ouster_ros::cloud_to_cloud_msg( cloud, h->timestamp, sensor_frame)); } diff --git a/ouster_ros/src/ros.cpp b/ouster_ros/src/ros.cpp index 7d8b1e89..66bd336e 100644 --- a/ouster_ros/src/ros.cpp +++ b/ouster_ros/src/ros.cpp @@ -94,9 +94,17 @@ sensor::ChanField suitable_return(sensor::ChanField input_field, bool second) { void scan_to_cloud(const ouster::XYZLut& xyz_lut, ouster::LidarScan::ts_t scan_ts, const ouster::LidarScan& ls, - ouster_ros::Cloud& cloud, int return_index) { + ouster_ros::Cloud& cloud, uint32_t channel_reduction_ratio, int return_index) { + if(channel_reduction_ratio != 1){ + if(channel_reduction_ratio <= 0) + throw std::invalid_argument("channel_reduction_ratio must be greater than zero"); + if(ls.h <= channel_reduction_ratio) + throw std::invalid_argument("channel_reduction_ratio must be less than or equal to the lidar channel count"); + if(ceil(log2(channel_reduction_ratio)) != floor(log2(channel_reduction_ratio))) + throw std::invalid_argument("channel_reduction_ratio must be 2^n"); + } bool second = (return_index == 1); - cloud.resize(ls.w * ls.h); + cloud.resize(ls.w * ls.h / channel_reduction_ratio); ouster::img_t near_ir; ouster::impl::visit_field( @@ -120,11 +128,11 @@ void scan_to_cloud(const ouster::XYZLut& xyz_lut, auto points = ouster::cartesian(range, xyz_lut); - for (auto u = 0; u < ls.h; u++) { + for (auto u = 0, w = 0; u < ls.h; u += channel_reduction_ratio, w++) { for (auto v = 0; v < ls.w; v++) { const auto xyz = points.row(u * ls.w + v); const auto ts = (ls.header(v).timestamp - scan_ts).count(); - cloud(v, u) = ouster_ros::Point{ + cloud(v, w) = ouster_ros::Point{ {{static_cast(xyz(0)), static_cast(xyz(1)), static_cast(xyz(2)), 1.0f}}, static_cast(signal(u, v)), From 11c4cccde8a8463576f6585f346fba9da30bfce8 Mon Sep 17 00:00:00 2001 From: Valts-M Date: Mon, 24 Jan 2022 16:16:54 +0200 Subject: [PATCH 2/3] removed unused includes --- ouster_ros/src/os_cloud_node.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/ouster_ros/src/os_cloud_node.cpp b/ouster_ros/src/os_cloud_node.cpp index d3c9791b..3b4024ea 100644 --- a/ouster_ros/src/os_cloud_node.cpp +++ b/ouster_ros/src/os_cloud_node.cpp @@ -6,7 +6,6 @@ #include #include #include -#include #include #include #include @@ -14,7 +13,6 @@ #include #include #include -#include #include "ouster/lidar_scan.h" #include "ouster/types.h" From 425d377f1fad685a3165ed0c938cae886e13d316 Mon Sep 17 00:00:00 2001 From: Valts-M Date: Fri, 4 Feb 2022 10:38:05 +0200 Subject: [PATCH 3/3] small change so the channel reduction ratio paramater works with the img_node also added the img_node to the launch file --- ouster_ros/ouster.launch | 6 ++++++ ouster_ros/src/img_node.cpp | 4 +++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/ouster_ros/ouster.launch b/ouster_ros/ouster.launch index 04ea55a5..1a06f11a 100644 --- a/ouster_ros/ouster.launch +++ b/ouster_ros/ouster.launch @@ -34,6 +34,12 @@ + + + + + + diff --git a/ouster_ros/src/img_node.cpp b/ouster_ros/src/img_node.cpp index b1d59b8c..40736958 100644 --- a/ouster_ros/src/img_node.cpp +++ b/ouster_ros/src/img_node.cpp @@ -52,6 +52,8 @@ int main(int argc, char** argv) { ros::init(argc, argv, "img_node"); ros::NodeHandle nh("~"); + uint32_t channel_reduction_ratio = nh.param("channel_reduction_ratio", 1); + ouster_ros::OSConfigSrv cfg{}; auto client = nh.serviceClient("os_config"); client.waitForExistence(); @@ -61,7 +63,7 @@ int main(int argc, char** argv) { } auto info = sensor::parse_metadata(cfg.response.metadata); - size_t H = info.format.pixels_per_column; + size_t H = info.format.pixels_per_column / channel_reduction_ratio; size_t W = info.format.columns_per_frame; auto udp_profile_lidar = info.format.udp_profile_lidar;