diff --git a/src/Conversions.cc b/src/Conversions.cc index 9ea41f4a8f..ad8b304a70 100644 --- 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 index 2227692e6d..71dc9cc6de 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -171,6 +171,15 @@ TEST(Conversions, Pose) auto pose = convert(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(msg2); + EXPECT_EQ(math::Pose3d(1, 2, 3, 1.0, 0, 0, 0), pose); } /////////////////////////////////////////////////