0
$\begingroup$

Rosanswers logo

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]

Originally posted by KNTRL on ROS Answers with karma: 25 on 2021-06-15

Post score: 1


Original comments

Comment by christophebedard on 2021-06-15:
might be related to https://github.com/cra-ros-pkg/robot_localization/issues/622

Comment by christophebedard on 2021-06-15:
so this is probably where the exception gets thrown: https://github.com/ros2/rclcpp/blob/1fff1b7cbafcbc698b5dda5b8e4a35a27c6748c4/rclcpp/src/rclcpp/time.cpp#L187-L191 Which means one time source is RCL_ROS_TIME=1 and the other is RCL_SYSTEM_TIME=2: https://github.com/ros2/rcl/blob/f96196512edd48f89eefdb6d590a7ef879053971/rcl/include/rcl/time.h#L59-L65 Even if you get the timestamp using the same function, it probably doesn't guarantee that the underlying clock is the same.

Comment by KNTRL on 2021-06-16:
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.

Comment by christophebedard on 2021-06-16:
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.

Comment by KNTRL on 2021-06-21:
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.

Comment by christophebedard on 2021-06-23:
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?

Comment by christophebedard on 2021-06-23:
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.

Comment by christophebedard on 2021-06-23:
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_localization/blob/9fba3e8e8b30b653f4a00b0a957e4275168fdf9e/src/filter_base.cpp#L50

Comment by christophebedard on 2021-06-23:
@KNTRL I used your example to prepare a full example along with steps to reproduce the issue: https://github.com/christophebedard/robot_localization-issue-622. And I posted under the robot_localization issue and explained what I did: https://github.com/cra-ros-pkg/robot_localization/issues/622#issuecomment-867150029. Hopefully someone can say whether this "fix" makes sense.

Comment by KNTRL on 2021-06-24:
Yes it worked, thank you very much for the debugging! May I ask how you are debugging in ROS2 ? I am still having a hard time to understand how this works. For now I am working with loggers to understand what is going on while troubleshooting problems.

Comment by christophebedard on 2021-06-24:
honestly I haven't tried using VSCode's debugging features in a long time. I started by building with --mixin debug and ran the ekf_node through GDB (launch prints the exact CLI command+arguments it used for a given process when it crashes) to get the backtrace of when it terminates/throws. Then I simply added prints/std::cout here and there (or I could use loggers) and looked around the code. In general, debugging when you use complex launch files is definitely a bit of a hassle (see https://github.com/ros2/launch_ros/issues/165).

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Posting the fix from my comments as an answer:

I described the issue and the fix here: https://github.com/cra-ros-pkg/robot_localization/issues/622#issuecomment-867150029. 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_localization/pull/679


Originally posted by christophebedard with karma: 641 on 2021-07-06

This answer was ACCEPTED on the original site

Post score: 3

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.