Merge f768cda into f7cd5aa

gazebosim · Feb 24, 2022 · 2a56a75 · 2a56a75
2 parents f7cd5aa + f768cda
commit 2a56a75
Show file tree

Hide file tree

Showing 4 changed files with 50 additions and 6 deletions.
diff --git 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<bool>("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<bool>("publish_model_pose",
+        this->dataPtr->publishNestedModelPose).first;
+
   this->dataPtr->publishVisualPose =
     _sdf->Get<bool>("publish_visual_pose",
         this->dataPtr->publishVisualPose).first;
@@ -364,18 +373,36 @@ void PosePublisherPrivate::InitializeEntitiesToPublish(
     visited.push_back(entity);
 
     auto link = _ecm.Component<components::Link>(entity);
-    auto nestedModel = _ecm.Component<components::Model>(entity);
     auto visual = _ecm.Component<components::Visual>(entity);
     auto collision = _ecm.Component<components::Collision>(entity);
     auto sensor = _ecm.Component<components::Sensor>(entity);
     auto joint = _ecm.Component<components::Joint>(entity);
 
+    auto isModel = _ecm.Component<components::Model>(entity);
+    auto parent = _ecm.Component<components::ParentEntity>(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<components::Model>(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<components::ParentEntity>(entity);
       if (parent)
       {
         auto parentName = _ecm.Component<components::Name>(parent->Data());

diff --git 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
@@ -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
@@ -63,11 +63,12 @@
         <plugin
            filename="ignition-gazebo-pose-publisher-system"
            name="ignition::gazebo::systems::PosePublisher">
-          <publish_link_pose>true</publish_link_pose>
+          <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_nested_model_pose>true</publish_nested_model_pose>
+          <publish_model_pose>false</publish_model_pose>
           <update_frequency>100</update_frequency>
         </plugin>
       </model>