Refactor plugins in an ECS based way #114

luca-della-vedova · 2024-01-19T03:52:17Z

New feature implementation

Implemented feature

Based on #113, fixes #95, fixes #9.
Needs open-rmf/rmf_traffic_editor#483

Implementation description

This PR ports the codebase to Harmonic, drops Gazebo Classic support and takes the chance to remove the common / gz split, which allows us to cleanup the codebase a fair bit and take advantage of ECS patterns.
Specifically, when using Gazebo classic, we were forced to adopt the same architecture of one plugin per object, leading to large inefficiencies, for example for each door we would have one state publisher + one request subscriber that would check each incoming request.

This PR refactors plugins to be in an ECS based way. Now doors / lifts have a single simple plugin called register_component that adds a component that contains all their properties.
There is now a single site wide door / lift manager system that iterates through all entities with door / lift components and performs whatever logic is needed.

For a more indepth overview of the architecture, there is a FOSSAsia 2023 talk here.

Furthermore, state updates moved from being continuous to being event based, to reduce traffic flow and improve scalability.
Specifically, we use a new feature in ROS 2 Iron, Matched Callbacks, now:

Whenever a new subscriber is detected, we send the full state of the system, slightly throttled to avoid data floods (one publish per simulation update cycle).
Whenever a change in a door state is detected, we send a message with the new state.
When none of the above is true, no messages are sent, reducing load on downstream subscribers.
State QoS is now reliable with a greater history depth since now we can't really afford to lose messages.

Both these changes should greatly with scaling RMF to larger sites, since they will cut down both computation time and network traffic.

Bonus tasks:

Make lift updates event based as well.
Refactor dispenser to be ECS based and change to event based.
Refactor ingestor to be ECS based and change to event based.
Refactor slotcar to be ECS based.
Remove rmf_robot_sim_common package and unify plugins.

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

Signed-off-by: Arjo Chakravarty <arjoc@google.com>

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

This reverts commit b2ed3f4. Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

luca-della-vedova · 2024-05-08T09:14:27Z

To help speed up development I removed the event based logic in 045ed07, this will have a performance impact but should keep the behavior of the rest of the stack unchanged.

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

mxgrey

I've tested this against 24.04 and rolling, and everything seems to be working.

I left some in-line comments for places where changes are needed, just to prevent segfaults and/or confusing crashes that could conceivably happen in some edge cases. If I find an opportunity, I'll make the changes myself and push them.

mxgrey · 2024-05-22T01:35:05Z

rmf_building_sim_gz_plugins/src/door.cpp

-      if (!joint)
+    _door_request_sub = _ros_node->create_subscription<DoorRequest>(
+      "door_requests", rclcpp::SystemDefaultsQoS(),
+      [&](DoorRequest::UniquePtr msg)


A capture by reference [&] is never what we want for a lambda that's going into a subscription. This gives the highest likelihood of accidental memory corruption with no benefit.

We should change this to capture-by-copy, and only include the specific items that we need to capture, making sure that we don't capture any raw pointers that could conceivably be destructed while this subscription is still in use.

rmf_building_sim_gz_plugins/src/door.cpp

mxgrey · 2024-05-22T03:39:59Z

rmf_building_sim_gz_plugins/src/door.cpp

    if (info.paused)
      return;

-    double t =
+    const double t =
      (std::chrono::duration_cast<std::chrono::nanoseconds>(info.simTime).


This duration cast doesn't really make sense since it's casting a nanosecond-integer into a nano-second-integer (in other words, doing nothing) and then manually multiplying by the conversion factor to get seconds.

I recommend imitating the way duration cast is done in rmf_traffic or maybe just using rmf_traffic::time::to_seconds directly since rmf_simulation probably has a transitive dependency on it anyway.

mxgrey · 2024-05-22T03:46:02Z

rmf_building_sim_gz_plugins/src/door.cpp

+      const components::Name* name_comp) -> bool
+      {
+        double dt =
+        (std::chrono::duration_cast<std::chrono::nanoseconds>(info.dt).


This duration cast also doesn't do anything helpful.

mxgrey · 2024-05-22T04:21:54Z

rmf_building_sim_gz_plugins/src/lift.cpp

+    const std::string& floor_name) const
+  {
+    std::vector<Entity> doors;
+    for (const auto& door_pair : lift.floors.at(floor_name).doors)


This risks an exception if it gets called for a floor that the lift doesn't serve. I recommend using .find(~) here and checking the result against .end().

mxgrey · 2024-05-22T04:39:02Z

rmf_building_sim_gz_plugins/src/lift.cpp

+
+    _lift_request_sub = _ros_node->create_subscription<LiftRequest>(
+      "lift_requests", reliable_qos,
+      [&](LiftRequest::UniquePtr msg)


We must not use capture-by-reference for subscription lambdas. This should be replaced with explicit capture-by-copy.

mxgrey · 2024-05-22T04:44:24Z

rmf_building_sim_gz_plugins/src/lift.cpp

+    std::unordered_set<Entity> finished_cmds;
+
+    const double dt =
+      (std::chrono::duration_cast<std::chrono::nanoseconds>(info.dt).


We should fix this duration cast as well.

mxgrey · 2024-05-23T00:16:18Z

rmf_building_sim_gz_plugins/src/register_component.cpp

+    const std::shared_ptr<const sdf::Element>& sdf,
+    EntityComponentManager& ecm, EventManager& /*_eventMgr*/) override
+  {
+    if (sdf->HasElement("component"))


Nothing to change here, just a remark: This approach of serializing components in the SDF is probably how SDF should be getting used in general. It would be worth discussing upstream whether the SDF maintainers would consider first-class support for this pattern.

mxgrey · 2024-05-23T00:19:22Z

rmf_robot_sim_gz_plugins/src/slotcar.cpp

@@ -227,10 +192,12 @@ void SlotcarPlugin::init_obstacle_exclusions(EntityComponentManager& ecm)
        {
          c = ::tolower(c);
        });
-        if (n.find("door") != std::string::npos ||
-        n.find("lift") != std::string::npos ||
+        if (ecm.Component<components::Door>(entity) != nullptr ||


This is a much better approach to filtering these entities 👍 👍

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>

mxgrey

I've made the recommended changes. Now it would be good if we can get CI working before merging, but if that's blocked by upstream issues that won't be resolved soon then I suppose we can move forward with merging and sort out the CI when things are ready upstream.

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

luca-della-vedova · 2024-05-29T12:21:47Z

Jazzy CI failure is expected until all rmf repos have a Jazzy brach

luca-della-vedova and others added 25 commits December 19, 2022 18:06

WIP component registration

b1520f0

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

WIP directions flipped but doors working

cfcd4d3

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

Doors fully working

51e1fc1

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

Minor cleanup, randomize pub times

62dad3e

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

Cleanup

3587651

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

Switch to position control

2185329

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

Further cleanup and refactor

ae35b0c

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

Final door cleanup

9006188

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

WIP skeleton for lifts

6077942

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

WIP lift state publishing

413a260

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

Refactor door to keep track of its state

68f1c0e

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

Finish lift implementation

fc18560

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

WIP add cabin door support, not working yet

288ca64

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

port to harmonic

922a27f

Signed-off-by: Arjo Chakravarty <arjoc@google.com>

Port rmf_robot_sim

2e770ea

Signed-off-by: Arjo Chakravarty <arjoc@google.com>

Bump to harmonic, finish lifts and doors

2fd24be

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Cleanup, fix compile errors

fc75dc7

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Implement payload logic in lift

1c5a898

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Cleanup and performance improvements

638ca8b

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Merge branch 'main' into luca/ecs_plugins_sandbox

2cbd93d

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Refactor crowd sim, remove common package

f99f0b6

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Uncrustify

12f32cf

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Drop TPE support

11ef238

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Make door only publish salient changes

7536796

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Uncrustify

d4444fa

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

This was referenced Jan 19, 2024

Harmonic release and ECS refactor open-rmf/rmf_traffic_editor#483

Merged

Jazzy Jalisco checklist open-rmf/rmf#416

Open

luca-della-vedova added 2 commits January 22, 2024 15:28

Make lift updates event based

c2c06c7

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Remove warning for joint velocity command

ef52004

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

luca-della-vedova mentioned this pull request Jan 23, 2024

Event based infrastructure state update open-rmf/rmf_visualization#68

Draft

luca-della-vedova added 4 commits February 2, 2024 14:32

Revert "Switch to open loop control for lifts"

0d4797b

This reverts commit b2ed3f4. Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Use components for obstacle exclusion

48c1657

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Add response to queried door

f489947

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Fix door ros interface parameters

c2ecedd

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

luca-della-vedova force-pushed the luca/ecs_plugins_sandbox branch from a0d55db to c2ecedd Compare March 15, 2024 09:28

luca-della-vedova added 7 commits March 18, 2024 16:36

Send one lift state update per cycle

0759309

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Move back to position reset for doors

9d4d91f

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Make sure to create AxisAlignedBox component if it cannot be read

0416124

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Merge branch 'main' into luca/ecs_plugins_sandbox

0016f59

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Minor cleanup / ign -> gz

7b99b86

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Move to vendored gz packages

8f25bfc

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Remove event based logic and go back to regular updates

045ed07

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

luca-della-vedova force-pushed the luca/ecs_plugins_sandbox branch from 02ad73d to 045ed07 Compare May 8, 2024 09:13

luca-della-vedova added 2 commits May 14, 2024 16:13

Remove gz classic plugins from CI

2a4d1c3

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

Move build workflow to noble

ddd63ba

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

mxgrey requested changes May 23, 2024

View reviewed changes

mxgrey mentioned this pull request May 23, 2024

Port to harmonic open-rmf/rmf_demos#206

Merged

mxgrey added 2 commits May 23, 2024 02:00

Tweaks to help edge cases

f609649

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>

Fix style

c1f74fd

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>

mxgrey previously approved these changes May 23, 2024

View reviewed changes

arjo129 mentioned this pull request May 23, 2024

Add support for gz harmonic. #113

Closed

Fix initial floor logic

54b559c

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

luca-della-vedova dismissed mxgrey’s stale review via 54b559c May 23, 2024 13:51

mxgrey approved these changes May 23, 2024

View reviewed changes

Empty commit to force CI update

87c0893

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>

luca-della-vedova merged commit d9e55c6 into main May 29, 2024
3 of 4 checks passed

luca-della-vedova deleted the luca/ecs_plugins_sandbox branch May 29, 2024 12:24

luca-della-vedova mentioned this pull request May 30, 2024

Migrate to Garden #92

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Refactor plugins in an ECS based way #114

Refactor plugins in an ECS based way #114

luca-della-vedova commented Jan 19, 2024 •

edited

luca-della-vedova commented May 8, 2024

mxgrey left a comment

mxgrey May 22, 2024

mxgrey May 22, 2024

mxgrey May 22, 2024

mxgrey May 22, 2024

mxgrey May 22, 2024

mxgrey May 22, 2024

mxgrey May 23, 2024

mxgrey May 23, 2024

mxgrey left a comment

luca-della-vedova commented May 29, 2024

Refactor plugins in an ECS based way #114

Refactor plugins in an ECS based way #114

Conversation

luca-della-vedova commented Jan 19, 2024 • edited

New feature implementation

Implemented feature

Implementation description

luca-della-vedova commented May 8, 2024

mxgrey left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mxgrey left a comment

Choose a reason for hiding this comment

luca-della-vedova commented May 29, 2024

luca-della-vedova commented Jan 19, 2024 •

edited