-
Notifications
You must be signed in to change notification settings - Fork 204
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
Comments
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? |
|
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:
Could you try modifying your config to remove the AngularBoundsFilter and see if the problem still occurs? |
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 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! |
Should be fixed by #186 , so i'm closing this. Feel free to reopen if it is still a problem. |
The filtered times are offset and stuck in a 1 second interval loop.
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:
[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
The text was updated successfully, but these errors were encountered: