From 277a49fc68f38f363333b37f5055f0d306d61265 Mon Sep 17 00:00:00 2001 From: Yannic Bachmann Date: Thu, 18 Jul 2024 18:17:09 +0200 Subject: [PATCH 1/7] Added scan_offset parameter. --- README.md | 3 ++- cfg/param.yaml | 1 + .../DepthImageToLaserScan.hpp | 10 +++++++--- src/DepthImageToLaserScan.cpp | 14 ++++++++------ src/DepthImageToLaserScanROS.cpp | 3 ++- test/DepthImageToLaserScanTest.cpp | 17 +++++++++-------- 6 files changed, 29 insertions(+), 19 deletions(-) diff --git a/README.md b/README.md index a3119af..c044952 100644 --- a/README.md +++ b/README.md @@ -17,5 +17,6 @@ Parameters * `scan_time` (float) - The time in seconds between scans to report to the consumer of the LaserScan message. This is set directly in the published message. Defaults to 0.033 seconds. * `range_min` (float) - The minimum distance in meters a projected point should be. Points closer than this are discarded. Defaults to 0.45 meters. * `range_max` (float) - The maximum distance in meters a projected point should be. Points further than this are discarded. Defaults to 10.0 meters. -* `scan_height` (int) - The row from the depth image to use for the laser projection. Defaults to 1. +* `scan_height` (int) - The number of rows from the depth image to use for the laser projection. Defaults to 1. +* `scan_offset` (float) - The height ratio of the depth image defining on what height the depth data should be taken from (0.0=bottom row of image, 1.0=top row). Defaults to 0.5. * `output_frame` (string) - The frame id to publish in the LaserScan message. Defaults to "camera_depth_frame". diff --git a/cfg/param.yaml b/cfg/param.yaml index 8aa0aaf..e1f4b4a 100644 --- a/cfg/param.yaml +++ b/cfg/param.yaml @@ -4,4 +4,5 @@ depthimage_to_laserscan: range_min: 0.45 range_max: 10.0 scan_height: 1 + scan_offset: 0.5 output_frame: "camera_depth_frame" diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp index 4d4b827..dd6fc70 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp +++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp @@ -69,13 +69,15 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final * radii for each angular increment. The output scan will output the closest radius that is * still not smaller than range_min. This can be used to vertically compress obstacles into * a single LaserScan. + * @param scan_offset Center position of the LaserScan. A value of 0.0 corresponds to the bottom of the image + * while 1.0 corresponds to the height of the image. * @param frame_id The output frame_id for the LaserScan. This will probably NOT be the same frame_id as the * depth image. Example: For OpenNI cameras, this should be set to 'camera_depth_frame' while * the camera uses 'camera_depth_optical_frame'. * */ explicit DepthImageToLaserScan( - float scan_time, float range_min, float range_max, int scan_height, + float scan_time, float range_min, float range_max, int scan_height, float scan_offset, const std::string & frame_id); ~DepthImageToLaserScan(); @@ -149,13 +151,14 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final * @param cam_model The image_geometry camera model for this image. * @param scan_msg The output LaserScan. * @param scan_height The number of vertical pixels to feed into each angular_measurement. + * @param scan_offset Height ratio of the image where the center of the scan line should be (0.0=bottom row, 1.0=top row). * */ template void convert( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, const image_geometry::PinholeCameraModel & cam_model, - const sensor_msgs::msg::LaserScan::UniquePtr & scan_msg, const int & scan_height) const + const sensor_msgs::msg::LaserScan::UniquePtr & scan_msg, const int & scan_height, const float & scan_offset) const { // Use correct principal point from calibration float center_x = cam_model.cx(); @@ -167,7 +170,7 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final const T * depth_row = reinterpret_cast(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(T); - int offset = static_cast(cam_model.cy() - static_cast(scan_height) / 2.0); + int offset = static_cast((cam_model.cy()*2*scan_offset) - static_cast(scan_height) / 2.0); depth_row += offset * row_step; // Offset to center of image for (int v = offset; v < offset + scan_height_; v++, depth_row += row_step) { for (uint32_t u = 0; u < depth_msg->width; u++) { // Loop over each pixel in row @@ -202,6 +205,7 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final float range_min_; ///< Stores the current minimum range to use. float range_max_; ///< Stores the current maximum range to use. int scan_height_; ///< Number of pixel rows to use when producing a laserscan from an area. + float scan_offset_; ///< Height ratio of the image where the center of the scan line should be (0.0=bottom row, 1.0=top row). ///< Output frame_id for each laserscan. This is likely NOT the camera's frame_id. std::string output_frame_id_; }; diff --git a/src/DepthImageToLaserScan.cpp b/src/DepthImageToLaserScan.cpp index 4c589cd..c7d18e9 100644 --- a/src/DepthImageToLaserScan.cpp +++ b/src/DepthImageToLaserScan.cpp @@ -49,8 +49,8 @@ namespace depthimage_to_laserscan DepthImageToLaserScan::DepthImageToLaserScan( float scan_time, float range_min, float range_max, - int scan_height, const std::string & frame_id) -: scan_time_(scan_time), range_min_(range_min), range_max_(range_max), scan_height_(scan_height), + int scan_height, float scan_offset, const std::string & frame_id) +: scan_time_(scan_time), range_min_(range_min), range_max_(range_max), scan_height_(scan_height), scan_offset_(scan_offset), output_frame_id_(frame_id) { } @@ -143,8 +143,10 @@ sensor_msgs::msg::LaserScan::UniquePtr DepthImageToLaserScan::convert_msg( scan_msg->range_max = range_max_; // Check scan_height vs image_height - if (static_cast(scan_height_) / 2.0 > cam_model_.cy() || - static_cast(scan_height_) / 2.0 > depth_msg->height - cam_model_.cy()) + double center_row = cam_model_.cy()*2*scan_offset_; + double bottom_row = center_row - scan_height_/2; + double top_row = center_row + scan_height_/2; + if (bottom_row < 0 || top_row >= depth_msg->height) { std::stringstream ss; ss << "scan_height ( " << scan_height_ << " pixels) is too large for the image height."; @@ -156,9 +158,9 @@ sensor_msgs::msg::LaserScan::UniquePtr DepthImageToLaserScan::convert_msg( scan_msg->ranges.assign(ranges_size, std::numeric_limits::quiet_NaN()); if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) { - convert(depth_msg, cam_model_, scan_msg, scan_height_); + convert(depth_msg, cam_model_, scan_msg, scan_height_, scan_offset_); } else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) { - convert(depth_msg, cam_model_, scan_msg, scan_height_); + convert(depth_msg, cam_model_, scan_msg, scan_height_, scan_offset_); } else { std::stringstream ss; ss << "Depth image has unsupported encoding: " << depth_msg->encoding; diff --git a/src/DepthImageToLaserScanROS.cpp b/src/DepthImageToLaserScanROS.cpp index 2c4c0fe..c6f2b65 100644 --- a/src/DepthImageToLaserScanROS.cpp +++ b/src/DepthImageToLaserScanROS.cpp @@ -68,11 +68,12 @@ DepthImageToLaserScanROS::DepthImageToLaserScanROS(const rclcpp::NodeOptions & o float range_max = this->declare_parameter("range_max", 10.0); int scan_height = this->declare_parameter("scan_height", 1); + int scan_offset = this->declare_parameter("scan_offset", 0.5); std::string output_frame = this->declare_parameter("output_frame", "camera_depth_frame"); dtl_ = std::make_unique( - scan_time, range_min, range_max, scan_height, output_frame); + scan_time, range_min, range_max, scan_height, scan_offset, output_frame); } DepthImageToLaserScanROS::~DepthImageToLaserScanROS() diff --git a/test/DepthImageToLaserScanTest.cpp b/test/DepthImageToLaserScanTest.cpp index 9547b84..8127f3f 100644 --- a/test/DepthImageToLaserScanTest.cpp +++ b/test/DepthImageToLaserScanTest.cpp @@ -45,6 +45,7 @@ const float g_scan_time = 1.0 / 30.0; const float g_range_min = 0.45; const float g_range_max = 10.0; const int g_scan_height = 1; +const float g_scan_offset = 0.5; const char g_output_frame[] = "camera_depth_frame"; // Inputs @@ -55,7 +56,7 @@ sensor_msgs::msg::CameraInfo::SharedPtr info_msg_; TEST(ConvertTest, setupLibrary) { depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, g_scan_height, g_output_frame); + g_range_max, g_scan_height, g_scan_offset, g_output_frame); depth_msg_.reset(new sensor_msgs::msg::Image); depth_msg_->header.stamp.sec = 0; @@ -128,7 +129,7 @@ TEST(ConvertTest, setupLibrary) TEST(ConvertTest, testExceptions) { depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, g_scan_height, g_output_frame); + g_range_max, g_scan_height, g_scan_offset, g_output_frame); // Test supported image encodings for exceptions // Does not segfault as long as scan_height = 1 @@ -145,7 +146,7 @@ TEST(ConvertTest, testScanHeight) { for (int scan_height = 1; scan_height <= 100; scan_height++) { depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, scan_height, g_output_frame); + g_range_max, scan_height, g_scan_offset, g_output_frame); uint16_t low_value = 500; uint16_t high_value = 3000; @@ -153,7 +154,7 @@ TEST(ConvertTest, testScanHeight) uint16_t * data = reinterpret_cast(&depth_msg_->data[0]); int row_step = depth_msg_->step / sizeof(uint16_t); - int offset = static_cast(info_msg_->k[5] - static_cast(scan_height) / 2.0); + int offset = static_cast((info_msg_->k[5]*2*g_scan_offset) - static_cast(scan_height) / 2.0); data += offset * row_step; // Offset to center of image for (int v = 0; v < scan_height; v++, data += row_step) { @@ -197,7 +198,7 @@ TEST(ConvertTest, testRandom) } depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, g_scan_height, g_output_frame); + g_range_max, g_scan_height, g_scan_offset, g_output_frame); // Convert sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(depth_msg_, info_msg_); @@ -226,7 +227,7 @@ TEST(ConvertTest, testNaN) } depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, g_scan_height, g_output_frame); + g_range_max, g_scan_height, g_scan_offset, g_output_frame); // Convert sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(float_msg, info_msg_); @@ -254,7 +255,7 @@ TEST(ConvertTest, testPositiveInf) } depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, g_scan_height, g_output_frame); + g_range_max, g_scan_height, g_scan_offset, g_output_frame); // Convert sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(float_msg, info_msg_); @@ -289,7 +290,7 @@ TEST(ConvertTest, testNegativeInf) } depthimage_to_laserscan::DepthImageToLaserScan dtl(g_scan_time, g_range_min, - g_range_max, g_scan_height, g_output_frame); + g_range_max, g_scan_height, g_scan_offset, g_output_frame); // Convert sensor_msgs::msg::LaserScan::SharedPtr scan_msg = dtl.convert_msg(float_msg, info_msg_); From eeb2e96b248349c86cca4728a03ca2c99732ad83 Mon Sep 17 00:00:00 2001 From: Yannic Bachmann Date: Fri, 19 Jul 2024 08:38:36 +0200 Subject: [PATCH 2/7] small fixes, more verbose error message --- README.md | 2 +- include/depthimage_to_laserscan/DepthImageToLaserScan.hpp | 8 ++++---- src/DepthImageToLaserScan.cpp | 2 +- src/DepthImageToLaserScanROS.cpp | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index c044952..525abfc 100644 --- a/README.md +++ b/README.md @@ -18,5 +18,5 @@ Parameters * `range_min` (float) - The minimum distance in meters a projected point should be. Points closer than this are discarded. Defaults to 0.45 meters. * `range_max` (float) - The maximum distance in meters a projected point should be. Points further than this are discarded. Defaults to 10.0 meters. * `scan_height` (int) - The number of rows from the depth image to use for the laser projection. Defaults to 1. -* `scan_offset` (float) - The height ratio of the depth image defining on what height the depth data should be taken from (0.0=bottom row of image, 1.0=top row). Defaults to 0.5. +* `scan_offset` (float) - The height ratio of the depth image defining on what height the depth data should be taken from (0.0=top row of image, 1.0=bottom row). Defaults to 0.5. * `output_frame` (string) - The frame id to publish in the LaserScan message. Defaults to "camera_depth_frame". diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp index dd6fc70..c35fa98 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp +++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp @@ -69,8 +69,8 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final * radii for each angular increment. The output scan will output the closest radius that is * still not smaller than range_min. This can be used to vertically compress obstacles into * a single LaserScan. - * @param scan_offset Center position of the LaserScan. A value of 0.0 corresponds to the bottom of the image - * while 1.0 corresponds to the height of the image. + * @param scan_offset Center position of the LaserScan. A value of 0.0 corresponds to the top row of the image + * while 1.0 corresponds to the bottom row of the image. * @param frame_id The output frame_id for the LaserScan. This will probably NOT be the same frame_id as the * depth image. Example: For OpenNI cameras, this should be set to 'camera_depth_frame' while * the camera uses 'camera_depth_optical_frame'. @@ -151,7 +151,7 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final * @param cam_model The image_geometry camera model for this image. * @param scan_msg The output LaserScan. * @param scan_height The number of vertical pixels to feed into each angular_measurement. - * @param scan_offset Height ratio of the image where the center of the scan line should be (0.0=bottom row, 1.0=top row). + * @param scan_offset Height ratio of the image where the center of the scan line should be (0.0=top row, 1.0=bottom row). * */ template @@ -205,7 +205,7 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final float range_min_; ///< Stores the current minimum range to use. float range_max_; ///< Stores the current maximum range to use. int scan_height_; ///< Number of pixel rows to use when producing a laserscan from an area. - float scan_offset_; ///< Height ratio of the image where the center of the scan line should be (0.0=bottom row, 1.0=top row). + float scan_offset_; ///< Height ratio of the image where the center of the scan line should be (0.0=top row, 1.0=bottom row). ///< Output frame_id for each laserscan. This is likely NOT the camera's frame_id. std::string output_frame_id_; }; diff --git a/src/DepthImageToLaserScan.cpp b/src/DepthImageToLaserScan.cpp index c7d18e9..517be2d 100644 --- a/src/DepthImageToLaserScan.cpp +++ b/src/DepthImageToLaserScan.cpp @@ -149,7 +149,7 @@ sensor_msgs::msg::LaserScan::UniquePtr DepthImageToLaserScan::convert_msg( if (bottom_row < 0 || top_row >= depth_msg->height) { std::stringstream ss; - ss << "scan_height ( " << scan_height_ << " pixels) is too large for the image height."; + ss << "scan_height ( " << scan_height_ << " pixels) is too large for the image height ( " << depth_msg->height << " ) with a scan_offset of " << scan_offset_ << " and cy of " << cam_model_.cy() << ". "; throw std::runtime_error(ss.str()); } diff --git a/src/DepthImageToLaserScanROS.cpp b/src/DepthImageToLaserScanROS.cpp index c6f2b65..980f344 100644 --- a/src/DepthImageToLaserScanROS.cpp +++ b/src/DepthImageToLaserScanROS.cpp @@ -68,7 +68,7 @@ DepthImageToLaserScanROS::DepthImageToLaserScanROS(const rclcpp::NodeOptions & o float range_max = this->declare_parameter("range_max", 10.0); int scan_height = this->declare_parameter("scan_height", 1); - int scan_offset = this->declare_parameter("scan_offset", 0.5); + float scan_offset = this->declare_parameter("scan_offset", 0.5); std::string output_frame = this->declare_parameter("output_frame", "camera_depth_frame"); From 9510e9c04957071fe1b1a4e265bb0570e84b849d Mon Sep 17 00:00:00 2001 From: MAHA Maia Date: Wed, 24 Jul 2024 15:53:45 +0200 Subject: [PATCH 3/7] Adhere to style guidelines --- .../DepthImageToLaserScan.hpp | 14 +++++++++----- src/DepthImageToLaserScan.cpp | 17 +++++++++-------- test/DepthImageToLaserScanTest.cpp | 3 ++- 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp index c35fa98..6184184 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp +++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp @@ -69,7 +69,7 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final * radii for each angular increment. The output scan will output the closest radius that is * still not smaller than range_min. This can be used to vertically compress obstacles into * a single LaserScan. - * @param scan_offset Center position of the LaserScan. A value of 0.0 corresponds to the top row of the image + * @param scan_offset Center position of the LaserScan. A value of 0.0 corresponds to the top row of the image * while 1.0 corresponds to the bottom row of the image. * @param frame_id The output frame_id for the LaserScan. This will probably NOT be the same frame_id as the * depth image. Example: For OpenNI cameras, this should be set to 'camera_depth_frame' while @@ -77,7 +77,7 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final * */ explicit DepthImageToLaserScan( - float scan_time, float range_min, float range_max, int scan_height, float scan_offset, + float scan_time, float range_min, float range_max, int scan_height, float scan_offset, const std::string & frame_id); ~DepthImageToLaserScan(); @@ -158,7 +158,9 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final void convert( const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, const image_geometry::PinholeCameraModel & cam_model, - const sensor_msgs::msg::LaserScan::UniquePtr & scan_msg, const int & scan_height, const float & scan_offset) const + const sensor_msgs::msg::LaserScan::UniquePtr & scan_msg, + const int & scan_height, + const float & scan_offset) const { // Use correct principal point from calibration float center_x = cam_model.cx(); @@ -170,7 +172,8 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final const T * depth_row = reinterpret_cast(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(T); - int offset = static_cast((cam_model.cy()*2*scan_offset) - static_cast(scan_height) / 2.0); + int offset = static_cast((cam_model.cy()*2*scan_offset) - + static_cast(scan_height) / 2.0); depth_row += offset * row_step; // Offset to center of image for (int v = offset; v < offset + scan_height_; v++, depth_row += row_step) { for (uint32_t u = 0; u < depth_msg->width; u++) { // Loop over each pixel in row @@ -205,7 +208,8 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final float range_min_; ///< Stores the current minimum range to use. float range_max_; ///< Stores the current maximum range to use. int scan_height_; ///< Number of pixel rows to use when producing a laserscan from an area. - float scan_offset_; ///< Height ratio of the image where the center of the scan line should be (0.0=top row, 1.0=bottom row). + ///< Height ratio of the image where the center of the scan line should be (0.0=top row, 1.0=bottom row). + float scan_offset_; ///< Output frame_id for each laserscan. This is likely NOT the camera's frame_id. std::string output_frame_id_; }; diff --git a/src/DepthImageToLaserScan.cpp b/src/DepthImageToLaserScan.cpp index 517be2d..4dd1162 100644 --- a/src/DepthImageToLaserScan.cpp +++ b/src/DepthImageToLaserScan.cpp @@ -50,8 +50,8 @@ namespace depthimage_to_laserscan DepthImageToLaserScan::DepthImageToLaserScan( float scan_time, float range_min, float range_max, int scan_height, float scan_offset, const std::string & frame_id) -: scan_time_(scan_time), range_min_(range_min), range_max_(range_max), scan_height_(scan_height), scan_offset_(scan_offset), - output_frame_id_(frame_id) +: scan_time_(scan_time), range_min_(range_min), range_max_(range_max), scan_height_(scan_height), + scan_offset_(scan_offset), output_frame_id_(frame_id) { } @@ -143,13 +143,14 @@ sensor_msgs::msg::LaserScan::UniquePtr DepthImageToLaserScan::convert_msg( scan_msg->range_max = range_max_; // Check scan_height vs image_height - double center_row = cam_model_.cy()*2*scan_offset_; - double bottom_row = center_row - scan_height_/2; - double top_row = center_row + scan_height_/2; - if (bottom_row < 0 || top_row >= depth_msg->height) - { + double center_row = cam_model_.cy() * 2 * scan_offset_; + double bottom_row = center_row - scan_height_ / 2; + double top_row = center_row + scan_height_ / 2; + if (bottom_row < 0 || top_row >= depth_msg->height) { std::stringstream ss; - ss << "scan_height ( " << scan_height_ << " pixels) is too large for the image height ( " << depth_msg->height << " ) with a scan_offset of " << scan_offset_ << " and cy of " << cam_model_.cy() << ". "; + ss << "scan_height ( " << scan_height_ << " pixels) is too large for the image height ( " << + depth_msg->height << " ) with a scan_offset of " << scan_offset_ << " and cy of " << + cam_model_.cy() << ". "; throw std::runtime_error(ss.str()); } diff --git a/test/DepthImageToLaserScanTest.cpp b/test/DepthImageToLaserScanTest.cpp index 8127f3f..5469bee 100644 --- a/test/DepthImageToLaserScanTest.cpp +++ b/test/DepthImageToLaserScanTest.cpp @@ -154,7 +154,8 @@ TEST(ConvertTest, testScanHeight) uint16_t * data = reinterpret_cast(&depth_msg_->data[0]); int row_step = depth_msg_->step / sizeof(uint16_t); - int offset = static_cast((info_msg_->k[5]*2*g_scan_offset) - static_cast(scan_height) / 2.0); + int offset = static_cast((info_msg_->k[5]*2*g_scan_offset) - + static_cast(scan_height) / 2.0); data += offset * row_step; // Offset to center of image for (int v = 0; v < scan_height; v++, data += row_step) { From 83a3203ff66da9395df89d7d1cfffa907baa1914 Mon Sep 17 00:00:00 2001 From: MAHA Maia Date: Wed, 24 Jul 2024 16:52:16 +0200 Subject: [PATCH 4/7] More styleguide/whitespace fixes --- include/depthimage_to_laserscan/DepthImageToLaserScan.hpp | 3 ++- test/DepthImageToLaserScanTest.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp index 6184184..f4d176c 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp +++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp @@ -208,7 +208,8 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final float range_min_; ///< Stores the current minimum range to use. float range_max_; ///< Stores the current maximum range to use. int scan_height_; ///< Number of pixel rows to use when producing a laserscan from an area. - ///< Height ratio of the image where the center of the scan line should be (0.0=top row, 1.0=bottom row). + ///< Height ratio of the image where the center of the scan line should be. + // (0.0=top row, 1.0=bottom row). float scan_offset_; ///< Output frame_id for each laserscan. This is likely NOT the camera's frame_id. std::string output_frame_id_; diff --git a/test/DepthImageToLaserScanTest.cpp b/test/DepthImageToLaserScanTest.cpp index 5469bee..14a3412 100644 --- a/test/DepthImageToLaserScanTest.cpp +++ b/test/DepthImageToLaserScanTest.cpp @@ -154,7 +154,7 @@ TEST(ConvertTest, testScanHeight) uint16_t * data = reinterpret_cast(&depth_msg_->data[0]); int row_step = depth_msg_->step / sizeof(uint16_t); - int offset = static_cast((info_msg_->k[5]*2*g_scan_offset) - + int offset = static_cast((info_msg_->k[5] * 2 * g_scan_offset) - static_cast(scan_height) / 2.0); data += offset * row_step; // Offset to center of image From 31d8c58056741980a2d313d881e5525e17442848 Mon Sep 17 00:00:00 2001 From: MAHA Maia Date: Wed, 24 Jul 2024 17:27:22 +0200 Subject: [PATCH 5/7] Hopefully the last whitespace fix :) --- include/depthimage_to_laserscan/DepthImageToLaserScan.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp index f4d176c..eadeeb1 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp +++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp @@ -70,7 +70,7 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final * still not smaller than range_min. This can be used to vertically compress obstacles into * a single LaserScan. * @param scan_offset Center position of the LaserScan. A value of 0.0 corresponds to the top row of the image - * while 1.0 corresponds to the bottom row of the image. + * while 1.0 corresponds to the bottom row of the image. * @param frame_id The output frame_id for the LaserScan. This will probably NOT be the same frame_id as the * depth image. Example: For OpenNI cameras, this should be set to 'camera_depth_frame' while * the camera uses 'camera_depth_optical_frame'. From 53b4f1480d269492d87543e3f00d3b65e293a11d Mon Sep 17 00:00:00 2001 From: MAHA Maia Date: Wed, 24 Jul 2024 17:43:34 +0200 Subject: [PATCH 6/7] uncrustify was still not happy... --- include/depthimage_to_laserscan/DepthImageToLaserScan.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp index eadeeb1..da6790b 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp +++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp @@ -172,7 +172,7 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final const T * depth_row = reinterpret_cast(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(T); - int offset = static_cast((cam_model.cy()*2*scan_offset) - + int offset = static_cast((cam_model.cy() * 2 * scan_offset) - static_cast(scan_height) / 2.0); depth_row += offset * row_step; // Offset to center of image for (int v = offset; v < offset + scan_height_; v++, depth_row += row_step) { From 11e8fba81816f773ae45b4018ad09529fdafdcfc Mon Sep 17 00:00:00 2001 From: Yannic Bachmann <76436302+YBachmann@users.noreply.github.com> Date: Thu, 25 Jul 2024 08:40:21 +0200 Subject: [PATCH 7/7] Update README.md Co-authored-by: Chris Lalancette --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 525abfc..1feb446 100644 --- a/README.md +++ b/README.md @@ -18,5 +18,5 @@ Parameters * `range_min` (float) - The minimum distance in meters a projected point should be. Points closer than this are discarded. Defaults to 0.45 meters. * `range_max` (float) - The maximum distance in meters a projected point should be. Points further than this are discarded. Defaults to 10.0 meters. * `scan_height` (int) - The number of rows from the depth image to use for the laser projection. Defaults to 1. -* `scan_offset` (float) - The height ratio of the depth image defining on what height the depth data should be taken from (0.0=top row of image, 1.0=bottom row). Defaults to 0.5. +* `scan_offset` (float) - The height ratio of the depth image defining which height the depth data should be taken from (0.0=top row of image, 1.0=bottom row). Defaults to 0.5. * `output_frame` (string) - The frame id to publish in the LaserScan message. Defaults to "camera_depth_frame".