From dd3e241fa431f45819a3b74e8dd5fe6b060b002f Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Tue, 17 Dec 2024 17:52:13 +0900 Subject: [PATCH 1/2] Add parameter for adjust current sign in battery plugin (#2696) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Tatsuro Sakaguchi Co-authored-by: Alejandro Hernández Cordero --- src/systems/battery_plugin/LinearBatteryPlugin.cc | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index 0f84e1423a..d4adcbd9f7 100644 --- 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("fix_issue_225"); + if (_sdf->HasElement("invert_current_sign")) + this->dataPtr->invertCurrentSign = + _sdf->Get("invert_current_sign"); + if (_sdf->HasElement("battery_name") && _sdf->HasElement("voltage")) { this->dataPtr->batteryName = _sdf->Get("battery_name"); @@ -624,7 +631,10 @@ void LinearBatteryPlugin::PostUpdate(const UpdateInfo &_info, msg.mutable_header()->mutable_stamp()->CopyFrom( convert(_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); From 9d6230a562c5d463306070b07269b135f9ecd134 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 18 Dec 2024 02:22:39 +0800 Subject: [PATCH 2/2] Fix publishing only top level model pose in pose publisher (#2697) Signed-off-by: Ian Chen --- src/systems/pose_publisher/PosePublisher.cc | 11 ++--- test/integration/pose_publisher_system.cc | 46 +++++++++++++++++++++ test/worlds/pose_publisher.sdf | 15 +++++++ 3 files changed, 65 insertions(+), 7 deletions(-) diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index 395f3dc9bf..4eda6045c8 100644 --- 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("publish_model_pose", this->dataPtr->publishNestedModelPose).first; @@ -394,9 +396,6 @@ 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) @@ -404,10 +403,8 @@ void PosePublisherPrivate::InitializeEntitiesToPublish( auto nestedModel = _ecm.Component(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 index 6470c4fbe0..0be9b1c062 100644 --- 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 index a30fb87a6b..904091cad4 100644 --- a/test/worlds/pose_publisher.sdf +++ b/test/worlds/pose_publisher.sdf @@ -514,5 +514,20 @@ + + 5 5 0 0 0 0 + + + false + false + false + false + false + true + + +