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

Signed-off-by: Ian Chen <ichen@openrobotics.org>

Author: iche033

src/systems/pose_publisher/PosePublisher.cc

@@ -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;
@@ -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;
       }
     }
 

test/integration/pose_publisher_system.cc

@@ -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);
+  }
 }

test/worlds/pose_publisher.sdf

@@ -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>