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

Odometry TF late in time when published with EKF

asked 2023-06-30 13:34:36 -0500

Dan__2022 gravatar image

updated 2023-06-30 14:36:33 -0500

Hi!

Something strange happens with my simpliest config robot (simulated withGazebo)

I have simple EKF config, and when

  • the odom->base_link TF is published by EKF filter node I have my odometry frame very slow, the robot has already stopped, the wheels aren't rotating, but odometry is still in movement, like in some time-slowdown mode:

image description

  • odom->base_link published by gazebo differential drive plugin - it's smooth and precise:

image description

ros Iron and gazebo are up-to-date all nodes use sim-time and have good frequency

image description

What should I check, please help me. Thanks!

EKF node yaml: for the first case

 ekf_filter_local_node:
        ros__parameters:

            use_sim_time: true
            frequency: 30.0
            sensor_timeout: 0.1
            transform_time_offset: 0.0
            two_d_mode: true
            publish_acceleration: true

            publish_tf: true

            print_diagnostics: true
            debug: false
            reset_on_time_jump: true

            # transform odom->base_link

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


            odom0: demo/odom
            odom0_config: [true,  true,  false,
                           false, false, false,
                           false, false, false,
                           false, false, true,
                           false, false, false]

gazebo diff-drive plugin is

<plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
      <ros>
        <namespace>/demo</namespace>
      </ros>

      <!-- wheels -->
      <left_joint>drivewhl_l_joint</left_joint>
      <right_joint>drivewhl_r_joint</right_joint>

           <!-- kinematics -->
          <wheel_separation>0.4</wheel_separation>
          <wheel_diameter>0.2</wheel_diameter>

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

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

          <odometry_frame>odom</odometry_frame>
          <robot_base_frame>base_link</robot_base_frame>
        </plugin>

[EDIT]

in 30 minutes odom is far-far late in time, seems, odom will react in an hour after the movement is done: image description

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2023-07-25 12:01:52 -0500

Dan__2022 gravatar image

Short explaination is that EKF filter, which publishes odom_tf, was too slow because of low covariances. added this code to local_ekf node yaml:

 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, 1.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, 1.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, 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, 1.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, 1.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, 1.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, 1.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, 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]

        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 ...
(more)
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2023-06-30 13:34:36 -0500

Seen: 69 times

Last updated: Jul 25 '23