Skip to content

Commit 9d6230a

Browse files
authored
Fix publishing only top level model pose in pose publisher (gazebosim#2697)
Signed-off-by: Ian Chen <ichen@openrobotics.org>
1 parent dd3e241 commit 9d6230a

File tree

3 files changed

+65
-7
lines changed

3 files changed

+65
-7
lines changed

src/systems/pose_publisher/PosePublisher.cc

+4-7
Original file line numberDiff line numberDiff line change
@@ -204,6 +204,8 @@ void PosePublisher::Configure(const Entity &_entity,
204204

205205
// for backward compatibility, publish_model_pose will be set to the
206206
// same value as publish_nested_model_pose if it is not specified.
207+
// todo(iche033) Remove backward compatibility and decouple model and
208+
// nested model pose parameter value in gz-sim10
207209
this->dataPtr->publishModelPose =
208210
_sdf->Get<bool>("publish_model_pose",
209211
this->dataPtr->publishNestedModelPose).first;
@@ -394,20 +396,15 @@ void PosePublisherPrivate::InitializeEntitiesToPublish(
394396
(collision && this->publishCollisionPose) ||
395397
(sensor && this->publishSensorPose);
396398

397-
// for backward compatibility, top level model pose will be published
398-
// if publishNestedModelPose is set to true unless the user explicity
399-
// disables this by setting publishModelPose to false
400399
if (isModel)
401400
{
402401
if (parent)
403402
{
404403
auto nestedModel = _ecm.Component<components::Model>(parent->Data());
405404
if (nestedModel)
406405
fillPose = this->publishNestedModelPose;
407-
}
408-
if (!fillPose)
409-
{
410-
fillPose = this->publishNestedModelPose && this->publishModelPose;
406+
else
407+
fillPose = this->publishModelPose;
411408
}
412409
}
413410

test/integration/pose_publisher_system.cc

+46
Original file line numberDiff line numberDiff line change
@@ -761,5 +761,51 @@ TEST_F(PosePublisherTest,
761761
auto p = msgs::Convert(poseMsgs[0]);
762762
EXPECT_EQ(expectedEntityPose, p);
763763
}
764+
}
765+
766+
/////////////////////////////////////////////////
767+
TEST_F(PosePublisherTest,
768+
GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelPoseOnly))
769+
{
770+
// Start server
771+
ServerConfig serverConfig;
772+
serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) +
773+
"/test/worlds/pose_publisher.sdf");
774+
775+
Server server(serverConfig);
776+
EXPECT_FALSE(server.Running());
777+
EXPECT_FALSE(*server.Running(0));
778+
779+
poseMsgs.clear();
780+
781+
// subscribe to the pose publisher
782+
transport::Node node;
783+
node.Subscribe(std::string("/model/test_publish_only_model_pose/pose"),
784+
&poseCb);
785+
786+
// Run server
787+
unsigned int iters = 1000u;
788+
server.Run(true, iters, false);
789+
790+
// Wait for messages to be received
791+
int sleep = 0;
792+
while (poseMsgs.empty() && sleep++ < 30)
793+
{
794+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
795+
}
796+
797+
EXPECT_TRUE(!poseMsgs.empty());
764798

799+
// only the pose of the model should be published and no other entity
800+
std::string expectedEntityName = "test_publish_only_model_pose";
801+
math::Pose3d expectedEntityPose(5, 5, 0, 0, 0, 0);
802+
for (auto &msg : poseMsgs)
803+
{
804+
ASSERT_LT(1, msg.header().data_size());
805+
ASSERT_LT(0, msg.header().data(1).value_size());
806+
std::string childFrameId = msg.header().data(1).value(0);
807+
EXPECT_EQ(expectedEntityName, childFrameId);
808+
auto p = msgs::Convert(poseMsgs[0]);
809+
EXPECT_EQ(expectedEntityPose, p);
810+
}
765811
}

test/worlds/pose_publisher.sdf

+15
Original file line numberDiff line numberDiff line change
@@ -514,5 +514,20 @@
514514
</plugin>
515515
</model>
516516

517+
<model name="test_publish_only_model_pose">
518+
<pose>5 5 0 0 0 0</pose>
519+
<link name="link1"/>
520+
<plugin
521+
filename="gz-sim-pose-publisher-system"
522+
name="gz::sim::systems::PosePublisher">
523+
<publish_link_pose>false</publish_link_pose>
524+
<publish_sensor_pose>false</publish_sensor_pose>
525+
<publish_collision_pose>false</publish_collision_pose>
526+
<publish_visual_pose>false</publish_visual_pose>
527+
<publish_nested_model_pose>false</publish_nested_model_pose>
528+
<publish_model_pose>true</publish_model_pose>
529+
</plugin>
530+
</model>
531+
517532
</world>
518533
</sdf>

0 commit comments

Comments
 (0)