diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index 395f3dc9bf..4eda6045c8 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -204,6 +204,8 @@ void PosePublisher::Configure(const Entity &_entity, // for backward compatibility, publish_model_pose will be set to the // same value as publish_nested_model_pose if it is not specified. + // todo(iche033) Remove backward compatibility and decouple model and + // nested model pose parameter value in gz-sim10 this->dataPtr->publishModelPose = _sdf->Get("publish_model_pose", this->dataPtr->publishNestedModelPose).first; @@ -394,9 +396,6 @@ void PosePublisherPrivate::InitializeEntitiesToPublish( (collision && this->publishCollisionPose) || (sensor && this->publishSensorPose); - // for backward compatibility, top level model pose will be published - // if publishNestedModelPose is set to true unless the user explicity - // disables this by setting publishModelPose to false if (isModel) { if (parent) @@ -404,10 +403,8 @@ void PosePublisherPrivate::InitializeEntitiesToPublish( auto nestedModel = _ecm.Component(parent->Data()); if (nestedModel) fillPose = this->publishNestedModelPose; - } - if (!fillPose) - { - fillPose = this->publishNestedModelPose && this->publishModelPose; + else + fillPose = this->publishModelPose; } } diff --git a/test/integration/pose_publisher_system.cc b/test/integration/pose_publisher_system.cc index 6470c4fbe0..0be9b1c062 100644 --- a/test/integration/pose_publisher_system.cc +++ b/test/integration/pose_publisher_system.cc @@ -761,5 +761,51 @@ TEST_F(PosePublisherTest, auto p = msgs::Convert(poseMsgs[0]); EXPECT_EQ(expectedEntityPose, p); } +} + +///////////////////////////////////////////////// +TEST_F(PosePublisherTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseOnly)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/pose_publisher.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + poseMsgs.clear(); + + // subscribe to the pose publisher + transport::Node node; + node.Subscribe(std::string("/model/test_publish_only_model_pose/pose"), + &poseCb); + + // Run server + unsigned int iters = 1000u; + server.Run(true, iters, false); + + // Wait for messages to be received + int sleep = 0; + while (poseMsgs.empty() && sleep++ < 30) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + EXPECT_TRUE(!poseMsgs.empty()); + // only the pose of the model should be published and no other entity + std::string expectedEntityName = "test_publish_only_model_pose"; + math::Pose3d expectedEntityPose(5, 5, 0, 0, 0, 0); + for (auto &msg : poseMsgs) + { + ASSERT_LT(1, msg.header().data_size()); + ASSERT_LT(0, msg.header().data(1).value_size()); + std::string childFrameId = msg.header().data(1).value(0); + EXPECT_EQ(expectedEntityName, childFrameId); + auto p = msgs::Convert(poseMsgs[0]); + EXPECT_EQ(expectedEntityPose, p); + } } diff --git a/test/worlds/pose_publisher.sdf b/test/worlds/pose_publisher.sdf index a30fb87a6b..904091cad4 100644 --- a/test/worlds/pose_publisher.sdf +++ b/test/worlds/pose_publisher.sdf @@ -514,5 +514,20 @@ + + 5 5 0 0 0 0 + + + false + false + false + false + false + true + + +