Updated example, initialized offset

Signed-off-by: Aditya <[email protected]>
gazebosim · Mar 8, 2022 · d314d42 · d314d42
1 parent 7a9cea5
commit d314d42
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 15 deletions.
diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc
@@ -94,7 +94,7 @@ class ignition::gazebo::systems::OdometryPublisherPrivate
   public: math::clock::time_point lastUpdateTime;
 
   /// \brief Allow specifying constant xyz and rpy offsets
-  public: ignition::math::Pose3d offset;
+  public: ignition::math::Pose3d offset = {0, 0, 0, 0, 0, 0};
 };
 
 //////////////////////////////////////////////////
@@ -145,22 +145,13 @@ void OdometryPublisher::Configure(const Entity &_entity,
     this->dataPtr->odomFrame = _sdf->Get<std::string>("odom_frame");
   }
 
-  if (!_sdf->HasElement("xyz_offset"))
-  {
-    this->dataPtr->offset.Pos() = ignition::math::Vector3d(0, 0, 0);
-  }
-  else
+  if (_sdf->HasElement("xyz_offset"))
   {
     this->dataPtr->offset.Pos() = _sdf->Get<ignition::math::Vector3d>(
       "xyz_offset");
   }
 
-  if (!_sdf->HasElement("rpy_offset"))
-  {
-    this->dataPtr->offset.Rot() =
-      ignition::math::Quaterniond(ignition::math::Vector3d(0, 0, 0));
-  }
-  else
+  if (_sdf->HasElement("rpy_offset"))
   {
     this->dataPtr->offset.Rot() =
       ignition::math::Quaterniond(_sdf->Get<ignition::math::Vector3d>(

diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc
@@ -432,8 +432,8 @@ class OdometryPublisherTest
     // Run for 3s and check the pose in the last message
     ASSERT_FALSE(odomPoses.empty());
     auto lastPose = odomPoses[odomPoses.size() - 1];
-    EXPECT_NEAR(lastPose.Pos().X(), 1, 1e-2);
-    EXPECT_NEAR(lastPose.Pos().Y(), -1, 1e-2);
+    EXPECT_NEAR(lastPose.Pos().X(), 11, 1e-2);
+    EXPECT_NEAR(lastPose.Pos().Y(), -11, 1e-2);
     EXPECT_NEAR(lastPose.Pos().Z(), 0, 1e-2);
 
     EXPECT_NEAR(lastPose.Rot().Roll(), 1.57, 1e-2);

diff --git a/test/worlds/odometry_offset.sdf b/test/worlds/odometry_offset.sdf
@@ -52,7 +52,7 @@
     </model>
 
     <model name='vehicle'>
-      <pose>0 0 0 0 0 0</pose>
+      <pose>10 -10 0 0 0 0</pose>
 
       <link name='chassis'>
         <pose>-0.151427 -0 0.175 0 -0 0</pose>