From 44320c3e92fa71284586cc79054d5683b0783791 Mon Sep 17 00:00:00 2001 From: Imaduddinamajid Date: Tue, 20 Mar 2018 15:40:07 +0300 Subject: [PATCH] Add scan_offset parameter --- cfg/Depth.cfg | 1 + .../DepthImageToLaserScan.h | 16 ++++++++++++++-- src/DepthImageToLaserScan.cpp | 8 ++++++-- src/DepthImageToLaserScanROS.cpp | 1 + test/DepthImageToLaserScanTest.cpp | 2 ++ 5 files changed, 24 insertions(+), 4 deletions(-) diff --git a/cfg/Depth.cfg b/cfg/Depth.cfg index 127c704..77af431 100755 --- a/cfg/Depth.cfg +++ b/cfg/Depth.cfg @@ -9,6 +9,7 @@ from math import pi gen = ParameterGenerator() # Name Type Reconfiguration level Description Default Min Max gen.add("scan_height", int_t, 0, "Height of the laser band (in pixels).", 1, 1, 500) +gen.add("scan_offset", double_t, 0, "Row set as the center of laserscan.", 0.500, 0.0, 1.0) gen.add("scan_time", double_t, 0, "Time for the entire scan sweep.", 0.033, 0.0, 1.0) gen.add("range_min", double_t, 0, "Minimum reported range (in meters).", 0.45, 0.0, 10.0) gen.add("range_max", double_t, 0, "Maximum reported range (in meters).", 10.0, 0.0, 10.0) diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.h b/include/depthimage_to_laserscan/DepthImageToLaserScan.h index e2921f7..9e18f3e 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScan.h +++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.h @@ -101,6 +101,16 @@ namespace depthimage_to_laserscan * */ void set_scan_height(const int scan_height); + + /** + * Sets the row to use as a center in the output LaserScan. + * + * SKIP IT FIRST, WILL WRITE IT LATER + * + * @param scan_offset I WILL WRITE IT LATER + * + */ + void set_scan_offset(const float scan_offset); /** * Sets the frame_id for the output LaserScan. @@ -168,7 +178,7 @@ namespace depthimage_to_laserscan */ template void convert(const sensor_msgs::ImageConstPtr& depth_msg, const image_geometry::PinholeCameraModel& cam_model, - const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height) const{ + const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height, const double& scan_offset) const{ // Use correct principal point from calibration float center_x = cam_model.cx(); float center_y = cam_model.cy(); @@ -181,7 +191,8 @@ namespace depthimage_to_laserscan const T* depth_row = reinterpret_cast(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(T); - int offset = (int)(cam_model.cy()-scan_height/2); + int offset = (int)((cam_model.cy()*2*scan_offset)-scan_height/2); + fprintf(stderr, "offset, cam_model.cy(), scan offset, scan height: %d, %f, %f, %d\n", offset, cam_model.cy(), scan_offset, scan_height); depth_row += offset*row_step; // Offset to center of image for(int v = offset; v < offset+scan_height_; v++, depth_row += row_step){ @@ -216,6 +227,7 @@ namespace depthimage_to_laserscan 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_; ///< Number of row being set as center of a laserscan. std::string output_frame_id_; ///< Output frame_id for each laserscan. This is likely NOT the camera's frame_id. }; diff --git a/src/DepthImageToLaserScan.cpp b/src/DepthImageToLaserScan.cpp index 6ad0487..5e5ce1e 100644 --- a/src/DepthImageToLaserScan.cpp +++ b/src/DepthImageToLaserScan.cpp @@ -128,11 +128,11 @@ sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg(const sensor_msgs:: 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 { @@ -157,6 +157,10 @@ void DepthImageToLaserScan::set_scan_height(const int scan_height){ scan_height_ = scan_height; } +void DepthImageToLaserScan::set_scan_offset(const float scan_offset){ + scan_offset_ = scan_offset; +} + void DepthImageToLaserScan::set_output_frame(const std::string output_frame_id){ output_frame_id_ = output_frame_id; } diff --git a/src/DepthImageToLaserScanROS.cpp b/src/DepthImageToLaserScanROS.cpp index fe915c7..a38ccc6 100644 --- a/src/DepthImageToLaserScanROS.cpp +++ b/src/DepthImageToLaserScanROS.cpp @@ -87,5 +87,6 @@ void DepthImageToLaserScanROS::reconfigureCb(depthimage_to_laserscan::DepthConfi dtl_.set_scan_time(config.scan_time); dtl_.set_range_limits(config.range_min, config.range_max); dtl_.set_scan_height(config.scan_height); + dtl_.set_scan_offset(config.scan_offset); dtl_.set_output_frame(config.output_frame_id); } diff --git a/test/DepthImageToLaserScanTest.cpp b/test/DepthImageToLaserScanTest.cpp index 1b4acc2..67c167b 100644 --- a/test/DepthImageToLaserScanTest.cpp +++ b/test/DepthImageToLaserScanTest.cpp @@ -61,6 +61,8 @@ TEST(ConvertTest, setupLibrary) dtl_.set_range_limits(range_min, range_max); const int scan_height = 1; dtl_.set_scan_height(scan_height); + const int scan_offset = 0.5; + dtl_.set_scan_offset(scan_offset); const std::string output_frame = "camera_depth_frame"; dtl_.set_output_frame(output_frame);