Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Makes thruster stop when battery runs out. #1495

Merged
merged 4 commits into from
May 20, 2022
Merged

Makes thruster stop when battery runs out. #1495

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
f5249f0
Makes thruster depend on battery
arjo129 May 18, 2022
5519232
codecheck
arjo129 May 18, 2022
62f2eb5
PR Feedback
arjo129 May 19, 2022
f3adc7c
Merge branch 'ign-gazebo6' into arjo/fix/thruster_obey_battery
arjo129 May 20, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
68 changes: 58 additions & 10 deletions src/systems/thruster/Thruster.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
#include <ignition/transport/Node.hh>

#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"
Expand Down Expand Up @@ -54,6 +55,12 @@ class ignition::gazebo::systems::ThrusterPrivateData
/// \brief Desired propeller angular velocity in rad / s
public: double propellerAngVel = 0.0;

/// \brief Enabled or 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;

Expand Down Expand Up @@ -97,13 +104,18 @@ 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,
/// returns true.
public: bool HasSufficientBattery(const EntityComponentManager &_ecm) const;
};

/////////////////////////////////////////////////
Expand All @@ -115,13 +127,14 @@ Thruster::Thruster():

/////////////////////////////////////////////////
void Thruster::Configure(
const ignition::gazebo::Entity &_entity,
const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_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
Expand Down Expand Up @@ -277,10 +290,10 @@ void Thruster::Configure(
}

/////////////////////////////////////////////////
void ThrusterPrivateData::OnCmdThrust(const ignition::msgs::Double &_msg)
void ThrusterPrivateData::OnCmdThrust(const msgs::Double &_msg)
{
std::lock_guard<std::mutex> 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
Expand All @@ -303,6 +316,28 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust)
return propAngularVelocity;
}

/////////////////////////////////////////////////
bool ThrusterPrivateData::HasSufficientBattery(
const EntityComponentManager &_ecm) const
{
bool result = true;
_ecm.Each<components::BatterySoC>([&](
const Entity &_entity,
const components::BatterySoC *_data
){
if(_ecm.ParentEntity(_entity) == this->modelEntity)
{
if(_data->Data() <= 0)
{
result = false;
}
}

return true;
});
return result;
}

/////////////////////////////////////////////////
void Thruster::PreUpdate(
const ignition::gazebo::UpdateInfo &_info,
Expand All @@ -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);
Expand Down Expand Up @@ -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")

17 changes: 11 additions & 6 deletions src/systems/thruster/Thruster.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<const sdf::Element> &_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<ThrusterPrivateData> dataPtr;
};
Expand Down
50 changes: 39 additions & 11 deletions test/integration/thruster.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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);
}

133 changes: 133 additions & 0 deletions test/worlds/thruster_battery.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,133 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="thruster">

<physics name="fast" type="none">
<!-- Zero to run as fast as possible -->
<real_time_factor>0</real_time_factor>
</physics>

<!-- prevent sinking -->
<gravity>0 0 0</gravity>

<plugin
filename="ignition-gazebo-physics-system"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="ignition-gazebo-scene-broadcaster-system"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>1 1 1 1</diffuse>
<specular>0.5 0.5 0.5 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>

<model name="sub">

<link name="body">
<pose>0 0 0 0 1.57 0</pose>
<inertial>
<mass>100</mass>
<inertia>
<ixx>33.89</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>33.89</iyy>
<iyz>0</iyz>
<izz>1.125</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<cylinder>
<length>2</length>
<radius>0.15</radius>
</cylinder>
</geometry>
</visual>
</link>

<link name="propeller">
<pose>-1.05 0 0 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.000354167</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.000021667</iyy>
<iyz>0</iyz>
<izz>0.000334167</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>0.01 0.25 0.05</size>
</box>
</geometry>
</visual>
</link>

<joint name="propeller_joint" type="revolute">
<parent>body</parent>
<child>propeller</child>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-1e+12</lower>
<upper>1e+12</upper>
<effort>1e6</effort>
<velocity>1e6</velocity>
</limit>
</axis>
</joint>

<plugin
filename="ignition-gazebo-thruster-system"
name="ignition::gazebo::systems::Thruster">
<namespace>lowbattery</namespace>
<joint_name>propeller_joint</joint_name>
<thrust_coefficient>0.005</thrust_coefficient>
<fluid_density>950</fluid_density>
<propeller_diameter>0.25</propeller_diameter>
<velocity_control>true</velocity_control>
<max_thrust_cmd>300</max_thrust_cmd>
<min_thrust_cmd>0</min_thrust_cmd>
</plugin>

<plugin filename="ignition-gazebo-linearbatteryplugin-system"
name="ignition::gazebo::systems::LinearBatteryPlugin">
<battery_name>linear_battery</battery_name>
<voltage>14.4</voltage>
<open_circuit_voltage_constant_coef>14.4</open_circuit_voltage_constant_coef>
<fix_issue_225>true</fix_issue_225>
<!-- Initial charge in Ah -->
<initial_charge>0.00005</initial_charge>
<capacity>400</capacity>
<!-- 190mOhm / 62 -->
<resistance>0.003064</resistance>
<!-- Power consumption: 14.4V * 2 A -->
<power_load>28.8</power_load>
<!-- <power_draining_topic>/battery/discharge</power_draining_topic> -->
<!-- Recharging the battery -->
<!-- Charging time: 400/( (15 + 4) * 0.5 ) = 42.1 -->
<enable_recharge>true</enable_recharge>
<recharge_by_topic>true</recharge_by_topic>
<charging_time>42.1</charging_time>
</plugin>
</model>

</world>
</sdf>