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.

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

Fix time stamp issue for angular filter #186

Merged
merged 5 commits into from
Nov 1, 2023
Merged

Fix time stamp issue for angular filter #186

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
1ee6c25
fix time stamp increment
JosefGst Sep 26, 2023
b04f5a3
add angular filter example
JosefGst Sep 26, 2023
4340e18
do time math with duration
JosefGst Oct 30, 2023
41efa1b
add endline
JosefGst Oct 30, 2023
8d24bf2
change to rclcpp Time
JosefGst Nov 1, 2023
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
18 changes: 18 additions & 0 deletions examples/angular_filter_example.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
from launch import LaunchDescription
JosefGst marked this conversation as resolved.
Show resolved Hide resolved
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
return LaunchDescription([
Node(
package="laser_filters",
executable="scan_to_scan_filter_chain",
parameters=[
PathJoinSubstitution([
get_package_share_directory("laser_filters"),
"examples", "angular_filter_example.yaml",
])],
)
])
JosefGst marked this conversation as resolved.
Show resolved Hide resolved
8 changes: 8 additions & 0 deletions examples/angular_filter_example.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
scan_to_scan_filter_chain:
ros__parameters:
filter1:
name: angle
type: laser_filters/LaserScanAngularBoundsFilter
params:
lower_angle: -1.52
upper_angle: 1.52
JosefGst marked this conversation as resolved.
Show resolved Hide resolved
2 changes: 1 addition & 1 deletion include/laser_filters/angular_bounds_filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
jonbinney marked this conversation as resolved.
Show resolved Hide resolved
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.

}
else{
filtered_scan.ranges[count] = input_scan.ranges[i];
Expand Down
Loading