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
+29 −2
Merged

Fix time stamp issue for angular filter #186

Show file tree
Hide file tree
Changes from 4 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",
])],
)
])
9 changes: 9 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,9 @@
scan_to_scan_filter_chain:
ros__parameters:
filter1:
name: angle
type: laser_filters/LaserScanAngularBoundsFilter
params:
lower_angle: -1.52
upper_angle: 1.52

5 changes: 4 additions & 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 @@ -72,14 +72,15 @@ namespace laser_filters
double start_angle = input_scan.angle_min;
double current_angle = input_scan.angle_min;
builtin_interfaces::msg::Time start_time = input_scan.header.stamp;
rclcpp::Duration d = rclcpp::Duration(0, 0);
unsigned int count = 0;
//loop through the scan and truncate the beginning and the end of the scan as necessary
for(unsigned int i = 0; i < input_scan.ranges.size(); ++i){
//wait until we get to our desired starting angle
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
d = d + rclcpp::Duration::from_seconds(input_scan.time_increment);
}
else{
filtered_scan.ranges[count] = input_scan.ranges[i];
Expand All @@ -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 •
edited
Loading

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.


//make sure to set all the needed fields on the filtered scan
filtered_scan.header.frame_id = input_scan.header.frame_id;
Expand Down