Skip to content

Commit e382fe3

Browse files
scpetersscpeters
committed
Backport SystemConfigurePriority to harmonic
This is part of #2500 that can be backported without breaking anything. None of the default system priorities are changed to preserve existing behavior. Signed-off-by: Steve Peters <scpeters@openrobotics.org>
1 parent d94c11e commit e382fe3

File tree

4 files changed

+103
-25
lines changed

4 files changed

+103
-25
lines changed

‎include/gz/sim/System.hh

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -124,6 +124,20 @@ namespace gz
124124
EventManager &_eventMgr) = 0;
125125
};
126126

127+
/// \class ISystemConfigure ISystem.hh gz/sim/System.hh
128+
/// \brief Interface for a system that implements optional configuration
129+
/// of the default priority value.
130+
///
131+
/// ConfigurePriority is called before the system is instantiated to
132+
/// override System::kDefaultPriority. It can still be overridden by the
133+
/// XML priority element.
134+
class ISystemConfigurePriority {
135+
/// \brief Configure the default priority of the system, which can still
136+
/// be overridden by the XML priority element.
137+
/// \return The default priority for the system.
138+
public: virtual System::PriorityType ConfigurePriority() = 0;
139+
};
140+
127141
/// \class ISystemConfigureParameters ISystem.hh gz/sim/System.hh
128142
/// \brief Interface for a system that declares parameters.
129143
///

‎src/SystemInternal.hh

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,8 @@ namespace gz
4848
configure(systemPlugin->QueryInterface<ISystemConfigure>()),
4949
configureParameters(
5050
systemPlugin->QueryInterface<ISystemConfigureParameters>()),
51+
configurePriority(
52+
systemPlugin->QueryInterface<ISystemConfigurePriority>()),
5153
reset(systemPlugin->QueryInterface<ISystemReset>()),
5254
preupdate(systemPlugin->QueryInterface<ISystemPreUpdate>()),
5355
update(systemPlugin->QueryInterface<ISystemUpdate>()),
@@ -66,6 +68,8 @@ namespace gz
6668
configure(dynamic_cast<ISystemConfigure *>(_system.get())),
6769
configureParameters(
6870
dynamic_cast<ISystemConfigureParameters *>(_system.get())),
71+
configurePriority(
72+
dynamic_cast<ISystemConfigurePriority *>(_system.get())),
6973
reset(dynamic_cast<ISystemReset *>(_system.get())),
7074
preupdate(dynamic_cast<ISystemPreUpdate *>(_system.get())),
7175
update(dynamic_cast<ISystemUpdate *>(_system.get())),
@@ -95,6 +99,11 @@ namespace gz
9599
/// Will be nullptr if the System doesn't implement this interface.
96100
public: ISystemConfigureParameters *configureParameters = nullptr;
97101

102+
/// \brief Access this system via the ISystemConfigurePriority
103+
/// interface.
104+
/// Will be nullptr if the System doesn't implement this interface.
105+
public: ISystemConfigurePriority *configurePriority = nullptr;
106+
98107
/// \brief Access this system via the ISystemReset interface
99108
/// Will be nullptr if the System doesn't implement this interface.
100109
public: ISystemReset *reset = nullptr;

‎src/SystemManager.cc

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -113,13 +113,20 @@ size_t SystemManager::ActivatePendingSystems()
113113
this->systems.push_back(system);
114114

115115
PriorityType p {System::kDefaultPriority};
116+
if (system.configurePriority)
117+
{
118+
p = system.configurePriority->ConfigurePriority();
119+
}
116120
const std::string kPriorityElementName
117121
{gz::sim::System::kPriorityElementName};
118122
if (system.configureSdf &&
119123
system.configureSdf->HasElement(kPriorityElementName))
120124
{
121125
PriorityType newPriority =
122126
system.configureSdf->Get<PriorityType>(kPriorityElementName);
127+
gzdbg << "Changing priority for system [" << system.name
128+
<< "] from {" << p
129+
<< "} to {" << newPriority << "}\n";
123130
p = newPriority;
124131
}
125132

‎src/SystemManager_TEST.cc

Lines changed: 73 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,8 @@
2828

2929
using namespace gz::sim;
3030

31+
constexpr System::PriorityType kPriority = 64;
32+
3133
/////////////////////////////////////////////////
3234
class SystemWithConfigure:
3335
public System,
@@ -69,6 +71,16 @@ class SystemWithUpdates:
6971
const EntityComponentManager &) override {};
7072
};
7173

74+
/////////////////////////////////////////////////
75+
class SystemWithPrioritizedUpdates:
76+
public SystemWithUpdates,
77+
public ISystemConfigurePriority
78+
{
79+
// Documentation inherited
80+
public: System::PriorityType ConfigurePriority() override
81+
{ return kPriority; }
82+
};
83+
7284
/////////////////////////////////////////////////
7385
TEST(SystemManager, Constructor)
7486
{
@@ -127,32 +139,40 @@ TEST(SystemManager, AddSystemNoEcm)
127139
EXPECT_EQ(1u, systemMgr.TotalByEntity(configEntity).size());
128140

129141
auto updateSystem = std::make_shared<SystemWithUpdates>();
142+
auto prioritizedSystem =
143+
std::make_shared<SystemWithPrioritizedUpdates>();
130144
Entity updateEntity{456u};
131145
systemMgr.AddSystem(updateSystem, updateEntity, nullptr);
146+
systemMgr.AddSystem(prioritizedSystem, updateEntity, nullptr);
132147
EXPECT_EQ(1u, systemMgr.ActiveCount());
133-
EXPECT_EQ(1u, systemMgr.PendingCount());
134-
EXPECT_EQ(2u, systemMgr.TotalCount());
148+
EXPECT_EQ(2u, systemMgr.PendingCount());
149+
EXPECT_EQ(3u, systemMgr.TotalCount());
135150
EXPECT_EQ(1u, systemMgr.SystemsConfigure().size());
136151
EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size());
137152
EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
138153
EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
139-
EXPECT_EQ(1u, systemMgr.TotalByEntity(updateEntity).size());
154+
EXPECT_EQ(2u, systemMgr.TotalByEntity(updateEntity).size());
140155

141156
systemMgr.ActivatePendingSystems();
142-
EXPECT_EQ(2u, systemMgr.ActiveCount());
157+
EXPECT_EQ(3u, systemMgr.ActiveCount());
143158
EXPECT_EQ(0u, systemMgr.PendingCount());
144-
EXPECT_EQ(2u, systemMgr.TotalCount());
159+
EXPECT_EQ(3u, systemMgr.TotalCount());
145160
EXPECT_EQ(1u, systemMgr.SystemsConfigure().size());
146-
// Expect PreUpdate and Update to contain one map entry with Priority 0 and
147-
// a vector of length 1.
148-
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size());
161+
// Expect PreUpdate and Update to contain two map entries:
162+
// 1. Priority {0} and a vector of length 1.
163+
// 2. Priority {kPriority} and a vector of length 1.
164+
EXPECT_EQ(2u, systemMgr.SystemsPreUpdate().size());
149165
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().count(0));
166+
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().count(kPriority));
150167
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().at(0).size());
151-
EXPECT_EQ(1u, systemMgr.SystemsUpdate().size());
168+
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().at(kPriority).size());
169+
EXPECT_EQ(2u, systemMgr.SystemsUpdate().size());
152170
EXPECT_EQ(1u, systemMgr.SystemsUpdate().count(0));
171+
EXPECT_EQ(1u, systemMgr.SystemsUpdate().count(kPriority));
153172
EXPECT_EQ(1u, systemMgr.SystemsUpdate().at(0).size());
154-
EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size());
155-
EXPECT_EQ(1u, systemMgr.TotalByEntity(updateEntity).size());
173+
EXPECT_EQ(1u, systemMgr.SystemsUpdate().at(kPriority).size());
174+
EXPECT_EQ(2u, systemMgr.SystemsPostUpdate().size());
175+
EXPECT_EQ(2u, systemMgr.TotalByEntity(updateEntity).size());
156176
}
157177

158178
/////////////////////////////////////////////////
@@ -201,23 +221,37 @@ TEST(SystemManager, AddSystemEcm)
201221
EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
202222

203223
auto updateSystem = std::make_shared<SystemWithUpdates>();
224+
auto prioritizedSystem =
225+
std::make_shared<SystemWithPrioritizedUpdates>();
204226
systemMgr.AddSystem(updateSystem, kNullEntity, nullptr);
227+
systemMgr.AddSystem(prioritizedSystem, kNullEntity, nullptr);
205228
EXPECT_EQ(1u, systemMgr.ActiveCount());
206-
EXPECT_EQ(1u, systemMgr.PendingCount());
207-
EXPECT_EQ(2u, systemMgr.TotalCount());
229+
EXPECT_EQ(2u, systemMgr.PendingCount());
230+
EXPECT_EQ(3u, systemMgr.TotalCount());
208231
EXPECT_EQ(1u, systemMgr.SystemsConfigure().size());
209232
EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size());
210233
EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
211234
EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
212235

213236
systemMgr.ActivatePendingSystems();
214-
EXPECT_EQ(2u, systemMgr.ActiveCount());
237+
EXPECT_EQ(3u, systemMgr.ActiveCount());
215238
EXPECT_EQ(0u, systemMgr.PendingCount());
216-
EXPECT_EQ(2u, systemMgr.TotalCount());
239+
EXPECT_EQ(3u, systemMgr.TotalCount());
217240
EXPECT_EQ(1u, systemMgr.SystemsConfigure().size());
218-
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size());
219-
EXPECT_EQ(1u, systemMgr.SystemsUpdate().size());
220-
EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size());
241+
// Expect PreUpdate and Update to contain two map entries:
242+
// 1. Priority {0} and a vector of length 1.
243+
// 2. Priority {kPriority} and a vector of length 1.
244+
EXPECT_EQ(2u, systemMgr.SystemsPreUpdate().size());
245+
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().count(0));
246+
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().count(kPriority));
247+
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().at(0).size());
248+
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().at(kPriority).size());
249+
EXPECT_EQ(2u, systemMgr.SystemsUpdate().size());
250+
EXPECT_EQ(1u, systemMgr.SystemsUpdate().count(0));
251+
EXPECT_EQ(1u, systemMgr.SystemsUpdate().count(kPriority));
252+
EXPECT_EQ(1u, systemMgr.SystemsUpdate().at(0).size());
253+
EXPECT_EQ(1u, systemMgr.SystemsUpdate().at(kPriority).size());
254+
EXPECT_EQ(2u, systemMgr.SystemsPostUpdate().size());
221255
}
222256

223257
/////////////////////////////////////////////////
@@ -247,28 +281,42 @@ TEST(SystemManager, AddAndRemoveSystemEcm)
247281
auto entity = ecm.CreateEntity();
248282

249283
auto updateSystemWithChild = std::make_shared<SystemWithUpdates>();
284+
auto prioritizedSystemWithChild =
285+
std::make_shared<SystemWithPrioritizedUpdates>();
250286
systemMgr.AddSystem(updateSystemWithChild, entity, nullptr);
287+
systemMgr.AddSystem(prioritizedSystemWithChild, entity, nullptr);
251288

252289
// Configure called during AddSystem
253290
EXPECT_EQ(1, configSystem->configured);
254291
EXPECT_EQ(1, configSystem->configuredParameters);
255292

256293
EXPECT_EQ(0u, systemMgr.ActiveCount());
257-
EXPECT_EQ(2u, systemMgr.PendingCount());
258-
EXPECT_EQ(2u, systemMgr.TotalCount());
294+
EXPECT_EQ(3u, systemMgr.PendingCount());
295+
EXPECT_EQ(3u, systemMgr.TotalCount());
259296
EXPECT_EQ(0u, systemMgr.SystemsConfigure().size());
260297
EXPECT_EQ(0u, systemMgr.SystemsPreUpdate().size());
261298
EXPECT_EQ(0u, systemMgr.SystemsUpdate().size());
262299
EXPECT_EQ(0u, systemMgr.SystemsPostUpdate().size());
263300

264301
systemMgr.ActivatePendingSystems();
265-
EXPECT_EQ(2u, systemMgr.ActiveCount());
302+
EXPECT_EQ(3u, systemMgr.ActiveCount());
266303
EXPECT_EQ(0u, systemMgr.PendingCount());
267-
EXPECT_EQ(2u, systemMgr.TotalCount());
304+
EXPECT_EQ(3u, systemMgr.TotalCount());
268305
EXPECT_EQ(1u, systemMgr.SystemsConfigure().size());
269-
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().size());
270-
EXPECT_EQ(1u, systemMgr.SystemsUpdate().size());
271-
EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size());
306+
// Expect PreUpdate and Update to contain two map entries:
307+
// 1. Priority {0} and a vector of length 1.
308+
// 2. Priority {kPriority} and a vector of length 1.
309+
EXPECT_EQ(2u, systemMgr.SystemsPreUpdate().size());
310+
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().count(0));
311+
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().count(kPriority));
312+
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().at(0).size());
313+
EXPECT_EQ(1u, systemMgr.SystemsPreUpdate().at(kPriority).size());
314+
EXPECT_EQ(2u, systemMgr.SystemsUpdate().size());
315+
EXPECT_EQ(1u, systemMgr.SystemsUpdate().count(0));
316+
EXPECT_EQ(1u, systemMgr.SystemsUpdate().count(kPriority));
317+
EXPECT_EQ(1u, systemMgr.SystemsUpdate().at(0).size());
318+
EXPECT_EQ(1u, systemMgr.SystemsUpdate().at(kPriority).size());
319+
EXPECT_EQ(2u, systemMgr.SystemsPostUpdate().size());
272320

273321
// Remove the entity
274322
ecm.RequestRemoveEntity(entity);

0 commit comments

Comments
 (0)