Ros rolling different time sources

AppliedRobotics commented 3 years ago

Bug report

Required Info:

Operating System: Ubuntu 20.04
Installation type: from source

Steps to reproduce issue

Install ros2 rolling and install robot localisation foxy branch

Actual behavior

I launch ekf node and a few times everything was okay, but than this issue appears:

[INFO] [launch]: All log files can be found below /home/nuc/.ros/log/2021-02-16-19-15-43-995821-nuc-49850 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [ekf_node-1]: process started with pid [49852] [ekf_node-1] terminate called after throwing an instance of 'std::runtime_error' [ekf_node-1] what(): can't subtract times with different time sources [1 != 2] [ERROR] [ekf_node-1]: process has died [pid 49852, exit code -6, cmd '/home/nuc/BF_ws/install/robot_localization/lib/robot_localization/ekf_node --ros-args -r __node:=ekf_filter_node --params-file /home/nuc/BF_ws/install/robot_localization/share/robot_localization/params/ekf.yaml'].

ayrton04 commented 3 years ago

Can you try this branch and let me know if it's better?

https://github.com/cra-ros-pkg/robot_localization/pull/613

AppliedRobotics commented 3 years ago

I tried, still same error

ridhwankamil commented 3 years ago

I am having the same issue with @AppliedRobotics

ayrton04 commented 3 years ago

Can you provide some reproduction steps? The report just says "I launch ekf node and a few times." What launch file did you run?

ridhwankamil commented 3 years ago

I will try to give detail reproduction steps from my side.

My objective is to test using the robot_localization package with turtlebot3_simulations package

Required Info:

Operating System: Ubuntu 20.04
ROS Installation type: from binary
robot_localization installation type: from source (foxy-devel branch)

Steps to reproduce issue: I modified the turtlebot3 gazebo model.sdf to disable broadcasting TF odom->base_footprint

  <ros>
      <!--<namespace>/demo</namespace>-->
      <remapping>odom:=raw_odom</remapping>
  </ros>

  <update_rate>30</update_rate>

  <!-- wheels -->
  <left_joint>wheel_left_joint</left_joint>
  <right_joint>wheel_right_joint</right_joint>

  <!-- kinematics -->
  <wheel_separation>0.287</wheel_separation>
  <wheel_diameter>0.066</wheel_diameter>

  <!-- limits -->
  <max_wheel_torque>20</max_wheel_torque>
  <max_wheel_acceleration>1.0</max_wheel_acceleration>

  <command_topic>cmd_vel</command_topic>

  <!-- output -->
  <publish_odom>true</publish_odom>
  <publish_odom_tf>false</publish_odom_tf>
  <publish_wheel_tf>false</publish_wheel_tf>

  <odometry_topic>odom</odometry_topic>
  <odometry_frame>odom</odometry_frame>
  <robot_base_frame>base_footprint</robot_base_frame>

Launch with use_sim_time = LaunchConfiguration('use_sim_time', default='true') ros2 launch turtlebot3_gazebo turtlebot3_house.launch.py

Then i launch ros2 launch robot_localization ekf.launch.py

`### ekf config file ### ekf_filter_node: ros__parameters: frequency: 30.0 sensor_timeout: 0.1 two_d_mode: true transform_time_offset: 0.0 transform_timeout: 0.0 print_diagnostics: true debug: false debug_out_file: /path/to/debug/file.txt permit_corrected_publication: false publish_acceleration: false publish_tf: true

    odom_frame: odom            # Defaults to "odom" if unspecified
    base_link_frame: base_footprint  # Defaults to "base_link" if unspecified
    world_frame: odom           # Defaults to the value of odom_frame if unspecified

    odom0: /raw_odom

    odom0_config: [false,  false,  false,
                   false, false, false,
                   true, true, false,
                   false, false, true,
                   false, false, false]

    odom0_queue_size: 2

    odom0_nodelay: false

    odom0_differential: true

    odom0_relative: false

    odom0_pose_rejection_threshold: 5.0
    odom0_twist_rejection_threshold: 1.0

    imu0: imu
    imu0_config: [false, false, false,
                  false,  false,  true,
                  false, false, false,
                  false,  false,  true,
                  false,  false,  true]
    imu0_nodelay: false
    imu0_differential: true
    imu0_relative: true
    imu0_queue_size: 5
    imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
    imu0_twist_rejection_threshold: 0.8                #
    imu0_linear_acceleration_rejection_threshold: 0.8  #

    imu0_remove_gravitational_acceleration: true

    use_control: false

    process_noise_covariance: [0.05, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.05, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.06, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.03, 0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.03, 0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.06, 0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.025, 0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.025, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.04, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.01, 0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.01, 0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.02, 0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.01, 0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.01, 0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.015]

    initial_estimate_covariance: [1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                  0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                  0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,  0.0,     0.0,     0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     1e-9,  0.0,     0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     1e-9,  0.0,    0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     1e-9, 0.0,    0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    1e-9, 0.0,
                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    1e-9]

`

Output:

[INFO] [launch]: All log files can be found below /home/ridhwan/.ros/log/2021-06-01-03-09-38-948817-ridhwan-10437 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [ekf_node-1]: process started with pid [10442] [ekf_node-1] Both imu0_differential and imu0_relative were set to true. Using differential mode. [ekf_node-1] terminate called after throwing an instance of 'std::runtime_error' [ekf_node-1] what(): can't subtract times with different time sources [1 != 2] [ERROR] [ekf_node-1]: process has died [pid 10442, exit code -6, cmd '/home/ridhwan/turtlebot3_ws/install/robot_localization/lib/robot_localization/ekf_node --ros-args -r __node:=ekf_filter_node --params-file /home/ridhwan/turtlebot3_ws/install/robot_localization/share/robot_localization/params/ekf.yaml --params-file /tmp/launch_params_k2l013cy'].

christophebedard commented 3 years ago

I've reproduced this bug (which is most likely the same as https://answers.ros.org/question/380328/robot_localization-cant-subtract-times-with-different-time-sources/).

I put a minimal working example here along with some steps to reproduce: https://github.com/christophebedard/robot_localization-issue-622

By debugging, I can see that the bad rclcpp::Time::operator-() call happens here: https://github.com/cra-ros-pkg/robot_localization/blob/9fba3e8e8b30b653f4a00b0a957e4275168fdf9e/src/filter_base.cpp#L360

reference_time is RCL_ROS_TIME=1 while latest_control_time_ is RCL_SYSTEM_TIME=2. I honestly don't really know which one of those rcl_clock_type_t values makes sense here, but I did notice that latest_control_time_ is initialized without any specific clock type: https://github.com/cra-ros-pkg/robot_localization/blob/9fba3e8e8b30b653f4a00b0a957e4275168fdf9e/src/filter_base.cpp#L50

whereas last_measurement_time_ (same line) is initialized with RCL_ROS_TIME=1, and latest_control_time_ under RosFilter<T> (different class) is initialized with RCL_ROS_TIME=1: https://github.com/cra-ros-pkg/robot_localization/blob/3b2cb792abe0261aa55e6f3f4ad49d7efebcdbd6/src/ros_filter.cpp#L82

So I just changed this line to explicitly make latest_control_time_ a RCL_ROS_TIME=1: https://github.com/cra-ros-pkg/robot_localization/blob/9fba3e8e8b30b653f4a00b0a957e4275168fdf9e/src/filter_base.cpp#L50

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),

and then the issue disappears. Like I said, I'm not sure what makes sense here. @ayrton04 any idea?

VishuMangla commented 3 years ago

I was having same issue as @AppliedRobotics and after doing changes in filter_base.cpp file mentioned above by @christophebedard, the issue is solved.

christophebedard commented 3 years ago

Thanks for the confirmation @VishuMangla!

I opened a PR with the fix: https://github.com/cra-ros-pkg/robot_localization/pull/679

cra-ros-pkg / robot_localization