0
$\begingroup$

Rosanswers logo

Hello everyone,

I am trying to set up robot_localization for my own robot. it’s a differential robot with jetson nano and ros melodic. It has Wheel Odometry, IMU and GPS When I fuse only Wheel Odometry and IMU, it’s work very well

And then I am running two instances of robot_localization ekf_local fuses only the imu (imu/data) and the wheel odometry (/odom) to provide the tf odom -- base_link ekf_global fuses the imu (imu/data), the wheel odometry (/odom) and the odometry from gps (/odometry/gps) to provide the tf map--odom

Unfortunately I can’t obtain the tf map --> odom

I have this error with roswtf

ERROR TF multiple authority contention:
 * node [/ekf_local] publishing transform [base_link] with parent [odom] already published by node [/ekf_global]
 * node [/ekf_global] publishing transform [base_link] with parent [odom] already published by node [/ekf_local]

It’s very clear but I have been searching for a long time for this problem but still have not found a solution. Now, I’m running out of ideas and that’s why I’m asking for help.

Thanks for advance for help

These are the main parameters of my configuration:

/odom

header: 
  seq: 2353
  stamp: 
    secs: 1621269914
    nsecs: 517945051
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
position: 
  x: 1.42776163451
  y: 1.05459486876
  z: 0.0
orientation: 
  x: 0.0
  y: -0.0
  z: -0.969285642944
  w: -0.245937679876


 covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

/imu/data

header: 
  seq: 23430
  stamp: 
    secs: 1621270028
    nsecs: 842851010
  frame_id: "imu"
orientation: 
  x: 0.0321049389572
  y: -0.0256961583669
  z: 0.862560831451



 w: 0.50427947845
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.00555555555556
  y: 0.00555555555556
  z: -0.00111111111111
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:



  x: -0.03
  y: 0.0
  z: -0.1

/odometry/filtered_global

header: 
  seq: 9837
  stamp: 
    secs: 1621270122
    nsecs: 593107462
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 


  position: 
  x: 3.1402834907
  y: -0.174301283379
  z: 0.363305654322

orientation:

  x: -0.0337846203609
  y: -0.0230412789275
  z: -0.733087734948
  w: 0.678903580608

/odometry/gps

header: 
  seq: 198
  stamp: 
    secs: 1621270660
    nsecs: 123980998
  frame_id: "odom"
child_frame_id: ''


  pose: 
  pose: 
position: 
  x: -47.288572479
  y: 34.9779323968
  z: 0.0

orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0

  covariance: [4.494400000000001, -6.32346808388906e-17, 0.0, 0.0, 0.0, 0.0, -1.3555156131737614e-16, 4.494400000000001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 17.977600000000002, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

launch file

 <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_gps"   args="0.0 0.0 0.0 0.0 0.0 0.0  base_link  gps" />

 <!--   ********************************   -->
  
     <node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_imu"   args="-0.015 0.12 0.0 0.0 0.0 0.0   base_link  imu" /> 

 <!--   **************************  Navsat_transform_node  **************************  -->
    <!--
    <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true" output ="screen">
    -->
 
    <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" clear_params="true" output ="screen">


    <param name="frequency" value="30.0"/>
    <param name="delay" value="3.0"/>

    <param name="magnetic_declination_radians" value="0.046"/>
    <param name="yaw_offset" value="1.57"/>
    <param name="zero_altitude" value="true"/>

    <param name="broadcast_cartesian_transform" value="false"/>



 <param name="publish_filtered_gps" value="true"/>
    <param name="use_odometry_yaw" value="false"/>
    <param name="wait_for_datum" value="false"/>

    <remap from="/imu/data" to="/imu/data" />
    <remap from="/gps/fix" to="/fix" />
    <remap from="/odometry/filtered" to="/odometry/filtered_global" />

    </node>
   
  <!--  ***************   LOCAL  EKF LOCALIZATION    ***********************       -->
     
     <node pkg="robot_localization" type="ekf_localization_node" name="ekf_local" clear_params="true">

        <rosparam command="load" file="$(find mybot_navigation)/config/ekf_local.yaml"/>

        <param name="imu0"  value="/imu/data"/>
        <param name="odom0" value="/odom"/>
  
    </node>

   <!--  ***************   GLOBAL  EKF LOCALIZATION    ***********************       -->
     
     <node pkg="robot_localization" type="ekf_localization_node" name="ekf_global" clear_params="true">

        <rosparam command="load" file="$(find mybot_navigation)/config/ekf_global.yaml"/>

        <param name="imu0"  value="/imu/data"/>
        <param name="odom0" value="/odom"/>
        <param name="odom1" value="/odometry/gps            "/>

        <remap from="odometry/filtered" to="odometry/filtered_global"/>
  
    </node>  

ekf_local

f

requency: 30

sensor_timeout: 0.1

two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false

publish_tf: true

map_frame: map
odom_frame: odom
base_link_frame: base_link

worl_frame: odom


  
    # Configuration Odometry
    odom0: odom
    
    odom0_config: [false,  false,  false,
                   false, false, false,
                   true, true, true,
                   false, false, true,
                   false, false, false]

odom0_queue_size: 10
odom0_differential: false
odom0_nodelay: true
odom0_relative: false

# Configuration IMU

imu0: imu/data
imu0_config: [false, false, false,
              true,  true, false,
              false, false, false,
              true, true, true,
              true, true, true]


imu0_differential: false
imu0_nodelay: false
imu0_relative: false
imu0_queue_size: 10

process_noise_covariance: [1e-3, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    1e-3, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.3, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.3, 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.5, 0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0.5, 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.3, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.3, 0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.3, 0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.3, 0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.3, 0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.3]


initial_estimate_covariance: [1e-9, 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,    1e-9, 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,    1.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,    1.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,    1.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,     1.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,     1.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,    1.0]

ekf_global

frequency: 30

sensor_timeout: 0.1

transform_time_offset: 0.2
transform_timeout: 0.0
print_diagnostics: true
two_d_mode: false

publish_tf: true

map_frame: map
odom_frame: odom
base_link_frame: base_link

worl_frame: map


# Configuration Odometry
odom0: odom

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

odom0_differential: false
odom0_nodelay: true
odom0_relative: false
odom0_queue_size: 10


# Configuration GPS
odom1: odometry/gps

odom1_config: [true,  true,  false,
               false, false, false,
               false, false, false,
               false, false, false,
               false, false, false]

odom1_differential: false
odom1_nodelay: true
odom1_relative: false
odom1_queue_size: 10

# Configuration IMU
imu0: imu/data

imu0_config: [false, false, false,
              true,  true, false,
              false, false, false,
              true, true, true,
              true, true, true]


imu0_differential: false
imuO_nodelay: true
imu0_relative: false
imu0_queue_size: 10

process_noise_covariance: [1.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, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 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.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 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.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3]

initial_estimate_covariance: [1.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,    1e-9, 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,    1.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,    1.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,    1.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,     1.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,     1.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,    1.0]

rqt_graph:

rqt_graph

tf_tree:

tf_tree


Originally posted by Bertrand on ROS Answers with karma: 3 on 2021-05-17

Post score: 0


Original comments

Comment by Humpelstilzchen on 2021-05-18:
Can you please add a tf tree and a node graph? (e.g. rqt_tf_tree/rqt_graph)

Comment by Bertrand on 2021-05-18:
Thanks to have a look at my problem There, the links for : rqt_graph https://drive.google.com/file/d/1zuM-59orj0flnZP5ziVCiNd_U5AS9w8j/view?usp=sharing tf_tree https://drive.google.com/file/d/1cVhrKlboEmmmDvdl058ffIMZJ_6kBI_d/view?usp=sharing

Comment by Humpelstilzchen on 2021-05-19:
Sorry was not able to check that yet, but you have all zero for your covariances. That is a usual source of error, but I doubt that this is the issue here.

Comment by gvdhoorn on 2021-05-20:
@Bertrand: please do not use off-site hosting for information which is just about central to your problem.

I've attached the screenshots to your post here.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

There is a typo in both robot_localization configs: "worl_frame" instead of "world_frame".

I admit it is really hard to see, I wonder if a script like roswtf could automatically catch those, e.g. by checking set parameters against defined parameters.

Please also fix the covariances for odom and imu, they are all zero. See Considerations for Each Sensor Message Type


Originally posted by Humpelstilzchen with karma: 1504 on 2021-05-19

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Bertrand on 2021-05-20:
Thanks a lot, I'm sorry for these simple mistakes, I work on it tomorrow I keep you informed

Comment by Bertrand on 2021-05-21:
I did outdoor test, it's works very well

$\endgroup$

Your Answer

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