From f5249f0319a216b4d01138488dca70071cf84acd Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 18 May 2022 14:46:08 +0800 Subject: [PATCH 1/3] Makes thruster depend on battery Previously the thruster system would not stop when the battery of a system was out. Now if a battery is attached to the model, the thruster will automatically stop once the battery runs out. Signed-off-by: Arjo Chakravarty --- src/systems/thruster/Thruster.cc | 68 +++++++++++++--- src/systems/thruster/Thruster.hh | 17 ++-- test/integration/thruster.cc | 50 +++++++++--- test/worlds/thruster_battery.sdf | 133 +++++++++++++++++++++++++++++++ 4 files changed, 241 insertions(+), 27 deletions(-) create mode 100644 test/worlds/thruster_battery.sdf diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 885f5ca209..ce6fdbe567 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -27,6 +27,7 @@ #include #include "ignition/gazebo/components/AngularVelocity.hh" +#include "ignition/gazebo/components/BatterySoC.hh" #include "ignition/gazebo/components/ChildLinkName.hh" #include "ignition/gazebo/components/JointAxis.hh" #include "ignition/gazebo/components/JointVelocityCmd.hh" @@ -54,6 +55,12 @@ class ignition::gazebo::systems::ThrusterPrivateData /// \brief Desired propeller angular velocity in rad / s public: double propellerAngVel = 0.0; + /// \brief Enabled ofr not + public: bool enabled = true; + + /// \brief Model entity + public: ignition::gazebo::Entity modelEntity; + /// \brief The link entity which will spin public: ignition::gazebo::Entity linkEntity; @@ -97,13 +104,17 @@ class ignition::gazebo::systems::ThrusterPrivateData /// \brief Diameter of propeller in m, default: 0.02 public: double propellerDiameter = 0.02; - /// \brief callback for handling thrust update - public: void OnCmdThrust(const ignition::msgs::Double &_msg); + /// \brief Callback for handling thrust update + public: void OnCmdThrust(const msgs::Double &_msg); - /// \brief function which computes angular velocity from thrust + /// \brief Function which computes angular velocity from thrust /// \param[in] _thrust Thrust in N /// \return Angular velocity in rad/s public: double ThrustToAngularVec(double _thrust); + + /// \brief Returns a boolean if the battery has sufficient charge to continue + /// \return True if battery is charged, false otherwise. If no battery found + public: bool HasSufficientBattery(const EntityComponentManager &_ecm) const; }; ///////////////////////////////////////////////// @@ -115,13 +126,14 @@ Thruster::Thruster(): ///////////////////////////////////////////////// void Thruster::Configure( - const ignition::gazebo::Entity &_entity, + const Entity &_entity, const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &/*_eventMgr*/) + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) { // Create model object, to access convenient functions - auto model = ignition::gazebo::Model(_entity); + this->dataPtr->modelEntity = _entity; + auto model = Model(_entity); auto modelName = model.Name(_ecm); // Get namespace @@ -277,10 +289,10 @@ void Thruster::Configure( } ///////////////////////////////////////////////// -void ThrusterPrivateData::OnCmdThrust(const ignition::msgs::Double &_msg) +void ThrusterPrivateData::OnCmdThrust(const msgs::Double &_msg) { std::lock_guard lock(mtx); - this->thrust = ignition::math::clamp(ignition::math::fixnan(_msg.data()), + this->thrust = math::clamp(math::fixnan(_msg.data()), this->cmdMin, this->cmdMax); // Thrust is proportional to the Rotation Rate squared @@ -303,6 +315,29 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust) return propAngularVelocity; } +///////////////////////////////////////////////// +bool ThrusterPrivateData::HasSufficientBattery( + const EntityComponentManager &_ecm) const +{ + bool result = true; + _ecm.Each([&]( + const Entity &_entity, + const components::BatterySoC* _data + ){ + if(_ecm.ParentEntity(_entity) == this->modelEntity) + { + if(_data->Data() <= 0) + { + result = false; + } + } + + return true; + }); + //igndbg << "Batteries" << batteryEntities.size() << std::endl; + return result; +} + ///////////////////////////////////////////////// void Thruster::PreUpdate( const ignition::gazebo::UpdateInfo &_info, @@ -311,6 +346,11 @@ void Thruster::PreUpdate( if (_info.paused) return; + if (!this->dataPtr->enabled) + { + return; + } + ignition::gazebo::Link link(this->dataPtr->linkEntity); auto pose = worldPose(this->dataPtr->linkEntity, _ecm); @@ -367,10 +407,18 @@ void Thruster::PreUpdate( unitVector * torque); } +///////////////////////////////////////////////// +void Thruster::PostUpdate(const UpdateInfo &/*unused*/, + const EntityComponentManager &_ecm) +{ + this->dataPtr->enabled = this->dataPtr->HasSufficientBattery(_ecm); +} + IGNITION_ADD_PLUGIN( Thruster, System, Thruster::ISystemConfigure, - Thruster::ISystemPreUpdate) + Thruster::ISystemPreUpdate, + Thruster::ISystemPostUpdate) IGNITION_ADD_PLUGIN_ALIAS(Thruster, "ignition::gazebo::systems::Thruster") diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 78eafb849b..96b397ea45 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -89,25 +89,30 @@ namespace systems /// ``` /// The vehicle should move in a circle. class Thruster: - public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPreUpdate + public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate { /// \brief Constructor public: Thruster(); /// Documentation inherited public: void Configure( - const ignition::gazebo::Entity &_entity, + const Entity &_entity, const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &/*_eventMgr*/) override; + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) override; /// Documentation inherited public: void PreUpdate( const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) override; + /// Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm); + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index d2333bc3f1..1d28df8d5d 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -155,17 +155,20 @@ void ThrusterTest::TestWorld(const std::string &_world, pub.Publish(msg); // Check movement - for (sleep = 0; modelPoses.back().Pos().X() < 5.0 && sleep < maxSleep; - ++sleep) + if (_namespace != "lowbattery") { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - fixture.Server()->Run(true, 100, false); - } - EXPECT_LT(sleep, maxSleep); - EXPECT_LT(5.0, modelPoses.back().Pos().X()); + for (sleep = 0; modelPoses.back().Pos().X() < 5.0 && sleep < maxSleep; + ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + fixture.Server()->Run(true, 100, false); + } + EXPECT_LT(sleep, maxSleep); + EXPECT_LT(5.0, modelPoses.back().Pos().X()); - EXPECT_EQ(100u * sleep, modelPoses.size()); - EXPECT_EQ(100u * sleep, propellerAngVels.size()); + EXPECT_EQ(100u * sleep, modelPoses.size()); + EXPECT_EQ(100u * sleep, propellerAngVels.size()); + } // max allowed force double force{300.0}; @@ -178,6 +181,14 @@ void ThrusterTest::TestWorld(const std::string &_world, double xTol{1e-2}; for (unsigned int i = 0; i < modelPoses.size(); ++i) { + if (_namespace == "lowbattery" && i > 545) + { + // Battery discharged should not accelerate + EXPECT_NEAR(modelPoses[i-1].Pos().X() - modelPoses[i-2].Pos().X(), + modelPoses[i].Pos().X() - modelPoses[i-1].Pos().X(), 1e-6); + continue; + } + auto pose = modelPoses[i]; auto time = dt * i; EXPECT_NEAR(force * time * time / (2 * mass), pose.Pos().X(), xTol); @@ -188,7 +199,7 @@ void ThrusterTest::TestWorld(const std::string &_world, // The joint velocity command adds some roll to the body which the PID // wrench doesn't - if (_namespace == "custom") + if (_namespace == "custom" || _namespace == "lowbattery") EXPECT_NEAR(0.0, pose.Rot().Roll(), 0.1); else EXPECT_NEAR(0.0, pose.Rot().Roll(), _baseTol); @@ -205,7 +216,14 @@ void ThrusterTest::TestWorld(const std::string &_world, // It takes a few iterations to reach the speed if (i > 25) { - EXPECT_NEAR(omega, angVel.X(), omegaTol) << i; + if (_namespace == "lowbattery" && i > 545) + { + EXPECT_NEAR(0.0, angVel.X(), _baseTol); + } + else + { + EXPECT_NEAR(omega, angVel.X(), omegaTol) << i; + } } EXPECT_NEAR(0.0, angVel.Y(), _baseTol); EXPECT_NEAR(0.0, angVel.Z(), _baseTol); @@ -234,3 +252,13 @@ TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(VelocityControl)) this->TestWorld(world, "custom", 0.005, 950, 0.25, 1e-2); } +///////////////////////////////////////////////// +TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(BatteryIntegration)) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "thruster_battery.sdf"); + + // Tolerance is high because the joint command disturbs the vehicle body + this->TestWorld(world, "lowbattery", 0.005, 950, 0.25, 1e-2); +} + diff --git a/test/worlds/thruster_battery.sdf b/test/worlds/thruster_battery.sdf new file mode 100644 index 0000000000..3cf5a3495e --- /dev/null +++ b/test/worlds/thruster_battery.sdf @@ -0,0 +1,133 @@ + + + + + + + 0 + + + + 0 0 0 + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + + 0 0 0 0 1.57 0 + + 100 + + 33.89 + 0 + 0 + 33.89 + 0 + 1.125 + + + + + + 2 + 0.15 + + + + + + + -1.05 0 0 0 0 0 + + 0.1 + + 0.000354167 + 0 + 0 + 0.000021667 + 0 + 0.000334167 + + + + + + 0.01 0.25 0.05 + + + + + + + body + propeller + + 1 0 0 + + -1e+12 + 1e+12 + 1e6 + 1e6 + + + + + + lowbattery + propeller_joint + 0.005 + 950 + 0.25 + true + 300 + 0 + + + + linear_battery + 14.4 + 14.4 + true + + 0.00005 + 400 + + 0.003064 + + 28.8 + + + + true + true + 42.1 + + + + + From 5519232c08bd1d08a3ed9dec37e00b84fe399bde Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 18 May 2022 14:59:17 +0800 Subject: [PATCH 2/3] codecheck Signed-off-by: Arjo Chakravarty --- src/systems/thruster/Thruster.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index ce6fdbe567..34ca73158a 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -334,7 +334,6 @@ bool ThrusterPrivateData::HasSufficientBattery( return true; }); - //igndbg << "Batteries" << batteryEntities.size() << std::endl; return result; } From 62f2eb5cad1d11b30784241aad846f0907cab08d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 19 May 2022 09:55:26 +0800 Subject: [PATCH 3/3] PR Feedback Signed-off-by: Arjo Chakravarty --- src/systems/thruster/Thruster.cc | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 34ca73158a..92a74bb3a7 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -55,7 +55,7 @@ class ignition::gazebo::systems::ThrusterPrivateData /// \brief Desired propeller angular velocity in rad / s public: double propellerAngVel = 0.0; - /// \brief Enabled ofr not + /// \brief Enabled or not public: bool enabled = true; /// \brief Model entity @@ -113,7 +113,8 @@ class ignition::gazebo::systems::ThrusterPrivateData public: double ThrustToAngularVec(double _thrust); /// \brief Returns a boolean if the battery has sufficient charge to continue - /// \return True if battery is charged, false otherwise. If no battery found + /// \return True if battery is charged, false otherwise. If no battery found, + /// returns true. public: bool HasSufficientBattery(const EntityComponentManager &_ecm) const; }; @@ -322,7 +323,7 @@ bool ThrusterPrivateData::HasSufficientBattery( bool result = true; _ecm.Each([&]( const Entity &_entity, - const components::BatterySoC* _data + const components::BatterySoC *_data ){ if(_ecm.ParentEntity(_entity) == this->modelEntity) {