diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index ab32210375..e37928eeb9 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -1112,13 +1112,14 @@ bool CreateCommand::Execute() this->iface->creator->SetParent(entity, this->iface->worldEntity); // Pose + std::optional createPose; if (createMsg->has_pose()) { - auto poseComp = this->iface->ecm->Component(entity); - *poseComp = components::Pose(msgs::Convert(createMsg->pose())); + createPose = msgs::Convert(createMsg->pose()); } + // Spherical coordinates - else if (createMsg->has_spherical_coordinates()) + if (createMsg->has_spherical_coordinates()) { auto scComp = this->iface->ecm->Component( this->iface->worldEntity); @@ -1141,12 +1142,24 @@ bool CreateCommand::Execute() math::SphericalCoordinates::SPHERICAL, math::SphericalCoordinates::LOCAL2); - auto poseComp = this->iface->ecm->Component(entity); - *poseComp = components::Pose({pos.X(), pos.Y(), pos.Z(), 0, 0, - IGN_DTOR(createMsg->spherical_coordinates().heading_deg())}); + // Override pos and add to yaw + if (!createPose.has_value()) + createPose = math::Pose3d::Zero; + createPose.value().SetX(pos.X()); + createPose.value().SetY(pos.Y()); + createPose.value().SetZ(pos.Z()); + createPose.value().Rot() = math::Quaterniond(0, 0, + IGN_DTOR(createMsg->spherical_coordinates().heading_deg())) * + createPose.value().Rot(); } } + if (createPose.has_value()) + { + auto poseComp = this->iface->ecm->Component(entity); + *poseComp = components::Pose(createPose.value()); + } + igndbg << "Created entity [" << entity << "] named [" << desiredName << "]" << std::endl; diff --git a/test/integration/spherical_coordinates.cc b/test/integration/spherical_coordinates.cc index 5890659086..6f50f269bd 100644 --- a/test/integration/spherical_coordinates.cc +++ b/test/integration/spherical_coordinates.cc @@ -273,6 +273,7 @@ TEST_F(SphericalCoordinatesTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(CreateEntity)) Entity modelEntity{kNullEntity}; math::SphericalCoordinates worldLatLon; math::Vector3d modelLatLon; + math::Pose3d modelPose; fixture.OnPostUpdate( [&]( const ignition::gazebo::UpdateInfo &, @@ -291,6 +292,8 @@ TEST_F(SphericalCoordinatesTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(CreateEntity)) auto modelCoord = sphericalCoordinates(modelEntity, _ecm); EXPECT_TRUE(modelCoord); modelLatLon = modelCoord.value(); + + modelPose = worldPose(modelEntity, _ecm); } iterations++; @@ -307,13 +310,21 @@ TEST_F(SphericalCoordinatesTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(CreateEntity)) double desiredLat{-23.0}; double desiredLon{-43.3}; + double desiredHeading{0.2}; + double desiredRoll{0.1}; + double desiredPitch{0.2}; + double desiredYaw{0.3}; msgs::EntityFactory req; req.set_sdf(modelStr); + msgs::Set(req.mutable_pose(), + {0, 0, 0, desiredRoll, desiredPitch, desiredYaw}); + auto scMsg = req.mutable_spherical_coordinates(); scMsg->set_latitude_deg(desiredLat); scMsg->set_longitude_deg(desiredLon); + scMsg->set_heading_deg(IGN_RTOD(desiredHeading)); msgs::Boolean res; bool result; @@ -338,4 +349,7 @@ TEST_F(SphericalCoordinatesTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(CreateEntity)) EXPECT_NE(kNullEntity, modelEntity); EXPECT_NEAR(modelLatLon.X(), desiredLat, 1e-6); EXPECT_NEAR(modelLatLon.Y(), desiredLon, 1e-6); + EXPECT_DOUBLE_EQ(modelPose.Rot().Roll(), desiredRoll); + EXPECT_DOUBLE_EQ(modelPose.Rot().Pitch(), desiredPitch); + EXPECT_DOUBLE_EQ(modelPose.Rot().Yaw(), desiredHeading + desiredYaw); }