Filtered times are offset from sensor times.
Opened this issue · 6 comments
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:
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
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?
/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.
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?
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 😄
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!