Fix pose msg conversion when msg is missing orientation (#450)

Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]>
gazebosim · Nov 6, 2020 · 432028f · 432028f
1 parent 074098d
commit 432028f
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 0 deletions.
diff --git a/src/Conversions.cc b/src/Conversions.cc
@@ -113,6 +113,7 @@ math::Pose3d ignition::gazebo::convert(const msgs::Pose &_in)
                    _in.orientation().x(),
                    _in.orientation().y(),
                    _in.orientation().z());
+  out.Correct();
 
   return out;
 }

diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc
@@ -171,6 +171,15 @@ TEST(Conversions, Pose)
 
   auto pose = convert<math::Pose3d>(msg);
   EXPECT_EQ(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3, 0.4), pose);
+
+  // Test empty orientation.
+  msgs::Pose msg2;
+  msg2.mutable_position()->set_x(1);
+  msg2.mutable_position()->set_y(2);
+  msg2.mutable_position()->set_z(3);
+
+  pose = convert<math::Pose3d>(msg2);
+  EXPECT_EQ(math::Pose3d(1, 2, 3, 1.0, 0, 0, 0), pose);
 }
 
 /////////////////////////////////////////////////