Skip to content

Commit 62272db

Browse files
arjo129arjo129
authored
Makes thruster stop when battery runs out. (#1495)
Previously the thruster system would not stop when the battery 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 <[email protected]>
1 parent f3686aa commit 62272db

File tree

4 files changed

+241
-27
lines changed

4 files changed

+241
-27
lines changed

‎src/systems/thruster/Thruster.cc

Lines changed: 58 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
#include <ignition/transport/Node.hh>
2828

2929
#include "ignition/gazebo/components/AngularVelocity.hh"
30+
#include "ignition/gazebo/components/BatterySoC.hh"
3031
#include "ignition/gazebo/components/ChildLinkName.hh"
3132
#include "ignition/gazebo/components/JointAxis.hh"
3233
#include "ignition/gazebo/components/JointVelocityCmd.hh"
@@ -54,6 +55,12 @@ class ignition::gazebo::systems::ThrusterPrivateData
5455
/// \brief Desired propeller angular velocity in rad / s
5556
public: double propellerAngVel = 0.0;
5657

58+
/// \brief Enabled or not
59+
public: bool enabled = true;
60+
61+
/// \brief Model entity
62+
public: ignition::gazebo::Entity modelEntity;
63+
5764
/// \brief The link entity which will spin
5865
public: ignition::gazebo::Entity linkEntity;
5966

@@ -97,13 +104,18 @@ class ignition::gazebo::systems::ThrusterPrivateData
97104
/// \brief Diameter of propeller in m, default: 0.02
98105
public: double propellerDiameter = 0.02;
99106

100-
/// \brief callback for handling thrust update
101-
public: void OnCmdThrust(const ignition::msgs::Double &_msg);
107+
/// \brief Callback for handling thrust update
108+
public: void OnCmdThrust(const msgs::Double &_msg);
102109

103-
/// \brief function which computes angular velocity from thrust
110+
/// \brief Function which computes angular velocity from thrust
104111
/// \param[in] _thrust Thrust in N
105112
/// \return Angular velocity in rad/s
106113
public: double ThrustToAngularVec(double _thrust);
114+
115+
/// \brief Returns a boolean if the battery has sufficient charge to continue
116+
/// \return True if battery is charged, false otherwise. If no battery found,
117+
/// returns true.
118+
public: bool HasSufficientBattery(const EntityComponentManager &_ecm) const;
107119
};
108120

109121
/////////////////////////////////////////////////
@@ -115,13 +127,14 @@ Thruster::Thruster():
115127

116128
/////////////////////////////////////////////////
117129
void Thruster::Configure(
118-
const ignition::gazebo::Entity &_entity,
130+
const Entity &_entity,
119131
const std::shared_ptr<const sdf::Element> &_sdf,
120-
ignition::gazebo::EntityComponentManager &_ecm,
121-
ignition::gazebo::EventManager &/*_eventMgr*/)
132+
EntityComponentManager &_ecm,
133+
EventManager &/*_eventMgr*/)
122134
{
123135
// Create model object, to access convenient functions
124-
auto model = ignition::gazebo::Model(_entity);
136+
this->dataPtr->modelEntity = _entity;
137+
auto model = Model(_entity);
125138
auto modelName = model.Name(_ecm);
126139

127140
// Get namespace
@@ -277,10 +290,10 @@ void Thruster::Configure(
277290
}
278291

279292
/////////////////////////////////////////////////
280-
void ThrusterPrivateData::OnCmdThrust(const ignition::msgs::Double &_msg)
293+
void ThrusterPrivateData::OnCmdThrust(const msgs::Double &_msg)
281294
{
282295
std::lock_guard<std::mutex> lock(mtx);
283-
this->thrust = ignition::math::clamp(ignition::math::fixnan(_msg.data()),
296+
this->thrust = math::clamp(math::fixnan(_msg.data()),
284297
this->cmdMin, this->cmdMax);
285298

286299
// Thrust is proportional to the Rotation Rate squared
@@ -303,6 +316,28 @@ double ThrusterPrivateData::ThrustToAngularVec(double _thrust)
303316
return propAngularVelocity;
304317
}
305318

319+
/////////////////////////////////////////////////
320+
bool ThrusterPrivateData::HasSufficientBattery(
321+
const EntityComponentManager &_ecm) const
322+
{
323+
bool result = true;
324+
_ecm.Each<components::BatterySoC>([&](
325+
const Entity &_entity,
326+
const components::BatterySoC *_data
327+
){
328+
if(_ecm.ParentEntity(_entity) == this->modelEntity)
329+
{
330+
if(_data->Data() <= 0)
331+
{
332+
result = false;
333+
}
334+
}
335+
336+
return true;
337+
});
338+
return result;
339+
}
340+
306341
/////////////////////////////////////////////////
307342
void Thruster::PreUpdate(
308343
const ignition::gazebo::UpdateInfo &_info,
@@ -311,6 +346,11 @@ void Thruster::PreUpdate(
311346
if (_info.paused)
312347
return;
313348

349+
if (!this->dataPtr->enabled)
350+
{
351+
return;
352+
}
353+
314354
ignition::gazebo::Link link(this->dataPtr->linkEntity);
315355

316356
auto pose = worldPose(this->dataPtr->linkEntity, _ecm);
@@ -367,10 +407,18 @@ void Thruster::PreUpdate(
367407
unitVector * torque);
368408
}
369409

410+
/////////////////////////////////////////////////
411+
void Thruster::PostUpdate(const UpdateInfo &/*unused*/,
412+
const EntityComponentManager &_ecm)
413+
{
414+
this->dataPtr->enabled = this->dataPtr->HasSufficientBattery(_ecm);
415+
}
416+
370417
IGNITION_ADD_PLUGIN(
371418
Thruster, System,
372419
Thruster::ISystemConfigure,
373-
Thruster::ISystemPreUpdate)
420+
Thruster::ISystemPreUpdate,
421+
Thruster::ISystemPostUpdate)
374422

375423
IGNITION_ADD_PLUGIN_ALIAS(Thruster, "ignition::gazebo::systems::Thruster")
376424

‎src/systems/thruster/Thruster.hh

Lines changed: 11 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -89,25 +89,30 @@ namespace systems
8989
/// ```
9090
/// The vehicle should move in a circle.
9191
class Thruster:
92-
public ignition::gazebo::System,
93-
public ignition::gazebo::ISystemConfigure,
94-
public ignition::gazebo::ISystemPreUpdate
92+
public System,
93+
public ISystemConfigure,
94+
public ISystemPreUpdate,
95+
public ISystemPostUpdate
9596
{
9697
/// \brief Constructor
9798
public: Thruster();
9899

99100
/// Documentation inherited
100101
public: void Configure(
101-
const ignition::gazebo::Entity &_entity,
102+
const Entity &_entity,
102103
const std::shared_ptr<const sdf::Element> &_sdf,
103-
ignition::gazebo::EntityComponentManager &_ecm,
104-
ignition::gazebo::EventManager &/*_eventMgr*/) override;
104+
EntityComponentManager &_ecm,
105+
EventManager &/*_eventMgr*/) override;
105106

106107
/// Documentation inherited
107108
public: void PreUpdate(
108109
const ignition::gazebo::UpdateInfo &_info,
109110
ignition::gazebo::EntityComponentManager &_ecm) override;
110111

112+
/// Documentation inherited
113+
public: void PostUpdate(const UpdateInfo &_info,
114+
const EntityComponentManager &_ecm);
115+
111116
/// \brief Private data pointer
112117
private: std::unique_ptr<ThrusterPrivateData> dataPtr;
113118
};

‎test/integration/thruster.cc

Lines changed: 39 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -155,17 +155,20 @@ void ThrusterTest::TestWorld(const std::string &_world,
155155
pub.Publish(msg);
156156

157157
// Check movement
158-
for (sleep = 0; modelPoses.back().Pos().X() < 5.0 && sleep < maxSleep;
159-
++sleep)
158+
if (_namespace != "lowbattery")
160159
{
161-
std::this_thread::sleep_for(std::chrono::milliseconds(100));
162-
fixture.Server()->Run(true, 100, false);
163-
}
164-
EXPECT_LT(sleep, maxSleep);
165-
EXPECT_LT(5.0, modelPoses.back().Pos().X());
160+
for (sleep = 0; modelPoses.back().Pos().X() < 5.0 && sleep < maxSleep;
161+
++sleep)
162+
{
163+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
164+
fixture.Server()->Run(true, 100, false);
165+
}
166+
EXPECT_LT(sleep, maxSleep);
167+
EXPECT_LT(5.0, modelPoses.back().Pos().X());
166168

167-
EXPECT_EQ(100u * sleep, modelPoses.size());
168-
EXPECT_EQ(100u * sleep, propellerAngVels.size());
169+
EXPECT_EQ(100u * sleep, modelPoses.size());
170+
EXPECT_EQ(100u * sleep, propellerAngVels.size());
171+
}
169172

170173
// max allowed force
171174
double force{300.0};
@@ -178,6 +181,14 @@ void ThrusterTest::TestWorld(const std::string &_world,
178181
double xTol{1e-2};
179182
for (unsigned int i = 0; i < modelPoses.size(); ++i)
180183
{
184+
if (_namespace == "lowbattery" && i > 545)
185+
{
186+
// Battery discharged should not accelerate
187+
EXPECT_NEAR(modelPoses[i-1].Pos().X() - modelPoses[i-2].Pos().X(),
188+
modelPoses[i].Pos().X() - modelPoses[i-1].Pos().X(), 1e-6);
189+
continue;
190+
}
191+
181192
auto pose = modelPoses[i];
182193
auto time = dt * i;
183194
EXPECT_NEAR(force * time * time / (2 * mass), pose.Pos().X(), xTol);
@@ -188,7 +199,7 @@ void ThrusterTest::TestWorld(const std::string &_world,
188199

189200
// The joint velocity command adds some roll to the body which the PID
190201
// wrench doesn't
191-
if (_namespace == "custom")
202+
if (_namespace == "custom" || _namespace == "lowbattery")
192203
EXPECT_NEAR(0.0, pose.Rot().Roll(), 0.1);
193204
else
194205
EXPECT_NEAR(0.0, pose.Rot().Roll(), _baseTol);
@@ -205,7 +216,14 @@ void ThrusterTest::TestWorld(const std::string &_world,
205216
// It takes a few iterations to reach the speed
206217
if (i > 25)
207218
{
208-
EXPECT_NEAR(omega, angVel.X(), omegaTol) << i;
219+
if (_namespace == "lowbattery" && i > 545)
220+
{
221+
EXPECT_NEAR(0.0, angVel.X(), _baseTol);
222+
}
223+
else
224+
{
225+
EXPECT_NEAR(omega, angVel.X(), omegaTol) << i;
226+
}
209227
}
210228
EXPECT_NEAR(0.0, angVel.Y(), _baseTol);
211229
EXPECT_NEAR(0.0, angVel.Z(), _baseTol);
@@ -234,3 +252,13 @@ TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(VelocityControl))
234252
this->TestWorld(world, "custom", 0.005, 950, 0.25, 1e-2);
235253
}
236254

255+
/////////////////////////////////////////////////
256+
TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(BatteryIntegration))
257+
{
258+
auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
259+
"test", "worlds", "thruster_battery.sdf");
260+
261+
// Tolerance is high because the joint command disturbs the vehicle body
262+
this->TestWorld(world, "lowbattery", 0.005, 950, 0.25, 1e-2);
263+
}
264+

‎test/worlds/thruster_battery.sdf

Lines changed: 133 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,133 @@
1+
<?xml version="1.0" ?>
2+
<sdf version="1.6">
3+
<world name="thruster">
4+
5+
<physics name="fast" type="none">
6+
<!-- Zero to run as fast as possible -->
7+
<real_time_factor>0</real_time_factor>
8+
</physics>
9+
10+
<!-- prevent sinking -->
11+
<gravity>0 0 0</gravity>
12+
13+
<plugin
14+
filename="ignition-gazebo-physics-system"
15+
name="ignition::gazebo::systems::Physics">
16+
</plugin>
17+
<plugin
18+
filename="ignition-gazebo-scene-broadcaster-system"
19+
name="ignition::gazebo::systems::SceneBroadcaster">
20+
</plugin>
21+
22+
<light type="directional" name="sun">
23+
<cast_shadows>true</cast_shadows>
24+
<pose>0 0 10 0 0 0</pose>
25+
<diffuse>1 1 1 1</diffuse>
26+
<specular>0.5 0.5 0.5 1</specular>
27+
<attenuation>
28+
<range>1000</range>
29+
<constant>0.9</constant>
30+
<linear>0.01</linear>
31+
<quadratic>0.001</quadratic>
32+
</attenuation>
33+
<direction>-0.5 0.1 -0.9</direction>
34+
</light>
35+
36+
<model name="sub">
37+
38+
<link name="body">
39+
<pose>0 0 0 0 1.57 0</pose>
40+
<inertial>
41+
<mass>100</mass>
42+
<inertia>
43+
<ixx>33.89</ixx>
44+
<ixy>0</ixy>
45+
<ixz>0</ixz>
46+
<iyy>33.89</iyy>
47+
<iyz>0</iyz>
48+
<izz>1.125</izz>
49+
</inertia>
50+
</inertial>
51+
<visual name="visual">
52+
<geometry>
53+
<cylinder>
54+
<length>2</length>
55+
<radius>0.15</radius>
56+
</cylinder>
57+
</geometry>
58+
</visual>
59+
</link>
60+
61+
<link name="propeller">
62+
<pose>-1.05 0 0 0 0 0</pose>
63+
<inertial>
64+
<mass>0.1</mass>
65+
<inertia>
66+
<ixx>0.000354167</ixx>
67+
<ixy>0</ixy>
68+
<ixz>0</ixz>
69+
<iyy>0.000021667</iyy>
70+
<iyz>0</iyz>
71+
<izz>0.000334167</izz>
72+
</inertia>
73+
</inertial>
74+
<visual name="visual">
75+
<geometry>
76+
<box>
77+
<size>0.01 0.25 0.05</size>
78+
</box>
79+
</geometry>
80+
</visual>
81+
</link>
82+
83+
<joint name="propeller_joint" type="revolute">
84+
<parent>body</parent>
85+
<child>propeller</child>
86+
<axis>
87+
<xyz>1 0 0</xyz>
88+
<limit>
89+
<lower>-1e+12</lower>
90+
<upper>1e+12</upper>
91+
<effort>1e6</effort>
92+
<velocity>1e6</velocity>
93+
</limit>
94+
</axis>
95+
</joint>
96+
97+
<plugin
98+
filename="ignition-gazebo-thruster-system"
99+
name="ignition::gazebo::systems::Thruster">
100+
<namespace>lowbattery</namespace>
101+
<joint_name>propeller_joint</joint_name>
102+
<thrust_coefficient>0.005</thrust_coefficient>
103+
<fluid_density>950</fluid_density>
104+
<propeller_diameter>0.25</propeller_diameter>
105+
<velocity_control>true</velocity_control>
106+
<max_thrust_cmd>300</max_thrust_cmd>
107+
<min_thrust_cmd>0</min_thrust_cmd>
108+
</plugin>
109+
110+
<plugin filename="ignition-gazebo-linearbatteryplugin-system"
111+
name="ignition::gazebo::systems::LinearBatteryPlugin">
112+
<battery_name>linear_battery</battery_name>
113+
<voltage>14.4</voltage>
114+
<open_circuit_voltage_constant_coef>14.4</open_circuit_voltage_constant_coef>
115+
<fix_issue_225>true</fix_issue_225>
116+
<!-- Initial charge in Ah -->
117+
<initial_charge>0.00005</initial_charge>
118+
<capacity>400</capacity>
119+
<!-- 190mOhm / 62 -->
120+
<resistance>0.003064</resistance>
121+
<!-- Power consumption: 14.4V * 2 A -->
122+
<power_load>28.8</power_load>
123+
<!-- <power_draining_topic>/battery/discharge</power_draining_topic> -->
124+
<!-- Recharging the battery -->
125+
<!-- Charging time: 400/( (15 + 4) * 0.5 ) = 42.1 -->
126+
<enable_recharge>true</enable_recharge>
127+
<recharge_by_topic>true</recharge_by_topic>
128+
<charging_time>42.1</charging_time>
129+
</plugin>
130+
</model>
131+
132+
</world>
133+
</sdf>

0 commit comments

Comments
 (0)