Merge pull request #186 from JosefGst/time_stamp

Fix time stamp issue for angular filter

examples/angular_filter_example.launch.py

@@ -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",
+                ])],
+        )
+    ])

examples/angular_filter_example.yaml

@@ -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
+

include/laser_filters/angular_bounds_filter.h

@@ -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];