Robot_Localization can't subtract times with different time sources
I am trying to use the Robot_Localization with an IMU and Odometry-Topic and face the following error:
[ekf_node-1] X acceleration is being measured from IMU; X velocity control input is disabled
[ekf_node-1] terminate called after throwing an instance of std::runtime_error
[ekf_node-1] what(): cant subtract times with different time sources [1 != 2]
at first I thought that this might be because my stamps are not perfectly synced, but after syncing them the problem still persisted.
In a past question it is being stated that working with .get_clock() should do the trick, but this is not the case here. Any help is appreciated.
Edit: I basically did a minimal publisher example that also won't work for R_L. I am on the foxy-devel branch btw. I wanted to try the ros2 branch but I can't get it to build at all.
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
class MinimalPublisher(Node):
def __init__(self):
super().__init__('minimal_publisher')
self.pub_imu = self.create_publisher(Imu, 'imu', 10)
self.pub_odo = self.create_publisher(Odometry, 'odo', 10)
timer_period = 0.1 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
self.imu_msg = Imu()
self.odo_msg = Odometry()
self.imu_msg.header.frame_id = 'base_link'
self.odo_msg.header.frame_id = 'odom'
self.odo_msg.child_frame_id = 'base_link'
def timer_callback(self):
synced_time = self.get_clock().now().to_msg()
self.imu_msg.header.stamp = synced_time
self.odo_msg.header.stamp = synced_time
self.pub_imu.publish(self.imu_msg)
self.pub_odo.publish(self.odo_msg)
self.get_logger().info('Published IMU and Odometry')
def main(args=None):
rclpy.init(args=args)
minimal_publisher = MinimalPublisher()
rclpy.spin(minimal_publisher)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
minimal_publisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
The config file is:
### ekf config file ###
ekf_filter_node:
ros__parameters:
frequency: 30.0
sensor_timeout: 0.1
two_d_mode: false
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug.txt
permit_corrected_publication: false
publish_acceleration: false
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
odom0: /odo
odom0_config: [true, true, false,
false, false, false,
false, false, false,
false, false, true,
false, false, false]
odom0_queue_size: 20
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
odom0_pose_rejection_threshold: 5.0
odom0_twist_rejection_threshold: 1.0
imu0: /imu
imu0_config: [false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 20
imu0_pose_rejection_threshold: 0.8
imu0_twist_rejection_threshold: 0.8
imu0_linear_acceleration_rejection_threshold: 0.8
imu0_remove_gravitational_acceleration: true
use_control: true
stamped_control: false
control_timeout: 0.2
control_config: [true, false, false, false, false, true]
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
might be related to https://github.com/cra-ros-pkg/robot_...
so this is probably where the exception gets thrown: https://github.com/ros2/rclcpp/blob/1... Which means one time source is
RCL_ROS_TIME=1
and the other isRCL_SYSTEM_TIME=2
: https://github.com/ros2/rcl/blob/f961... Even if you get the timestamp using the same function, it probably doesn't guarantee that the underlying clock is the same.So I tried everything that came up to my mind, even handling the stamp over with
which should mean that I have a literal copy of the clock, if I understood this right. I am still though facing this problem.
if you could provide a minimal working example (i.e. code that someone can build to reproduce the issue, either in your question itself or a link to a GitHub repo or gist) it would really help.
I just provided a minimal example in the question above as a minimal publisher that writes a stamped imu and odo message and it throws the very same error still.
thanks. I was able to reproduce it on Foxy and Rolling. What's interesting is that it seems to work fine if I don't use your config file (and use the default one instead). Have you looked into that?
ah nevermind, I assume it's because it's not actually doing anything because your minimal_publisher doesn't publish on the same topics as the ones in the default config file.
if you modify this line in
robot_localization
and changelatest_control_time_(0)
tolatest_control_time_(0, 0, RCL_ROS_TIME)
, does it work for you? https://github.com/cra-ros-pkg/robot_...