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.

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

Filtered times are offset from sensor times. #184

Open
HansLarsen opened this issue Aug 9, 2023 · 12 comments
Open

Filtered times are offset from sensor times. #184

HansLarsen opened this issue Aug 9, 2023 · 12 comments

Comments

@HansLarsen
Copy link

HansLarsen commented Aug 9, 2023

The filtered times are offset and stuck in a 1 second interval loop.
TIMESTAMPS
Heres plot juggler, the red line is the input laser scans and the orange line is the output from the filter.

I'm on commit 65cbefd
The distribution is ROS2 humble, everything build from the latest sources, according to the build from source guide, on Ubuntu 18.04.

The delay is likewise present if you print them in the callback:

Callback
  void callback(const std::shared_ptr<const sensor_msgs::msg::LaserScan> & msg_in)
  {
    RCLCPP_INFO(
      nh_->get_logger(), "%d, %d", msg_in->header.stamp.sec, msg_in->header.stamp.nanosec);
    // Run the filter chain
    if (filter_chain_.update(*msg_in, msg_)) {
      //only publish result if filter succeeded
      output_pub_->publish(msg_);
      RCLCPP_INFO(nh_->get_logger(), "%d, %d", msg_.header.stamp.sec, msg_.header.stamp.nanosec);
    }
  }

[scan_to_scan_filter_chain-1] [INFO] [1691585756.502811379] [sick_tim_5xx.scan_to_scan_filter_chain]: 1691579413, 906227588 [scan_to_scan_filter_chain-1] [INFO] [1691585756.504288207] [sick_tim_5xx.scan_to_scan_filter_chain]: 1691579392, 906227588 [scan_to_scan_filter_chain-1] [INFO] [1691585757.162848118] [sick_tim_5xx.scan_to_scan_filter_chain]: 1691579413, 972615242 [scan_to_scan_filter_chain-1] [INFO] [1691585757.164226961] [sick_tim_5xx.scan_to_scan_filter_chain]: 1691579392, 972615242 [scan_to_scan_filter_chain-1] [INFO] [1691585757.827929301] [sick_tim_5xx.scan_to_scan_filter_chain]: 1691579414, 39208173 [scan_to_scan_filter_chain-1] [INFO] [1691585757.829719778] [sick_tim_5xx.scan_to_scan_filter_chain]: 1691579392, 39208173 [scan_to_scan_filter_chain-1] [INFO] [1691585758.502889853] [sick_tim_5xx.scan_to_scan_filter_chain]: 1691579414, 105945587 [scan_to_scan_filter_chain-1] [INFO] [1691585758.504508106] [sick_tim_5xx.scan_to_scan_filter_chain]: 1691579392, 105945587 [scan_to_scan_filter_chain-1] [INFO] [1691585759.167960141] [sick_tim_5xx.scan_to_scan_filter_chain]: 1691579414, 172862529 [scan_to_scan_filter_chain-1] [INFO] [1691585759.169524268] [sick_tim_5xx.scan_to_scan_filter_chain]: 1691579392, 172862529 [scan_to_scan_filter_chain-1] [INFO] [1691585759.832868401] [sick_tim_5xx.scan_to_scan_filter_chain]: 1691579414, 239361286 [scan_to_scan_filter_chain-1] [INFO] [1691585759.834581287] [sick_tim_5xx.scan_to_scan_filter_chain]: 1691579392, 239361286

@jonbinney
Copy link
Contributor

Wow, that is odd!

ScanToScanFilterChain just calls FilterChain::update(), which just calls each filter in order. Each filter in the chain can do whatever they want to the timestamp; but typically should be setting the output timestamp to the input timestamp. For example, BoxFilter starts by setting the output message to equal the input message, which includes setting the timestamp.

What filters are you running? Could you post your config file for the filter chain?

@HansLarsen
Copy link
Author

/sick_tim_5xx/scan_to_scan_filter_chain: ros__parameters: filter1: name: angle type: laser_filters/LaserScanAngularBoundsFilter params: lower_angle: -2.0943951 #-120deg upper_angle: 2.35619 #135deg filter2: name: shadows type: laser_filters/ScanShadowsFilter params: min_angle: 00.0 max_angle: 180.0 neighbors: 1 window: 3 filter3: name: box type: laser_filters/LaserScanBoxFilter params: box_frame: lidar_base min_x: -0.2 max_x: 0.2 min_y: -0.2 max_y: 0.2 min_z: -2.0 max_z: 2.0 filter4: name: box_inside type: laser_filters/LaserScanBoxFilter params: box_frame: base_link min_x: -2.0 max_x: 2.0 min_y: -2.0 max_y: 2.0 min_z: -2.0 max_z: 2.0 invert: true
The frames base_link and lidar_base are both static, so its a bit confusing that they would alter the timestamp.

@jonbinney
Copy link
Contributor

Looks like there are three kinds of filter in the chain; here is where each of them update the timestamp in their output scan:

ScanShadowsFilter and BoxFilter look fine; as far as I can tell they just copy the input stamp to the output. AngularBoundsFilter modifies the timestamp since it removes some rays at the beginning of the scan:

start_time.set__sec(start_time.sec + input_scan.time_increment);

Could you try modifying your config to remove the AngularBoundsFilter and see if the problem still occurs?

@JosefGst
Copy link
Contributor

JosefGst commented Sep 15, 2023

I experienced the same time stamp issue with the angular filter. Is there any specific reason why the time stamp will be updated in the angular filter but not for the others? For me, I copied the input time stamp to the output time stamp, and it made the angular filter work again. Would be happy to make a pull request if wanted 😄
image

@jonbinney
Copy link
Contributor

The reason for changing the timestamp is that not all readings in a LaserScan message happen at the same time. The laser (typically) spins and takes readings one after another. So the first reading in a scan happens at the time indicated by the timestamp, but the nth reading in a message happens at msg.header.stamp + (n -1) * msg.time_increment. If the AngularBoundsFilter removes the first m readings, then it needs to adjust the timestamp to new_msg.header.stamp = old_msg.header.stamp + m * msg.time_increment)

But this must not be implemented correctly in the code. If you can find the bug and fix it, then go ahead and make a PR and I'll review it. Thanks in advance!

JosefGst referenced this issue Sep 26, 2023

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
@jonbinney
Copy link
Contributor

Should be fixed by #186 , so i'm closing this. Feel free to reopen if it is still a problem.

@alireza-moayyedi
Copy link

Hello, indeed I have the same issue however hopefully it is fixed in #186 . The only problem is that the latest release (2.0.7) is for July 31, 2023 whereas the fix was added on Nov 1, 2023. I was wondering if there is any plan on making a new release for ROS2 (Jazzy or Iron)?

@jonbinney
Copy link
Contributor

@alireza-moayyedi could you try compiling from source and verify that your problem is fixed now? If it is, I'll do the release soon.

@alireza-moayyedi
Copy link

@jonbinney Hello, I tried compiling from source and the problem seems to be fixed now. Below you can see the comparisons. The terminals are showing from left to right:

raw sensor topic echo - raw sensor topic delay - filtered data topic echo - filtered data topic delay

With the current apt release (2.0.7):
apt

From source:
source

As you can see the delay does not make sense in the apt release while it seems logical from source.

@jonbinney
Copy link
Contributor

Thank you! I'll try to find some time this weekend to do the releases into Jazzy and Iron.

@alireza-moayyedi
Copy link

@jonbinney thanks for taking the time! Much appreciated.

@jonbinney
Copy link
Contributor

Did releases on iron, jazzy, and rolling. It'll take some time to build on the farm and get synced.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

4 participants