Skip to content

scan_offset parameter for Ros2 #80

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
YBachmann wants to merge 7 commits into
base: ros2
Choose a base branch
from
Open

scan_offset parameter for Ros2 #80

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
277a49f
Added scan_offset parameter.
y-bachmann Jul 18, 2024
eeb2e96
small fixes, more verbose error message
y-bachmann Jul 19, 2024
9510e9c
Adhere to style guidelines
Jul 24, 2024
83a3203
More styleguide/whitespace fixes
Jul 24, 2024
31d8c58
Hopefully the last whitespace fix :)
Jul 24, 2024
53b4f14
uncrustify was still not happy...
Jul 24, 2024
11e8fba
Update README.md
YBachmann Jul 25, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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".
1 change: 1 addition & 0 deletions cfg/param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,4 +4,5 @@ depthimage_to_laserscan:
range_min: 0.45
range_max: 10.0
scan_height: 1
scan_offset: 0.5

@clalancette clalancette Jul 24, 2024

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

My major question here has to do with whether scan_offset makes sense as a float. Since 0.0 means the top, and 1.0 means the bottom, 0.5 means "in the middle". But what if there are an odd number of scan lines? The user would have no ability to choose exactly which line to use, and instead would just have to take whichever one 0.5 rounded down to.

That said, I'm not all too familiar with this stuff. But would it make sense to let the user pick a specific scan line?

@YBachmann YBachmann Jul 24, 2024 •
edited
Loading

Copy link
Copy Markdown
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I understand your concern, however in my opinion a float makes a lot of sense because:

  • even if there are an odd number of lines (which is quite uncommon) you can still pick a specific line. The scan_offset value would is specific_line / image_height.
  • I don't think it ist even necessary to specify an exact row for the vast majority of usecases. Most people will just roughly look at their depthimages and say "I want to use the top half of my images for the laserscan, so let's set scan-height to 0.25".
  • As a user I would much rather have the ability to specify that I always want e.g. the center of the image to be used (scan_offset=0.5), rather than specify a specific row. This way the center will still be used even if I use a different image resolution in the future (e.g. because I scale down my images for performence reasons).

output_frame: "camera_depth_frame"
15 changes: 12 additions & 3 deletions include/depthimage_to_laserscan/DepthImageToLaserScan.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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<typename T>
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();
Expand All @@ -167,7 +172,8 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final
const T * depth_row = reinterpret_cast<const T *>(&depth_msg->data[0]);
int row_step = depth_msg->step / sizeof(T);

int offset = static_cast<int>(cam_model.cy() - static_cast<double>(scan_height) / 2.0);
int offset = static_cast<int>((cam_model.cy() * 2 * scan_offset) -
static_cast<double>(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
Expand Down Expand Up @@ -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_;
};
Expand Down
19 changes: 11 additions & 8 deletions src/DepthImageToLaserScan.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
}

Expand Down Expand Up @@ -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<double>(scan_height_) / 2.0 > cam_model_.cy() ||
static_cast<double>(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());
}

Expand All @@ -156,9 +159,9 @@ sensor_msgs::msg::LaserScan::UniquePtr DepthImageToLaserScan::convert_msg(
scan_msg->ranges.assign(ranges_size, std::numeric_limits<float>::quiet_NaN());

if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) {
convert<uint16_t>(depth_msg, cam_model_, scan_msg, scan_height_);
convert<uint16_t>(depth_msg, cam_model_, scan_msg, scan_height_, scan_offset_);
} else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) {
convert<float>(depth_msg, cam_model_, scan_msg, scan_height_);
convert<float>(depth_msg, cam_model_, scan_msg, scan_height_, scan_offset_);
} else {
std::stringstream ss;
ss << "Depth image has unsupported encoding: " << depth_msg->encoding;
Expand Down
3 changes: 2 additions & 1 deletion src/DepthImageToLaserScanROS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<depthimage_to_laserscan::DepthImageToLaserScan>(
scan_time, range_min, range_max, scan_height, output_frame);
scan_time, range_min, range_max, scan_height, scan_offset, output_frame);
}

DepthImageToLaserScanROS::~DepthImageToLaserScanROS()
Expand Down
18 changes: 10 additions & 8 deletions test/DepthImageToLaserScanTest.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -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
Expand All @@ -145,15 +146,16 @@ 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;

int data_len = depth_msg_->width;
uint16_t * data = reinterpret_cast<uint16_t *>(&depth_msg_->data[0]);
int row_step = depth_msg_->step / sizeof(uint16_t);

int offset = static_cast<int>(info_msg_->k[5] - static_cast<double>(scan_height) / 2.0);
int offset = static_cast<int>((info_msg_->k[5] * 2 * g_scan_offset) -
static_cast<double>(scan_height) / 2.0);
data += offset * row_step; // Offset to center of image

for (int v = 0; v < scan_height; v++, data += row_step) {
Expand Down Expand Up @@ -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_);
Expand Down Expand Up @@ -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_);
Expand Down Expand Up @@ -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_);
Expand Down Expand Up @@ -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_);
Expand Down