Prevention of (0,0,0) Gps Odometry Due To Failed Transform #40

1hada · 2023-09-21T16:43:03Z

Overview

Developer(s): @1hada

Impact

There is a bug in which the navsat transform can send an invalid (0,0,0) gps odometry message. this results in downstream users receiving invalid data and not realizing this. This can cause safety concerns in which a vehicle will react very differently than expected.
This PR mitigates this safety issue by preventing the navsat transform from sending invalid gps odometry when the transform is unavailable.

Description

Logged data shows that a valid GPS update can result in an invalid (0,0,0) GPS odometry message sent from the navsat transform.
This issue was tracked to the navsat transform not finding the last transform when needed.
The navsat transform code only logs a throttled message, but does not prevent the invalid data from being sent.
This PR prevents the invalid data from being sent to avoid downstream users from receiving invalid data.
Since the output data is typically used as measurements into a filter (e.g. the Kalman Filters in this package), the filter should not be harmed by the lack of update and will instead grow the covariance. We suggest monitoring to prevent acting on high covariances from the filter.

Changes

Added check for whether the Transform between the GPS module and the Base Link was successful.
Not publishing GPS odometry if the Transform between GPS and baselink is not successful. OR if the transform between baselink and world frame is not available.

Testing:

Tests were conducted using NEWTIME=$(date -d "$(date '+%F %T.%N Z') - 0.5 second" '+%F %T.%N') ; timedatectl set-time "$NEWTIME" after making sure set-ntp was set to false.
Tests were also conducted using NEWTIME=$(date -d "$(date '+%F %T.%N Z') - 30.5 second" '+%F %T.%N') ; timedatectl set-time "$NEWTIME" after making sure set-ntp was set to false.
In all tests, no (0,0,0) was observed as a result of the time shift. This shows the fix passes.

Checklist:

PR Title updated to include [Platforms]
Write entry on CHANGELOG.rst
Tested on vehicle

simple merge into noetic

Merge Noetic-Devel Into Noetic

Update Noetic to match noetic-devel for HOTFIX release

Merge noetic-devel into noetic

Merging Noetic-Devel to Noetic

shortcut-integration · 2023-09-21T16:43:07Z

This pull request has been linked to Shortcut Story #32541: Initial Prevention of (0,0,0) gps odometry during time jump.

1hada · 2023-09-21T16:44:06Z

src/navsat_transform.cpp

+        // Rotate the covariance as well
+        tf2::Matrix3x3 rot(cartesian_world_transform_.getRotation());
+        Eigen::MatrixXd rot_6d(POSE_SIZE, POSE_SIZE);
+        rot_6d.setIdentity();

-      // Rotate the covariance
-      latest_cartesian_covariance_ = rot_6d * latest_cartesian_covariance_.eval() * rot_6d.transpose();
+        for (size_t rInd = 0; rInd < POSITION_SIZE; ++rInd)
+        {
+          rot_6d(rInd, 0) = rot.getRow(rInd).getX();
+          rot_6d(rInd, 1) = rot.getRow(rInd).getY();
+          rot_6d(rInd, 2) = rot.getRow(rInd).getZ();
+          rot_6d(rInd+POSITION_SIZE, 3) = rot.getRow(rInd).getX();
+          rot_6d(rInd+POSITION_SIZE, 4) = rot.getRow(rInd).getY();
+          rot_6d(rInd+POSITION_SIZE, 5) = rot.getRow(rInd).getZ();
+        }

-      // Now fill out the message. Set the orientation to the identity.
-      tf2::toMsg(transformed_cartesian_robot, gps_odom.pose.pose);
-      gps_odom.pose.pose.position.z = (zero_altitude_ ? 0.0 : gps_odom.pose.pose.position.z);
+        // Rotate the covariance
+        latest_cartesian_covariance_ = rot_6d * latest_cartesian_covariance_.eval() * rot_6d.transpose();

-      // Copy the measurement's covariance matrix so that we can rotate it later
-      for (size_t i = 0; i < POSE_SIZE; i++)
-      {
-        for (size_t j = 0; j < POSE_SIZE; j++)
+        // Now fill out the message. Set the orientation to the identity.
+        tf2::toMsg(transformed_cartesian_robot, gps_odom.pose.pose);
+        gps_odom.pose.pose.position.z = (zero_altitude_ ? 0.0 : gps_odom.pose.pose.position.z);
+
+        // Copy the measurement's covariance matrix so that we can rotate it later
+        for (size_t i = 0; i < POSE_SIZE; i++)
        {
-          gps_odom.pose.covariance[POSE_SIZE * i + j] = latest_cartesian_covariance_(i, j);
+          for (size_t j = 0; j < POSE_SIZE; j++)
+          {
+            gps_odom.pose.covariance[POSE_SIZE * i + j] = latest_cartesian_covariance_(i, j);
+          }
        }
-      }

-      // Mark this GPS as used
-      gps_updated_ = false;
-      new_data = true;
+        // Mark this GPS as used
+        gps_updated_ = false;
+        new_data = true;
+      }


That code was only indented. not changed.

AGummyBear · 2023-09-21T19:16:23Z

src/navsat_transform.cpp

-      gps_updated_ = false;
-      new_data = true;
+        // Mark this GPS as used
+        gps_updated_ = false;


Do we need to do something with this class-member variable in an else condition? If gps_updated_ remains true, what consequence does that have?

If gps_updated_ remains true, what consequence does that have?

The only consequence is that next time the prepareGpsOdometry(..) is called , then it would use the latest latest_cartesian_pose_ ( made from the latest time a gps fix message was received.) . We'd also be using that latest gps fix header time as well.

1hada and others added 7 commits March 8, 2023 15:02

Merge pull request #28 from Greenzie/noetic-devel

06e1a20

simple merge into noetic

Merge pull request #32 from Greenzie/noetic-devel

5a9e94b

Merge Noetic-Devel Into Noetic

Merge pull request #35 from Greenzie/noetic-devel

061f0d8

Update Noetic to match noetic-devel for HOTFIX release

Merge pull request #37 from Greenzie/noetic-devel

46092db

Merge noetic-devel into noetic

Merge pull request #39 from Greenzie/noetic-devel

d01460b

Merging Noetic-Devel to Noetic

adding check for if the final gps odom has been transformed.

d30c8c5

changelog update

f7c9f36

1hada requested review from taylorwentzel and IntelligentJackal September 21, 2023 16:43

1hada commented Sep 21, 2023

View reviewed changes

AGummyBear reviewed Sep 21, 2023

View reviewed changes

taylorwentzel approved these changes Sep 21, 2023

View reviewed changes

AGummyBear approved these changes Sep 22, 2023

View reviewed changes

IntelligentJackal approved these changes Sep 22, 2023

View reviewed changes

1hada changed the title ~~[Intrepid] Prevention of (0,0,0) Gps Odometry Due To Failed Transform~~ Prevention of (0,0,0) Gps Odometry Due To Failed Transform Sep 22, 2023

1hada merged commit ee5cfe1 into noetic-devel Sep 22, 2023
2 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Prevention of (0,0,0) Gps Odometry Due To Failed Transform #40

Prevention of (0,0,0) Gps Odometry Due To Failed Transform #40

1hada commented Sep 21, 2023 •

edited by IntelligentJackal

Loading

shortcut-integration bot commented Sep 21, 2023

1hada Sep 21, 2023

AGummyBear Sep 21, 2023

1hada Sep 21, 2023

Prevention of (0,0,0) Gps Odometry Due To Failed Transform #40

Prevention of (0,0,0) Gps Odometry Due To Failed Transform #40

Conversation

1hada commented Sep 21, 2023 • edited by IntelligentJackal Loading

Overview

Impact

Description

Changes

Testing:

Checklist:

shortcut-integration bot commented Sep 21, 2023

1hada Sep 21, 2023

Choose a reason for hiding this comment

AGummyBear Sep 21, 2023

Choose a reason for hiding this comment

1hada Sep 21, 2023

Choose a reason for hiding this comment

1hada commented Sep 21, 2023 •

edited by IntelligentJackal

Loading