ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

Robot_Localization can't subtract times with different time sources

asked 2021-06-15 09:16:18 -0500

KNTRL gravatar image

updated 2021-06-23 07:08:40 -0500

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]
edit retag flag offensive close merge delete

Comments

1
christophebedard gravatar image christophebedard  ( 2021-06-15 10:14:47 -0500 )edit
1

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 is RCL_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.

christophebedard gravatar image christophebedard  ( 2021-06-15 10:26:10 -0500 )edit

So I tried everything that came up to my mind, even handling the stamp over with

msg1.header.stamp = msg2.header.stamp

which should mean that I have a literal copy of the clock, if I understood this right. I am still though facing this problem.

KNTRL gravatar image KNTRL  ( 2021-06-16 11:15:37 -0500 )edit
1

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.

christophebedard gravatar image christophebedard  ( 2021-06-16 12:24:22 -0500 )edit

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.

KNTRL gravatar image KNTRL  ( 2021-06-21 02:43:53 -0500 )edit
1

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?

christophebedard gravatar image christophebedard  ( 2021-06-23 12:37:23 -0500 )edit
1

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.

christophebedard gravatar image christophebedard  ( 2021-06-23 12:41:06 -0500 )edit
1

if you modify this line in robot_localization and change latest_control_time_(0) to latest_control_time_(0, 0, RCL_ROS_TIME), does it work for you? https://github.com/cra-ros-pkg/robot_...

christophebedard gravatar image christophebedard  ( 2021-06-23 14:56:22 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
3

answered 2021-07-06 09:24:31 -0500

Posting the fix from my comments as an answer:

I described the issue and the fix here: https://github.com/cra-ros-pkg/robot_.... In short, this should fix it:

diff --git a/src/filter_base.cpp b/src/filter_base.cpp
index 5b75d0c..005a2c1 100644
--- a/src/filter_base.cpp
+++ b/src/filter_base.cpp
@@ -47,7 +47,7 @@ namespace robot_localization
 FilterBase::FilterBase()
 : initialized_(false), use_control_(false),
   use_dynamic_process_noise_covariance_(false), control_timeout_(0, 0u),
-  last_measurement_time_(0, 0, RCL_ROS_TIME), latest_control_time_(0),
+  last_measurement_time_(0, 0, RCL_ROS_TIME), latest_control_time_(0, 0, RCL_ROS_TIME),
   sensor_timeout_(0, 0u), debug_stream_(nullptr),
   acceleration_gains_(TWIST_SIZE, 0.0),
   acceleration_limits_(TWIST_SIZE, 0.0),

I opened a PR here: https://github.com/cra-ros-pkg/robot_...

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2021-06-15 09:16:18 -0500

Seen: 1,930 times

Last updated: Jul 06 '21