Merge 259e9ef into 1cb2374

Author: mjcarroll

core/include/gz/msgs/convert/Pose.hh

@@ -40,8 +40,17 @@ inline void Set(gz::msgs::Pose *_msg, const gz::math::Pose3d &_data)
 
 inline void Set(gz::math::Pose3d *_data, const gz::msgs::Pose &_msg)
 {
-  auto pos = Convert(_msg.position());
-  auto orientation = Convert(_msg.orientation());
+  gz::math::Vector3d pos;
+  gz::math::Quaterniond orientation;
+
+  if (_msg.has_position())
+    pos = Convert(_msg.position());
+
+  // This bit is critical.  If orientation hasn't been set in the message,
+  // then we want the quaternion to default to identity.
+  if (_msg.has_orientation())
+    orientation = Convert(_msg.orientation());
+
   _data->Set(pos, orientation);
 }
 

test/integration/Utility_TEST.cc

@@ -122,6 +122,23 @@ TEST(UtilityTest, ConvertMsgPoseToMath)
   EXPECT_TRUE(math::equal(v.Rot().W(), 0.27059805007309851));
 }
 
+/////////////////////////////////////////////////
+TEST(UtilityTest, ConvertMsgPoseToMathIdentity)
+{
+  // Setting position but not orientation should still
+  // result in a valid identity quaternion
+  msgs::Pose msg;
+  msg.mutable_position()->set_x(1.0);
+  msg.mutable_position()->set_y(2.0);
+  msg.mutable_position()->set_z(2.0);
+
+  math::Pose3d v = msgs::Convert(msg);
+  EXPECT_TRUE(math::equal(v.Rot().W(), 1.0));
+  EXPECT_TRUE(math::equal(v.Rot().X(), 0.0));
+  EXPECT_TRUE(math::equal(v.Rot().Y(), 0.0));
+  EXPECT_TRUE(math::equal(v.Rot().Z(), 0.0));
+}
+
 /////////////////////////////////////////////////
 TEST(MsgsTest, ConvertMathColorToMsgs)
 {