Add orientation to Odom with covariance. (#1876)

Also added test to avoid regression. Co-authored-by: James Goppert <[email protected]>
gazebosim · Feb 1, 2023 · d506dbd · d506dbd
1 parent af73ebe
commit d506dbd
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 0 deletions.
diff --git 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
@@ -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();