EKF Sensor Fusion Reference Frame Confusion

[rjnieves]

When starting a Swarmie simulated run, the ekf_localization service that fuses odometry, IMU, and simulated GPS data is unable to produce any output because the labels assigned to the reference frames are not entirely related. Focusing on the "achilles" rover, the /achilles_MAP node doing the EKF sensor fusion regularly reports the following warning message:

[ WARN] [1558460149.046710535, 1457.277000000]: Could not obtain transform from base_link to achilles/base_link. Error was Could not find a connection between 'achilles/base_link' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.

Based on the computation graph, /achilles_MAP receives its sensor readings from /achilles/imu, /achilles/odom/navsat, and /achilles/odom/filtered topics. The /achilles/odom/filtered topic is itself an EKF-based sensor fusion that receives odometry from the Gazebo simulation via /achilles/odom topic. /achilles/odom/navsat is not producing any output (verified via rostopic echo -n 1 /achilles/odom/navsat), but /achilles/imu produces samples with a frame_id of achilles/base_link. Similarly, /achilles/odom produces samples with a child_frame_id of achilles/base_link. So far those two topics have a clear path in the transform tree set up by the environment, evidenced by the fact that the /achilles_ODOM EKF fusion node is able to produce output from those two inputs. The problem starts with the output from the /achilles_ODOM node. The aforementioned node produces samples via the /achilles/odom/filtered topic for eventual consumption by the /achilles_MAP EKF node. The samples produced by /achilles_ODOM contain a child_frame_id of base_link. Faced with a lack of an established relationship between the achilles/base_link and base_link reference frames, /achilles_MAP basically throws its hands up in the air continuously reporting the documented warning shown earlier in this report. Unless the base_link reference frame reported by the /achilles_ODOM node truly refers to a frame not related to the "achilles" swarmie (that would be odd, since all the documentation I've read on ROS robot localization states that base_link is always anchored to the robot), I believe /achilles_ODOM should be producing samples with a child_frame_id of achilles/base_link.

I would have assumed that the samples produced by the /achilles_ODOM EKF fusion node would not alter the child_frame_id reported from the source /achilles/odom topic samples. From reading through the ROS Robot Localization wiki page, there doesn't seem to be a way to tune such behavior. I assume the tf_prefix configured at launch is not designed to have an effect on how /achilles_ODOM populates odometry samples.

Any help you can provide to resolve this issue will be most appreciated!

Details on the host environment are:

• Ubuntu 16.04
• Core i7-7700 CPU
• 16GB RAM
• NVIDIA GTX 970 using proprietary drivers
• ROS Kinetic Kame desktop-full installation (as required in repository documentation)

A report fragment from tf_monitor regarding the reference frames being transacted during the simulation follows:

RESULTS: for all Frames

Frames:
Frame: achilles/camera_link published by unknown_publisher Average Delay: -0.111 Max Delay: 0
Frame: aeneas/camera_link published by unknown_publisher Average Delay: -0.104 Max Delay: 0
Frame: ajax/camera_link published by unknown_publisher Average Delay: -0.111 Max Delay: 0
Frame: base_link published by unknown_publisher Average Delay: 0.0424 Max Delay: 0.078

All Broadcasters:
Node: unknown_publisher 74.7664 Hz, Average Delay: -0.01425 Max Delay: 0.078

Sample outputs acquired during a simulation run for the pertinent topics follows:

/achilles/odom

header: 
  seq: 1113
  stamp: 
    secs: 753
    nsecs: 509000000
  frame_id: "achilles/odom"
child_frame_id: "achilles/base_link"
pose: 
  pose: 
    position: 
      x: -1.29508458625
      y: -0.000211570264346
      z: 0.0
    orientation: 
      x: -1.38297738598e-06
      y: 1.20544195471e-05
      z: -0.000830962545951
      w: 0.999999654677
  covariance: [0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]
twist: 
  twist: 
    linear: 
      x: 1.01685156444e-05
      y: 5.0597081402e-05
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -0.00011794091591
  covariance: [0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]
---

/achilles/imu

header: 
  seq: 1257
  stamp: 
    secs: 767
    nsecs: 900000000
  frame_id: "achilles/base_link"
orientation: 
  x: -0.0224001610787
  y: -0.0218322288495
  z: -0.000860195993494
  w: 0.999510303414
orientation_covariance: [0.0012755102040816321, 0.0, 0.0, 0.0, 0.0012755102040816321, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.0818270900424
  y: 0.00177570859961
  z: 0.100639312599
angular_velocity_covariance: [0.0025000000000000005, 0.0, 0.0, 0.0, 0.0025000000000000005, 0.0, 0.0, 0.0, 0.000225]
linear_acceleration: 
  x: 0.424689128116
  y: -0.608418610307
  z: 9.29627907127
linear_acceleration_covariance: [0.12249999999999998, 0.0, 0.0, 0.0, 0.12249999999999998, 0.0, 0.0, 0.0, 0.09]
---

/achilles/odom/filtered

header: 
  seq: 2217
  stamp: 
    secs: 873
    nsecs: 613000000
  frame_id: "achilles/odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 0.00228357976332
      y: -1.55183957814e-06
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.000129639877294
      w: 0.999999991597
  covariance: [18.769827752897584, -45412.17640359878, 0.0, 0.0, 0.0, 4.035981505853958e-16, -45412.176403599, 285692089.4059026, 0.0, 0.0, 0.0, -5.992269290383455e-14, 0.0, 0.0, 9.9872040953055e-07, -1.888278771163367e-22, -2.160883534723523e-19, 0.0, 0.0, 0.0, -1.8882787711633664e-22, 9.974457215245064e-07, 1.1905614389377754e-31, 0.0, 0.0, 0.0, -2.1608835347235223e-19, -1.828320253652615e-31, 9.974457215245064e-07, 0.0, 4.0359815058539566e-16, -5.992269290383459e-14, 0.0, 0.0, 0.0, 0.0007800361267026867]
twist: 
  twist: 
    linear: 
      x: 1.0173254863e-05
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0641828343283
  covariance: [9.658430262089214e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 37758.56438082822, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.980824545097928e-07, -1.1626937158365496e-31, -1.3290017178332258e-28, 0.0, 0.0, 0.0, -1.1626937158365485e-31, 9.923954210392151e-07, 2.9150868274292988e-40, 0.0, 0.0, 0.0, -1.3290017178332262e-28, -4.47292254663197e-40, 9.923954210392151e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0004508606164653256]
---

[rjnieves]

I believe I found at least a work-around, if not a fix altogether. Apparently after version 2.4.6 of ros-kinetic-robot-localization, a configuration parameter was added which let users customize the child_frame_id output by ekf_localization_node. The name of the parameter is base_link_frame_output. Configuring this parameter for both the $(arg name)_ODOM and $(arg name)_MAP EKF localization nodes to a value of $(arg name)/base_link makes the nodes produce odometer samples that correctly identify the individual robot's fixed frame. My current installation of ROS was using ros-kinetic-robot-localization version 2.4.7.

Beware when reading the documentation provided by the ros-kinetic-robot-localization package. In later revisions of the state_estimation_nodes.rst file they refer to the aforementioned configuration parameter as base_link_output_frame. Reading through the code, however (src/ros_filter.cpp line 754 in the kinetic-devel branch), shows that the node's programmed parameter name is actually base_link_frame_output.

If needed or requested, I can fork/edit then create a pull request with the fix, but it is pretty simple.