From d506dbdac77a9995952f70745b304b92ea2ae08f Mon Sep 17 00:00:00 2001
From: Benjamin Perseghetti <bperseghetti@rudislabs.com>
Date: Wed, 1 Feb 2023 09:31:02 -0500
Subject: [PATCH] Add orientation to Odom with covariance. (#1876)

Also added test to avoid regression.

Co-authored-by: James Goppert <james.goppert@gmail.com>
---
 src/systems/odometry_publisher/OdometryPublisher.cc | 4 ++++
 test/integration/odometry_publisher.cc              | 4 ++++
 2 files changed, 8 insertions(+)

diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc
index b9d4e09369..10932c6478 100644
--- a/src/systems/odometry_publisher/OdometryPublisher.cc
+++ b/src/systems/odometry_publisher/OdometryPublisher.cc
@@ -467,6 +467,10 @@ void OdometryPublisherPrivate::UpdateOdometry(
   msgCovariance.mutable_pose_with_covariance()->
     mutable_pose()->mutable_position()->set_z(msg.pose().position().z());
 
+  // Copy orientation from odometry msg.
+  msgs::Set(msgCovariance.mutable_pose_with_covariance()->mutable_pose()->
+    mutable_orientation(), pose.Rot());
+
   // Copy twist from odometry msg.
   msgCovariance.mutable_twist_with_covariance()->
     mutable_twist()->mutable_angular()->set_x(msg.twist().angular().x());
diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc
index dace4d208c..19edce7753 100644
--- a/test/integration/odometry_publisher.cc
+++ b/test/integration/odometry_publisher.cc
@@ -495,11 +495,14 @@ class OdometryPublisherTest
 
     std::vector<math::Vector3d> odomLinVels;
     std::vector<math::Vector3d> odomAngVels;
+    std::vector<math::Quaterniond> odomAngs;
     google::protobuf::RepeatedField<float> odomTwistCovariance;
     // Create function to store data from odometry messages
     std::function<void(const msgs::OdometryWithCovariance &)> odomCb =
       [&](const msgs::OdometryWithCovariance &_msg)
       {
+        odomAngs.push_back(msgs::Convert(_msg.pose_with_covariance().
+          pose().orientation()));
         odomLinVels.push_back(msgs::Convert(_msg.twist_with_covariance().
           twist().linear()));
         odomAngVels.push_back(msgs::Convert(_msg.twist_with_covariance().
@@ -521,6 +524,7 @@ class OdometryPublisherTest
 
     // Verify the Gaussian noise.
     ASSERT_FALSE(odomLinVels.empty());
+    ASSERT_FALSE(odomAngs.empty());
     ASSERT_FALSE(odomAngVels.empty());
     int n = odomLinVels.size();