Skip to content

Commit

Permalink
Add scan_offset parameter
Browse files Browse the repository at this point in the history
  • Loading branch information
@imaduddinamajid
imaduddinamajid committed Mar 20, 2018
1 parent f135fbd commit 44320c3
Show file tree
Hide file tree
Showing 5 changed files with 24 additions and 4 deletions.
1 change: 1 addition & 0 deletions cfg/Depth.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

This comment has been minimized.

Sign in to view

Copy link
@damienjadeduff

damienjadeduff Mar 20, 2018

Make the whitespace consistent. It is important in big projects with lots of parameters.

This comment has been minimized.

Sign in to view

Copy link
@imaduddinamajid

imaduddinamajid Mar 21, 2018

Author Owner

All right, Dr. Damien.
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)
Expand Down
16 changes: 14 additions & 2 deletions include/depthimage_to_laserscan/DepthImageToLaserScan.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

This comment has been minimized.

Sign in to view

Copy link
@damienjadeduff

damienjadeduff Mar 20, 2018

OK but do write it and have me check it before you send the pull request.

This comment has been minimized.

Sign in to view

Copy link
@imaduddinamajid

imaduddinamajid Mar 21, 2018

Author Owner

Sure, I will.
*
* @param scan_offset I WILL WRITE IT LATER
*
*/
void set_scan_offset(const float scan_offset);

This comment has been minimized.

Sign in to view

Copy link
@damienjadeduff

damienjadeduff Mar 20, 2018

It should be a const int like scan_height. Why a float?

This comment has been minimized.

Sign in to view

Copy link
@imaduddinamajid

imaduddinamajid Mar 21, 2018

Author Owner

I thought it should be a float since the value would be between 0 and 1. 0 for offset at y=0 and 1 for offset at y=IMAGE_HEIGHT. That way will make the offset become a continuous value instead of discrete. Please do correct me if I was wrong.

/**
* Sets the frame_id for the output LaserScan.
Expand Down Expand Up @@ -168,7 +178,7 @@ namespace depthimage_to_laserscan
*/
template<typename T>
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{

This comment has been minimized.

Sign in to view

Copy link
@damienjadeduff

damienjadeduff Mar 20, 2018

Above is a float, here is a double. But I still think it should be an int.

This comment has been minimized.

Sign in to view

Copy link
@imaduddinamajid

imaduddinamajid Mar 21, 2018

Author Owner

This is a bug, it should be a const float&. The reason on why it should be a float has been already explained at the previous command.
// Use correct principal point from calibration
float center_x = cam_model.cx();
float center_y = cam_model.cy();
Expand All @@ -181,7 +191,8 @@ namespace depthimage_to_laserscan
const T* depth_row = reinterpret_cast<const T*>(&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);

This comment has been minimized.

Sign in to view

Copy link
@damienjadeduff

damienjadeduff Mar 20, 2018

If this is debugging code it will be removed before the PR and all commits squashed.

This comment has been minimized.

Sign in to view

Copy link
@imaduddinamajid

imaduddinamajid Mar 21, 2018

Author Owner

Yes, I will surely remove it.
depth_row += offset*row_step; // Offset to center of image

for(int v = offset; v < offset+scan_height_; v++, depth_row += row_step){
Expand Down Expand Up @@ -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.

This comment has been minimized.

Sign in to view

Copy link
@damienjadeduff

damienjadeduff Mar 20, 2018

Should be int?

This comment has been minimized.

Sign in to view

Copy link
@imaduddinamajid

imaduddinamajid Mar 21, 2018

Author Owner

I thought it should be float. The reason is explained in the previous comment.

This comment has been minimized.

Sign in to view

Copy link
@damienjadeduff

damienjadeduff Mar 21, 2018

I see. I think that is OK.
std::string output_frame_id_; ///< Output frame_id for each laserscan. This is likely NOT the camera's frame_id.
};

Expand Down
8 changes: 6 additions & 2 deletions src/DepthImageToLaserScan.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,11 +128,11 @@ sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg(const sensor_msgs::

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_);

This comment has been minimized.

Sign in to view

Copy link
@damienjadeduff

damienjadeduff Mar 20, 2018

Most of this looks great.

This comment has been minimized.

Sign in to view

Copy link
@imaduddinamajid

imaduddinamajid Mar 21, 2018

Author Owner

Thank you, Dr. Damien.
}
else
{
Expand All @@ -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;
}
1 change: 1 addition & 0 deletions src/DepthImageToLaserScanROS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

This comment has been minimized.

Sign in to view

Copy link
@damienjadeduff

damienjadeduff Mar 20, 2018

Sometimes scan_height is above scan_offset sometimes visa versa. Please be consistent.

This comment has been minimized.

Sign in to view

Copy link
@imaduddinamajid

imaduddinamajid Mar 21, 2018

Author Owner

Dr. Damien, I thought you were right. However, when I checked through all of the files I didn't find any single scan_offset written above the scan_height. Did I miss it?

This comment has been minimized.

Sign in to view

Copy link
@damienjadeduff

damienjadeduff Mar 21, 2018

Oh wow I think you are right... again! I need to pay more attention.
dtl_.set_output_frame(config.output_frame_id);
}
2 changes: 2 additions & 0 deletions test/DepthImageToLaserScanTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

This comment has been minimized.

Sign in to view

Copy link
@damienjadeduff

damienjadeduff Mar 20, 2018

OK yes test code is important.
const std::string output_frame = "camera_depth_frame";
dtl_.set_output_frame(output_frame);

Expand Down

5 comments on commit 44320c3

@damienjadeduff
Copy link

Choose a reason for hiding this comment

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

My review is done. Good job. I agree with you using a float is a good idea. With a little bit of a clean up it will be ready to make a PR from but let's use it a bit first. Could you run the unit test by the way?

@imaduddinamajid
Copy link
Owner Author

@imaduddinamajid imaduddinamajid commented on 44320c3 Mar 21, 2018 •
edited
Loading

Choose a reason for hiding this comment

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

I did the test using the test launch file. Am I correct?

imad@imad-Lenovo-ideapad-310-15IKB:~/robot_ws/src/depthimage_to_laserscan$ roslaunch depthimage_to_laserscan depthimage_to_laserscan.test 
... logging to /home/imad/.ros/log/c8187fb0-2cc4-11e8-99ee-701ce768b8f7/roslaunch-imad-Lenovo-ideapad-310-15IKB-3659.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/imad/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://imad-Lenovo-ideapad-310-15IKB:43086/

SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.7

NODES
  /
    depthimage_to_laserscan (depthimage_to_laserscan/depthimage_to_laserscan)

auto-starting new master
process[master]: started with pid [3672]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to c8187fb0-2cc4-11e8-99ee-701ce768b8f7
process[rosout-1]: started with pid [3685]
started core service [/rosout]
process[depthimage_to_laserscan-2]: started with pid [3688]

@damienjadeduff
Copy link

Choose a reason for hiding this comment

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

Looks good - TBH I don't know much about gtest or how it is used in ROS. I think go with that. By the way, there are PRs waiting since 2014! https://github.com/ros-perception/depthimage_to_laserscan/pulls

Also I think our method will help this guy if he does some math to figure out the exact offset necessary: ros-perception#16

@imaduddinamajid
Copy link
Owner Author

Choose a reason for hiding this comment

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

Yes, I saw that issue before I started working yesterday. It made me feel more motivated since I might help him with this. :)

I have opened a PR from this branch to my indigo-devel. After you review and approve it, I will open a PR to their indigo-devel. I hope they are still active working on it.

@damienjadeduff
Copy link

Choose a reason for hiding this comment

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

Yes refer to the issue when you open the final PR. I will look for your PR now.

Please sign in to comment.