Skip to content

Fix publishing only top level model pose in pose publisher (backport #2697) #2946

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Jun 12, 2025
Merged

Fix publishing only top level model pose in pose publisher (backport #2697) #2946

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 4 additions & 7 deletions src/systems/pose_publisher/PosePublisher.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<bool>("publish_model_pose",
this->dataPtr->publishNestedModelPose).first;
Expand Down Expand Up @@ -394,20 +396,15 @@ 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)
{
auto nestedModel = _ecm.Component<components::Model>(parent->Data());
if (nestedModel)
fillPose = this->publishNestedModelPose;
}
if (!fillPose)
{
fillPose = this->publishNestedModelPose && this->publishModelPose;
else
fillPose = this->publishModelPose;
}
}

Expand Down
46 changes: 46 additions & 0 deletions test/integration/pose_publisher_system.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
15 changes: 15 additions & 0 deletions test/worlds/pose_publisher.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -514,5 +514,20 @@
</plugin>
</model>

<model name="test_publish_only_model_pose">
<pose>5 5 0 0 0 0</pose>
<link name="link1"/>
<plugin
filename="gz-sim-pose-publisher-system"
name="gz::sim::systems::PosePublisher">
<publish_link_pose>false</publish_link_pose>
<publish_sensor_pose>false</publish_sensor_pose>
<publish_collision_pose>false</publish_collision_pose>
<publish_visual_pose>false</publish_visual_pose>
<publish_nested_model_pose>false</publish_nested_model_pose>
<publish_model_pose>true</publish_model_pose>
</plugin>
</model>

</world>
</sdf>
Loading