Add orientation to Odom with covariance.

Also added test to avoid regression.

Signed-off-by: Benjamin Perseghetti <[email protected]>

Co-authored-by: James Goppert <[email protected]>

Author: bperseghetti

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());

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();