diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index 8d550f68f67..6152c7d2a03 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -111,6 +111,9 @@ class ignition::gazebo::systems::PosePublisherPrivate /// \brief True to publish nested model pose public: bool publishNestedModelPose = false; + /// \brief True to publish model pose + public: bool publishModelPose = false; + /// \brief Frequency of pose publications in Hz. A negative frequency /// publishes as fast as possible (i.e, at the rate of the simulation step) public: double updateFrequency = -1; @@ -197,6 +200,12 @@ void PosePublisher::Configure(const Entity &_entity, _sdf->Get("publish_nested_model_pose", this->dataPtr->publishNestedModelPose).first; + // for backward compatibility, publish_model_pose will be set to the + // same value as publish_nested_model_pose if it is not specified. + this->dataPtr->publishModelPose = + _sdf->Get("publish_model_pose", + this->dataPtr->publishNestedModelPose).first; + this->dataPtr->publishVisualPose = _sdf->Get("publish_visual_pose", this->dataPtr->publishVisualPose).first; @@ -364,18 +373,36 @@ void PosePublisherPrivate::InitializeEntitiesToPublish( visited.push_back(entity); auto link = _ecm.Component(entity); - auto nestedModel = _ecm.Component(entity); auto visual = _ecm.Component(entity); auto collision = _ecm.Component(entity); auto sensor = _ecm.Component(entity); auto joint = _ecm.Component(entity); + auto isModel = _ecm.Component(entity); + auto parent = _ecm.Component(entity); + bool fillPose = (link && this->publishLinkPose) || - (nestedModel && this->publishNestedModelPose) || (visual && this->publishVisualPose) || (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(parent->Data()); + if (nestedModel) + fillPose = this->publishNestedModelPose; + } + if (!fillPose) + { + fillPose = this->publishNestedModelPose && this->publishModelPose; + } + } + if (fillPose) { std::string frame; @@ -386,7 +413,6 @@ void PosePublisherPrivate::InitializeEntitiesToPublish( childFrame = removeParentScope(scopedName(entity, _ecm, "::", false), "::"); - auto parent = _ecm.Component(entity); if (parent) { auto parentName = _ecm.Component(parent->Data()); diff --git a/src/systems/pose_publisher/PosePublisher.hh b/src/systems/pose_publisher/PosePublisher.hh index abd794ea251..e3797b0bf62 100644 --- a/src/systems/pose_publisher/PosePublisher.hh +++ b/src/systems/pose_publisher/PosePublisher.hh @@ -43,9 +43,11 @@ namespace systems /// publish_visual_pose : Set to true to publish visual pose /// publish_collision_pose : Set to true to publish collision pose /// publish_sensor_pose : Set to true to publish sensor pose + /// publish_model_pose : Set to true to publish model pose. /// publish_nested_model_pose : Set to true to publish nested model pose. The /// pose of the model that contains this system is - /// also published. + /// also published unless publish_model_pose is + /// set to false /// use_pose_vector_msg : Set to true to publish an /// ignition::msgs::Pose_V message instead of /// mulitple ignition::msgs::Pose messages. diff --git a/test/integration/pose_publisher_system.cc b/test/integration/pose_publisher_system.cc index 0a1a19f3d63..ddb4f131e05 100644 --- a/test/integration/pose_publisher_system.cc +++ b/test/integration/pose_publisher_system.cc @@ -742,4 +742,19 @@ TEST_F(PosePublisherTest, } EXPECT_TRUE(!poseMsgs.empty()); + + // only the pose of the nested model should be published and no other entity + std::string expectedEntityName = "model_00::model_01"; + math::Pose3d expectedEntityPose(1, 0, 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); + math::Pose3d pose; + auto p = msgs::Convert(poseMsgs[0]); + EXPECT_EQ(expectedEntityPose, p); + } + } diff --git a/test/worlds/nested_model.sdf b/test/worlds/nested_model.sdf index 06063d67eb1..68ab84077e2 100644 --- a/test/worlds/nested_model.sdf +++ b/test/worlds/nested_model.sdf @@ -63,11 +63,12 @@ - true + false false false false - false + true + false 100