Merge branch 'gz-sim9' into dockerbuildfix

gazebosim · Dec 22, 2024 · 3babb7c · 3babb7c
2 parents 91acee9 + 9d6230a
commit 3babb7c
Show file tree

Hide file tree

Showing 4 changed files with 76 additions and 8 deletions.
diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc
@@ -178,6 +178,9 @@ class gz::sim::systems::LinearBatteryPluginPrivate
 
   /// \brief Initial power load set trough config
   public: double initialPowerLoad = 0.0;
+
+  /// \brief Flag to invert the current sign
+  public: bool invertCurrentSign{false};
 };
 
 /////////////////////////////////////////////////
@@ -273,6 +276,10 @@ void LinearBatteryPlugin::Configure(const Entity &_entity,
   if (_sdf->HasElement("fix_issue_225"))
     this->dataPtr->fixIssue225 = _sdf->Get<bool>("fix_issue_225");
 
+  if (_sdf->HasElement("invert_current_sign"))
+    this->dataPtr->invertCurrentSign =
+      _sdf->Get<bool>("invert_current_sign");
+
   if (_sdf->HasElement("battery_name") && _sdf->HasElement("voltage"))
   {
     this->dataPtr->batteryName = _sdf->Get<std::string>("battery_name");
@@ -624,7 +631,10 @@ void LinearBatteryPlugin::PostUpdate(const UpdateInfo &_info,
   msg.mutable_header()->mutable_stamp()->CopyFrom(
       convert<msgs::Time>(_info.simTime));
   msg.set_voltage(this->dataPtr->battery->Voltage());
-  msg.set_current(this->dataPtr->ismooth);
+  if (this->dataPtr->invertCurrentSign)
+    msg.set_current(-this->dataPtr->ismooth);
+  else
+    msg.set_current(this->dataPtr->ismooth);
   msg.set_charge(this->dataPtr->q);
   msg.set_capacity(this->dataPtr->c);
 

diff --git a/src/systems/pose_publisher/PosePublisher.cc b/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;
       }
     }
 

diff --git a/test/integration/pose_publisher_system.cc b/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);
+  }
 }
diff --git a/test/worlds/pose_publisher.sdf b/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>