Skip to content

Commit

Permalink
Merge pull request #186 from JosefGst/time_stamp
Browse files Browse the repository at this point in the history
Fix time stamp issue for angular filter
  • Loading branch information
jonbinney authored Nov 1, 2023
2 parents 65cbefd + 8d24bf2 commit 905f5e9
Show file tree
Hide file tree
Showing 3 changed files with 29 additions and 2 deletions.
18 changes: 18 additions & 0 deletions examples/angular_filter_example.launch.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
from launch import LaunchDescription
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
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

4 changes: 2 additions & 2 deletions include/laser_filters/angular_bounds_filter.h
Original file line number Diff line number Diff line change
Expand Up @@ -71,15 +71,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::Time start_time = input_scan.header.stamp;
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);
start_time += rclcpp::Duration::from_seconds(input_scan.time_increment);
}
else{
filtered_scan.ranges[count] = input_scan.ranges[i];
Expand Down

0 comments on commit 905f5e9

Please sign in to comment.