Fix time stamp issue for angular filter

[JosefGst]

• Changed the time increment to nanoseconds.
  This also solved the time stamp issue Filtered times are offset from sensor times. #184 for the angular filter for me.
• Added angular filter example

[jonbinney]

@JosefGst thank you for doing this! Reviewing now.

[jonbinney]

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

[JosefGst]

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.

[JosefGst]

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  

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  

It looks correct to me. But please proof it.

[jonbinney]

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.

[JosefGst]

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 :)

[jonbinney]

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!

[JosefGst]

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

[jonbinney]

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