Skip to content
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.

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

Fix time stamp issue for angular filter #186

Merged
merged 5 commits into from
Nov 1, 2023

Conversation

JosefGst
Copy link
Contributor

@jonbinney
Copy link
Contributor

@JosefGst thank you for doing this! Reviewing now.

examples/angular_filter_example.launch.py Outdated Show resolved Hide resolved
examples/angular_filter_example.yaml Outdated Show resolved Hide resolved
include/laser_filters/angular_bounds_filter.h Show resolved Hide resolved
@@ -79,7 +79,7 @@ namespace laser_filters
if(start_angle < lower_angle_){
start_angle += input_scan.angle_increment;
current_angle += input_scan.angle_increment;
start_time.set__sec(start_time.sec + input_scan.time_increment);
start_time.set__nanosec(start_time.nanosec + (input_scan.time_increment * 10e9)); // convert time increment to nanoseconds
Copy link
Contributor

Choose a reason for hiding this comment

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

I think there are still a couple of issues:

  • there are 1e9 nanoseconds in a second, so you should multiply by 1e9, not 10e9, right?
  • set_nanosec only updates the number of nanoseconds. The Time message type only allows nanosec to be between 0 and 1e9, so if the scan starts near the end of a second and we remove a bunch of beams from the beginning, then we could end up with an invalid number of nanoseconds. Instead we would want to add nanoseconds / 1e9 to start_time.sec and nanoseconds % 1e9 to start_time.nanosec

Time math is always tricky to get just right, so if there's a way to do this using utility functions that handle the details for us, that would be ideal. The duration type should handle converting a floating point number of seconds to a proper duration for us in its from_seconds method: https://docs.ros2.org/foxy/api/rclcpp/classrclcpp_1_1Duration.html#a39ce5aa6d0bb190d7d59857aa7a9af1a

How about:

start_time += Duration::from_seconds(input_scan.time_increment);

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Thank you for your review and pointing out the remaining issues.
You're right, it should be 1e9.
I will try to use the time duration as you recommend.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Sorry, was on holliday and didn't had the hardware for testing. But the start time is now calculated with the Duration object 😄
I tried with the rplidar which publishes:

angle_min: -3.1241390705108643
angle_max: 3.1415927410125732

And the settings for the filter:

scan_to_scan_filter_chain:
  ros__parameters:
    filter1:
      name: angle
      type: laser_filters/LaserScanAngularBoundsFilter
      params:
        lower_angle: -1.52
        upper_angle: 1.52  

2023-10-30_17-32
In the second case I tried to cut off more data at the beginning. The start time of the filtered data (red) is slightly more delayed.

scan_to_scan_filter_chain:
  ros__parameters:
    filter1:
      name: angle
      type: laser_filters/LaserScanAngularBoundsFilter
      params:
        lower_angle: 1.5
        upper_angle: 1.52  

2023-10-30_17-33
It looks correct to me. But please proof it.

examples/angular_filter_example.launch.py Show resolved Hide resolved
@@ -99,6 +100,8 @@ namespace laser_filters

}
}
start_time.sec += d.seconds();
start_time.nanosec += d.nanoseconds();
Copy link
Contributor

Choose a reason for hiding this comment

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

Does start_time += d not work? I think that handling seconds and nanoseconds automatically for you is one of the nice part about using Duration objects.

Copy link
Contributor Author

@JosefGst JosefGst Oct 31, 2023

Choose a reason for hiding this comment

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

Unfortunately I couldn't make start_time += d work.
Let me know if you have any other advice to improve. I'm very thankfull for your guidance :)

Copy link
Contributor

Choose a reason for hiding this comment

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

Ah, I see the problem. ROS2 has different types for serializing timestamps than for computing with time. Try changing the earlier line from:

builtin_interfaces::msg::Time start_time = input_scan.header.stamp;

to

rclcpp::Time start_time = input_scan.header.stamp;

Then start_time will be an rclcpp:::Time type, and start_time += d; should work.

Thanks for working on this!

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Great, thank you for your advice. Now the code can be formated back to very similar as the original was.

@jonbinney
Copy link
Contributor

Looks great! I'm going to let the CI tests run one more time and then I'll merge. Thanks @JosefGst !

@jonbinney jonbinney merged commit 905f5e9 into ros-perception:ros2 Nov 1, 2023
5 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants