(self, stereo_pair_msg)
| 218 | if self.imu_last_received_timestamp is not None and current_timestamp - self.imu_last_received_timestamp > 0.1: |
| 219 | delta_timestamp = current_timestamp - self.imu_last_received_timestamp |
| 220 | self.get_logger().warning(f"IMU timestamp jump {delta_timestamp} s is too large, it means the IMU is not working properly") |
| 221 | self.imu_last_received_timestamp = current_timestamp |
| 222 | accel_data = np.array([[imu_msg.linear_acceleration.x], [imu_msg.linear_acceleration.y], [imu_msg.linear_acceleration.z]]) |
| 223 | gyro_data = np.array([[imu_msg.angular_velocity.x], [imu_msg.angular_velocity.y], [imu_msg.angular_velocity.z]]) |
| 224 | with self.imu_measurements_lock: |
| 225 | self.imu_measurements.append([current_timestamp, accel_data.flatten(), gyro_data.flatten()]) |
| 226 | |
| 227 | def _aligned_imu_callback(self, imu_msg): |
| 228 | self._process_imu_msg(imu_msg) |
| 229 | |
| 230 | def _aligned_stereo_callback(self, stereo_pair_msg): |
| 231 | image_timestamp = stamp2second(stereo_pair_msg.header.stamp) |
| 232 | if image_timestamp - self.last_processed_timestamp < 0.1333: |
| 233 | return |
nothing calls this directly
no test coverage detected