Skip to content
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

Fix publishing only top level model pose in pose publisher #2697

Merged
merged 3 commits into from
Dec 17, 2024
Merged

Fix publishing only top level model pose in pose publisher #2697

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
da815ba
Support publishing only top level model pose
iche033 Dec 10, 2024
0973ac5
update comment
iche033 Dec 10, 2024
3e86ee3
Merge branch 'gz-sim9' into pose_pub_model
iche033 Dec 13, 2024
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
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So, if I understand this correctly, the truth table is:

publish_model_pose publish_nested_model_pose Top level model is published
T T T
T F T
T X F
F T F
F F F
F X F
X T T (This is what we want to change in gz-sim10?)
X F F
X X F

whereas before, it was

publish_model_pose publish_nested_model_pose Top level model is published
T T T
T F F (This is the only difference)
T X F
F T F
F F F
F X F
X T T
X F F
X X F

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes my interpretation is the same as what's described in the tables above.

The code change in gz-sim10 would be:

   this->dataPtr->publishModelPose =
     _sdf->Get<bool>("publish_model_pose",
-        this->dataPtr->publishNestedModelPose).first;
+        this->dataPtr->publishdModelPose).first;

and the entry in that table would become:

publish_model_pose publish_nested_model_pose Top level model is published
X T F

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