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