GPS/IMU are measured from the wrong pose

[osrf-migration]

Original report (archived issue) by Kevin Allen (Bitbucket: kev-the-dev).

---

Team UF has spent about two weeks investigating instability in our control / state estimation systems. After much debugging, the problem has been identified as a bug upstream in VRX. There is some possibility that this is caused by a difference in dependency versions (we use a modified container), so I encourage the maintainers to attempt to reproduce the issue independently.

‌

Summary

The IMU and GPS report (in their header messages) that they are being measured relative to the link at the pose teams specify in their sensor_config.yaml. However, these measurements are actually at base_link. AFAIK, this is not a documented behavior. These discrepancies will obviously throw off state estimation significantly.

‌

How to reproduce

• Create a wamv with a gps at a some pose nearby to base link. Launch vrx, echo out the gps fix topic, and note the lat / long
• Create a second wamv with a gps at some absurdly far away pose (x=10000).
• Launch vrx, echo out the gps fix, and note the lat / long
• In both cases, the lat / long is reported as the same. The analogous experiment is true of the IMU. The pose specified in sensor_config.yaml does not affect the pose the plugin measures relative to.

‌

Analysis

After the URDF is generated, everything seems okay. Links exist at the correct poses and the hector_gazebo plugins have these links specified

  <gazebo>
    <plugin filename="libhector_gazebo_ros_imu.so" name="imu_plugin_imu_wamv">
     ...
      <bodyName>imu_wamv_link</bodyName>
      ...
      <!-- Manually prepend namespace to tf frame. -->
      <frameId>wamv/imu_wamv_link</frameId>
     ...
    </plugin>
  </gazebo>

However, if you add some debug lines to hector_gazebo, you will see that at some point during the URDF->SDF process this tag seems to be changed.

diff --git a/hector_gazebo_plugins/src/gazebo_ros_gps.cpp b/hector_gazebo_plugins/src/gazebo_ros_gps.cpp
index 79d860b..b27becf 100644
--- a/hector_gazebo_plugins/src/gazebo_ros_gps.cpp
+++ b/hector_gazebo_plugins/src/gazebo_ros_gps.cpp
@@ -74,20 +74,34 @@ void GazeboRosGps::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
 
   if (!_sdf->HasElement("bodyName"))
   {
+    ROS_ERROR("No body name flag, defaulting to base_link");
+
     link = _model->GetLink();
     link_name_ = link->GetName();
   }
   else {
+
     link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString();
+    ROS_ERROR_STREAM("Measuring GPS relative to " << link_name_);
+
     link = _model->GetLink(link_name_);
   }
 
+
   if (!link)
   {
     ROS_FATAL("GazeboRosGps plugin error: bodyName: %s does not exist\n", link_name_.c_str());
     return;
   }
 
+
+    for(auto link : _model->GetLinks())
+    {
+      ROS_ERROR_STREAM("Link " << link->GetName() << " Pose " << link->RelativePose());
+    }
+
+
+
   // default parameters
   frame_id_ = "/world";
   fix_topic_ = "fix";

This produces:

[ERROR] [1573845827.616271176, 0.001000000]: Measuring GPS relative to base_link
[ERROR] [1573845827.616431306, 0.001000000]: Link base_link Pose 0 0 0 0 -0 0

Indicating that hector_gazebo actually processes the sensors relative to base_link, but reports them at the sensor link.

‌

Mitigiation

As a hack, Team UF runs our URDF through a script to set the gps / imu links to have a pose of zero so transforms work correctly when published by robot_state_publisher. Others will have to change the hard coded joint names to their own.

#!/usr/bin/python2
'''
Fixes an URDF file which uses hector_gazebo_plugins for gps/imu.
Beacause gazebo_ros's URDF->SDF blindly replaces link name strings with base_link, thus
making the imu/gps plugins measure from base_link but report a different link.
This fixes the urdf to have these sensors also be at baselink so transforms work correctly.
Note: currently hardcoded for VRX purposes. Simply change the strings to work more genericly.
'''
from __future__ import print_function
import xml.etree.ElementTree as ET
import sys


def set_joint_origin_to_zero(root, name):
    tag = root.find("*[@name='{}']".format(name))
    origin = tag.find("origin")
    origin.set('rpy', '0 0 0')
    origin.set('xyz', '0 0 0')


def fix_gps(root):
    set_joint_origin_to_zero(root, 'gps_wamv_joint')


def fix_imu(root):
    set_joint_origin_to_zero(root, 'imu_wamv_joint')


def fix_file(filename):
    tree = ET.parse(filename)
    root = tree.getroot()
    fix_gps(root)
    fix_imu(root)
    print(ET.tostring(root))


if __name__ == '__main__':
    fix_file(sys.argv[1])                             

‌

[osrf-migration]

Original comment by Brian Bingham (Bitbucket: brian_bingham).

---

Kevin,

This is a great write up to the issue. Thank you. We have been discussing and Carlos is working it, but may take a few hours due to timezones.

Does your proposed mitigation (zeroing the frame pose value in the URDF) address the issue for UF?

‌

[osrf-migration]

Original comment by Kevin Allen (Bitbucket: kev-the-dev).

---

Our mitigation works for our purposes, as it causes the transforms from sensor frames to base_link to be zeroed

[osrf-migration]

Original comment by Carlos Agüero (Bitbucket: caguero, GitHub: caguero).

---

See pull request #216.

[osrf-migration]

Original comment by Coline Ramee (Bitbucket: Coline Ramee).

---

Hi,

We noticed the same issue right after dress_rehearsal. We mitigated it by just assuming in our state estimator that the measurement from the sensors were in base_link and disregarding the headers.

Georgia Tech

[osrf-migration]

Original comment by Kevin Allen (Bitbucket: kev-the-dev).

---

My opinion is that things should be left as-is for phase3, as it would impose significant development time to adjust to any merged fix for this bug unless the deadline is extended in light of this bug.

[osrf-migration]

Original comment by Brian Bingham (Bitbucket: brian_bingham).

---

Coline Ramee (Coline Ramee) Thank you for chiming in. Good to know GT was able to work around.

[osrf-migration]

Original comment by Brian Bingham (Bitbucket: brian_bingham).

---

I expect the VRX technical team will be able to put our heads together Tues morning when timezones align to finalized this issue. In the meantime, please continue to work with the system as-is.

[osrf-migration]

Original comment by Brian Bingham (Bitbucket: brian_bingham).

---

Our current plan is as follows:

1. Not make any changes to the GPS/IMU setup this late in the competition, i.e., not merge PR#216. The reasoning is that this is a significant change to the system that may have unintentional effects. Introducing the change this late in the competition could have serious consequences. Furthermore, for the two teams we’ve heard from they have work arounds for the issue. We are assuming that many of the other teams are unaffected by the issue or have been able to address the issue.
2. Once we finalize the decision (today?) we will make an email/forum announcement so that if there are teams that are silently struggling with this issue they are aware of the potential remedies.

Before we finalize this, one question for Kevin Allen (kev-the-dev) and the UF team. To be clear, you are running your mitigation/hack solution within your submitted VRX team - you are not expecting that this program would be run on the VRX server, correct?

‌

[osrf-migration]

Original comment by Kevin Allen (Bitbucket: kev-the-dev).

---

Correct, our mitigation does not require any changes to VRX upstream.

[osrf-migration]

Original comment by Brian Bingham (Bitbucket: brian_bingham).

---

Great - thank you for the confirmation.

[osrf-migration]

Original comment by Brian Bingham (Bitbucket: brian_bingham).

---

Temporary Resolution

At the current time, a few days before the final VRX due date, we are not going to make a change to address the inconsistent offsets between the sensors frames and the base_link frame. Both UF and Georgia Tech report that they have mitigation solutions that are functional and we don’t want to risk disturbing their solutions or those of other participants.

We will leave the issue open (unresolved) so that it continues to be tracked. After VRX we will likely make the change when there is less risk and appropriate time for testing.

Thank you all for working on this last minute issue.

[osrf-migration]

Original comment by Carlos Agüero (Bitbucket: caguero, GitHub: caguero).

---

• changed state from "new" to "resolved"

Merged in fix_issue_188 (pull request #216)

Fix issue #188

Approved-by: Brian Bingham briansbingham@gmail.com

→ <<cset 3e44023>>

[osrf-migration]

Original comment by Hashir Zahir (Bitbucket: hzahir).

---

Hi guys, (Brian Bingham (brian_bingham) , @caguero ) thanks for the amazing fix! I noticed similar issues while working with the IMU plugin while using fixed joints. Using revolute joints fixed the issue (mostly) or so I thought as I have discovered a bug. Rotations in yaw for the IMU plugin work perfectly with this new fix (the fixed joint method reports inaccurate readings), however, rotations in pitch or roll result in incorrect readings from the IMU.

For example, using <xacro:wamv_imu name="imu_wamv" y="-0.2" R="3.14159"/> where the roll is set to 180 degrees instead of yaw, the raw readings reported by the IMU plugin is (r,p,y) = (0,0, 2.76) when the correct readings are supposed to be (r,p,y) = (0,0,-2.76) (notice the yaw has been flipped/inverted). This does not make sense since physical IMU has been flipped 180 degrees. Setting an arbitary value of rpy in the macro results in even more disastrous readings.

Example: <xacro:wamv_imu name="imu_wamv" y="-0.2" R="1" P="0" Y="0"/>

results in rpy = (1.966, -0.319, -2.928), when the actual reading should have been rpy = (1,0,-2.76).

Do you guys know why this is happening? Although I understand this is a strange requirement for teams to mount their sensors in weird positions (roll, pitch). Thanks.