diff --git a/README.md b/README.md index a3119af..1feb446 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 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". 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..da6790b 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 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'. * */ 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,16 @@ 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=top row, 1.0=bottom 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 +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() - 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 +208,9 @@ 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). + 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 4c589cd..4dd1162 100644 --- a/src/DepthImageToLaserScan.cpp +++ b/src/DepthImageToLaserScan.cpp @@ -49,9 +49,9 @@ namespace depthimage_to_laserscan DepthImageToLaserScan::DepthImageToLaserScan( float scan_time, float range_min, float range_max, - int scan_height, const std::string & frame_id) + 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), - output_frame_id_(frame_id) + scan_offset_(scan_offset), output_frame_id_(frame_id) { } @@ -143,11 +143,14 @@ 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."; + 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()); } @@ -156,9 +159,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..980f344 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); + float 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..14a3412 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,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] - 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 +199,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 +228,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 +256,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 +291,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_);