From d46fba0fabd2d088222c5070996f69e6a9b1dcaa Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 19 Jan 2024 14:39:48 +0800
Subject: [PATCH] ECS refactor for lifts and doors

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 .../building_map/building.py                  |   2 +-
 .../building_map/doors/double_sliding_door.py |  13 +-
 .../building_map/doors/double_swing_door.py   |  13 +-
 .../building_map/doors/sliding_door.py        |  14 +-
 .../building_map/doors/swing_door.py          |  14 +-
 rmf_building_map_tools/building_map/lift.py   | 163 ++++------
 .../building_map/templates/ign_world.sdf      | 288 ++++++++++++------
 7 files changed, 293 insertions(+), 214 deletions(-)

diff --git a/rmf_building_map_tools/building_map/building.py b/rmf_building_map_tools/building_map/building.py
index a0ab55db2..42ea3f0b0 100644
--- a/rmf_building_map_tools/building_map/building.py
+++ b/rmf_building_map_tools/building_map/building.py
@@ -503,7 +503,7 @@ def generate_sdf_world(self, options):
                 {'name': 'toggle_floors', 'filename': 'libtoggle_floors.so'})
 
         elif 'ignition' in options:
-            plugin_ele = gui_ele.find('.//plugin[@filename="GzScene3D"]')
+            plugin_ele = gui_ele.find('.//plugin[@filename="MinimalScene"]')
             camera_pose_ele = plugin_ele.find('camera_pose')
             camera_pose_ele.text = camera_pose
 
diff --git a/rmf_building_map_tools/building_map/doors/double_sliding_door.py b/rmf_building_map_tools/building_map/doors/double_sliding_door.py
index 7c984c961..00132f699 100644
--- a/rmf_building_map_tools/building_map/doors/double_sliding_door.py
+++ b/rmf_building_map_tools/building_map/doors/double_sliding_door.py
@@ -30,20 +30,23 @@ def generate(self, world_ele, options):
 
         if not self.plugin == 'none':
             plugin_ele = SubElement(self.model_ele, 'plugin')
-            plugin_ele.set('name', 'door')
-            plugin_ele.set('filename', 'libdoor.so')
+            plugin_ele.set('name', 'register_component')
+            plugin_ele.set('filename', 'libregister_component.so')
+            component_ele = SubElement(plugin_ele, 'component')
+            component_ele.set('name', 'Door')
             plugin_params = {
                 'v_max_door': '0.2',
                 'a_max_door': '0.2',
                 'a_nom_door': '0.08',
                 'dx_min_door': '0.001',
-                'f_max_door': '100.0'
+                'f_max_door': '100.0',
+                'ros_interface': 'true'
             }
             for param_name, param_value in plugin_params.items():
-                ele = SubElement(plugin_ele, param_name)
+                ele = SubElement(component_ele, param_name)
                 ele.text = param_value
 
-            door_ele = SubElement(plugin_ele, 'door')
+            door_ele = SubElement(component_ele, 'door')
             door_ele.set('name', self.name)
             door_ele.set('type', 'DoubleSlidingDoor')
             door_ele.set('left_joint_name', 'left_joint')
diff --git a/rmf_building_map_tools/building_map/doors/double_swing_door.py b/rmf_building_map_tools/building_map/doors/double_swing_door.py
index 3bd0e19f9..d768849f7 100644
--- a/rmf_building_map_tools/building_map/doors/double_swing_door.py
+++ b/rmf_building_map_tools/building_map/doors/double_swing_door.py
@@ -48,20 +48,23 @@ def generate(self, world_ele, options):
 
         if not self.plugin == 'none':
             plugin_ele = SubElement(self.model_ele, 'plugin')
-            plugin_ele.set('name', 'door')
-            plugin_ele.set('filename', 'libdoor.so')
+            plugin_ele.set('name', 'register_component')
+            plugin_ele.set('filename', 'libregister_component.so')
+            component_ele = SubElement(plugin_ele, 'component')
+            component_ele.set('name', 'Door')
             plugin_params = {
                 'v_max_door': '0.5',
                 'a_max_door': '0.3',
                 'a_nom_door': '0.15',
                 'dx_min_door': '0.01',
-                'f_max_door': '500.0'
+                'f_max_door': '500.0',
+                'ros_interface': 'true'
             }
             for param_name, param_value in plugin_params.items():
-                ele = SubElement(plugin_ele, param_name)
+                ele = SubElement(component_ele, param_name)
                 ele.text = param_value
 
-            door_ele = SubElement(plugin_ele, 'door')
+            door_ele = SubElement(component_ele, 'door')
             door_ele.set('name', self.name)
             door_ele.set('type', 'DoubleSwingDoor')
             door_ele.set('left_joint_name', 'left_joint')
diff --git a/rmf_building_map_tools/building_map/doors/sliding_door.py b/rmf_building_map_tools/building_map/doors/sliding_door.py
index 781db3b80..4c47b811a 100644
--- a/rmf_building_map_tools/building_map/doors/sliding_door.py
+++ b/rmf_building_map_tools/building_map/doors/sliding_door.py
@@ -16,23 +16,27 @@ def generate(self, world_ele, options):
 
         if not self.plugin == 'none':
             plugin_ele = SubElement(self.model_ele, 'plugin')
-            plugin_ele.set('name', 'door')
-            plugin_ele.set('filename', 'libdoor.so')
+            plugin_ele.set('name', 'register_component')
+            plugin_ele.set('filename', 'libregister_component.so')
+            component_ele = SubElement(plugin_ele, 'component')
+            component_ele.set('name', 'Door')
             plugin_params = {
                 'v_max_door': '0.2',
                 'a_max_door': '0.2',
                 'a_nom_door': '0.08',
                 'dx_min_door': '0.001',
-                'f_max_door': '100.0'
+                'f_max_door': '100.0',
+                'ros_interface': 'true'
             }
             for param_name, param_value in plugin_params.items():
-                ele = SubElement(plugin_ele, param_name)
+                ele = SubElement(component_ele, param_name)
                 ele.text = param_value
 
-            door_ele = SubElement(plugin_ele, 'door')
+            door_ele = SubElement(component_ele, 'door')
             door_ele.set('name', self.name)
             door_ele.set('type', 'SlidingDoor')
             door_ele.set('left_joint_name', 'empty_joint')
             door_ele.set('right_joint_name', 'right_joint')
 
         world_ele.append(self.model_ele)
+
diff --git a/rmf_building_map_tools/building_map/doors/swing_door.py b/rmf_building_map_tools/building_map/doors/swing_door.py
index eba38280a..166b5571d 100644
--- a/rmf_building_map_tools/building_map/doors/swing_door.py
+++ b/rmf_building_map_tools/building_map/doors/swing_door.py
@@ -25,23 +25,27 @@ def generate(self, world_ele, options):
 
         if not self.plugin == 'none':
             plugin_ele = SubElement(self.model_ele, 'plugin')
-            plugin_ele.set('name', 'door')
-            plugin_ele.set('filename', 'libdoor.so')
+            plugin_ele.set('name', 'register_component')
+            plugin_ele.set('filename', 'libregister_component.so')
+            component_ele = SubElement(plugin_ele, 'component')
+            component_ele.set('name', 'Door')
             plugin_params = {
                 'v_max_door': '0.5',
                 'a_max_door': '0.3',
                 'a_nom_door': '0.15',
                 'dx_min_door': '0.01',
-                'f_max_door': '500.0'
+                'f_max_door': '500.0',
+                'ros_interface': 'true'
             }
             for param_name, param_value in plugin_params.items():
-                ele = SubElement(plugin_ele, param_name)
+                ele = SubElement(component_ele, param_name)
                 ele.text = param_value
 
-            door_ele = SubElement(plugin_ele, 'door')
+            door_ele = SubElement(component_ele, 'door')
             door_ele.set('name', self.name)
             door_ele.set('type', 'SwingDoor')
             door_ele.set('left_joint_name', 'empty_joint')
             door_ele.set('right_joint_name', 'right_joint')
 
         world_ele.append(self.model_ele)
+
diff --git a/rmf_building_map_tools/building_map/lift.py b/rmf_building_map_tools/building_map/lift.py
index dd286b5e3..0ea8846b2 100644
--- a/rmf_building_map_tools/building_map/lift.py
+++ b/rmf_building_map_tools/building_map/lift.py
@@ -66,10 +66,11 @@ def generate_cabin_door(self, lift_model_ele, name):
         door_pose.text = \
             f'{x} {y} 0 0 0 {self.motion_axis_orientation}'
 
-        self.generate_door_link_and_joint(door_model_ele, parent='platform')
+        self.generate_door_link(door_model_ele, parent='platform')
+        self.generate_joint(lift_model_ele, "platform", name)
 
         if self.plugin:
-            self.generate_door_plugin(door_model_ele, name)
+            self.generate_door_plugin(door_model_ele, name, True)
 
     def generate_shaft_door(self, world_ele, x, y, z, yaw, name):
         model_ele = SubElement(world_ele, 'model')
@@ -82,7 +83,8 @@ def generate_shaft_door(self, world_ele, x, y, z, yaw, name):
         yaw_new = yaw + self.motion_axis_orientation
         door_pose.text = f'{x_new} {y_new} {z} 0 0 {yaw_new}'
 
-        self.generate_door_link_and_joint(model_ele)
+        self.generate_door_link(model_ele)
+        self.generate_joint(model_ele)
 
         floor_thickness = 0.05
         ramp_depth = self.gap * 2
@@ -97,9 +99,33 @@ def generate_shaft_door(self, world_ele, x, y, z, yaw, name):
         model_ele.append(joint('ramp_joint', 'fixed', 'world', 'ramp'))
 
         if self.plugin:
-            self.generate_door_plugin(model_ele, name)
+            self.generate_door_plugin(model_ele, name, False)
 
-    def generate_door_link_and_joint(self, model_ele, parent='world'):
+    def generate_joint(
+            self, parent_element, parent_name="world", child_name=None):
+        if child_name is not None:
+            name_prefix = f'{child_name}_'
+            joint_child_prefix = f'{child_name}::'
+        else:
+            name_prefix = ''
+            joint_child_prefix = f''
+        parent_element.append(joint(f'{name_prefix}right_joint',
+                                    'prismatic',
+                                    parent_name,
+                                    f'{joint_child_prefix}right_door',
+                                    joint_axis='x',
+                                    lower_limit=0,
+                                    upper_limit=self.width / 2))
+
+        parent_element.append(joint(f'{name_prefix}left_joint',
+                                    'prismatic',
+                                    parent_name,
+                                    f'{joint_child_prefix}left_door',
+                                    joint_axis='x',
+                                    lower_limit=-self.width / 2,
+                                    upper_limit=0))
+
+    def generate_door_link(self, model_ele, parent='world'):
         door_size = [self.width / 2, self.thickness, self.height]
         right_door_pose = Element('pose')
         right_door_pose.text = f'{self.width / 4} 0 {self.height / 2} 0 0 0'
@@ -110,14 +136,6 @@ def generate_door_link_and_joint(self, model_ele, parent='world'):
                                   material=lift_material(),
                                   bitmask='0x02'))
 
-        model_ele.append(joint('right_joint',
-                               'prismatic',
-                               parent,
-                               'right_door',
-                               joint_axis='x',
-                               lower_limit=0,
-                               upper_limit=self.width / 2))
-
         left_door_pose = Element('pose')
         left_door_pose.text = f'{-self.width / 4} 0 {self.height / 2} 0 0 0'
 
@@ -127,88 +145,25 @@ def generate_door_link_and_joint(self, model_ele, parent='world'):
                                   material=lift_material(),
                                   bitmask='0x02'))
 
-        model_ele.append(joint('left_joint',
-                               'prismatic',
-                               parent,
-                               'left_door',
-                               joint_axis='x',
-                               lower_limit=-self.width / 2,
-                               upper_limit=0))
-
-    def generate_door_plugin(self, model_ele, name):
+    def generate_door_plugin(self, model_ele, name, append_prefix=False):
+        if append_prefix is True:
+            prefix = f'{name}_'
+        else:
+            prefix = ''
         plugin_ele = SubElement(model_ele, 'plugin')
-        plugin_ele.set('name', 'door')
-        plugin_ele.set('filename', 'libdoor.so')
+        plugin_ele.set('name', 'register_component')
+        plugin_ele.set('filename', 'libregister_component.so')
+        component_ele = SubElement(plugin_ele, 'component')
+        component_ele.set('name', 'Door')
         for param_name, param_value in self.params.items():
-            ele = SubElement(plugin_ele, param_name)
+            ele = SubElement(component_ele, param_name)
             ele.text = f'{param_value}'
-        door_ele = SubElement(plugin_ele, 'door')
-        door_ele.set('left_joint_name', 'left_joint')
+        door_ele = SubElement(component_ele, 'door')
+        door_ele.set('left_joint_name', f'{prefix}left_joint')
         door_ele.set('name', f'{name}')
-        door_ele.set('right_joint_name', 'right_joint')
+        door_ele.set('right_joint_name', f'{prefix}right_joint')
         door_ele.set('type', 'DoubleSlidingDoor')
 
-    # TODO: remove this function once nesting model is supported in ignition.
-    def generate_cabin_door_ign(self, lift_model_ele, name):
-        # This is for cabin door generation for ignition gazebo as it doesn't
-        # support nested models yet. Once ignition gazebo supports nested
-        # models, this should be removed.
-        (x, y) = self.cabin_door_pose
-        yaw = self.motion_axis_orientation
-        right_x = x + np.cos(yaw) * self.width/4
-        left_x = x - np.cos(yaw) * self.width/4
-        right_y = y + np.sin(yaw) * self.width/4
-        left_y = y - np.sin(yaw) * self.width/4
-
-        door_size = [self.width / 2, self.thickness, self.height]
-        right_door_pose = Element('pose')
-        right_door_pose.text = \
-            f'{right_x} {right_y} {self.height / 2} 0 0 {yaw}'
-
-        lift_model_ele.append(box_link(f'{name}_right_door',
-                                       door_size,
-                                       right_door_pose,
-                                       material=lift_material(),
-                                       bitmask='0x02'))
-
-        lift_model_ele.append(joint(f'{name}_right_joint',
-                                    'prismatic',
-                                    'platform',
-                                    f'{name}_right_door',
-                                    joint_axis='x',
-                                    lower_limit=0,
-                                    upper_limit=self.width / 2))
-
-        left_door_pose = Element('pose')
-        left_door_pose.text = f'{left_x} {left_y} {self.height / 2} 0 0 {yaw}'
-
-        lift_model_ele.append(box_link(f'{name}_left_door',
-                                       door_size,
-                                       left_door_pose,
-                                       material=lift_material(),
-                                       bitmask='0x02'))
-
-        lift_model_ele.append(joint(f'{name}_left_joint',
-                                    'prismatic',
-                                    'platform',
-                                    f'{name}_left_door',
-                                    joint_axis='x',
-                                    lower_limit=-self.width / 2,
-                                    upper_limit=0))
-
-        if self.plugin:
-            plugin_ele = SubElement(lift_model_ele, 'plugin')
-            plugin_ele.set('name', 'door')
-            plugin_ele.set('filename', 'libdoor.so')
-            for param_name, param_value in self.params.items():
-                ele = SubElement(plugin_ele, param_name)
-                ele.text = f'{param_value}'
-            door_ele = SubElement(plugin_ele, 'door')
-            door_ele.set('left_joint_name', f'{name}_left_joint')
-            door_ele.set('name', f'{name}')
-            door_ele.set('right_joint_name', f'{name}_right_joint')
-            door_ele.set('type', 'DoubleSlidingDoor')
-
 
 class Lift:
     def __init__(self, yaml_node, name, transform, levels, coordinate_system):
@@ -429,28 +384,23 @@ def generate_cabin(self, world_ele, options):
                                     joint_axis='z'))
 
         # cabin doors
-        # TODO: remove the if statement here once nesting model is supported
-        # in ignition.
-        if 'ignition' in options:
-            for lift_door in self.doors:
-                lift_door.generate_cabin_door_ign(
-                    lift_model_ele, f'CabinDoor_{self.name}_{lift_door.name}')
-        else:
-            for lift_door in self.doors:
-                lift_door.generate_cabin_door(
-                    lift_model_ele, f'CabinDoor_{self.name}_{lift_door.name}')
+        for lift_door in self.doors:
+            lift_door.generate_cabin_door(
+                lift_model_ele, f'CabinDoor_{self.name}_{lift_door.name}')
 
         # lift cabin plugin
         if self.plugins:
             plugin_ele = SubElement(lift_model_ele, 'plugin')
-            plugin_ele.set('name', 'lift')
-            plugin_ele.set('filename', 'liblift.so')
+            plugin_ele.set('name', 'register_component')
+            plugin_ele.set('filename', 'libregister_component.so')
+            component_ele = SubElement(plugin_ele, 'component')
+            component_ele.set('name', 'Lift')
 
-            lift_name_ele = SubElement(plugin_ele, 'lift_name')
+            lift_name_ele = SubElement(component_ele, 'lift_name')
             lift_name_ele.text = f'{self.name}'
 
             for level_name, door_names in self.level_doors.items():
-                floor_ele = SubElement(plugin_ele, 'floor')
+                floor_ele = SubElement(component_ele, 'floor')
                 floor_ele.set('name', f'{level_name}')
                 floor_ele.set(
                     'elevation', f'{self.level_elevation[level_name]}')
@@ -464,13 +414,13 @@ def generate_cabin(self, world_ele, options):
                             'shaft_door',
                             f'ShaftDoor_{self.name}_{level_name}_{door.name}')
 
-            initial_floor_ele = SubElement(plugin_ele, 'initial_floor')
+            initial_floor_ele = SubElement(component_ele, 'initial_floor')
             initial_floor_ele.text = f'{self.initial_floor_name}'
             for param_name, param_value in self.params.items():
-                ele = SubElement(plugin_ele, param_name)
+                ele = SubElement(component_ele, param_name)
                 ele.text = f'{param_value}'
 
-            cabin_joint_ele = SubElement(plugin_ele, 'cabin_joint_name')
+            cabin_joint_ele = SubElement(component_ele, 'cabin_joint_name')
             cabin_joint_ele.text = 'cabin_joint'
         else:
             static_lift_ele = SubElement(lift_model_ele, 'static')
@@ -479,3 +429,4 @@ def generate_cabin(self, world_ele, options):
         # pose
         model_pose = SubElement(lift_model_ele, 'pose')
         model_pose.text = f'{self.x} {self.y} 0 0 0 {self.yaw}'
+
diff --git a/rmf_building_map_tools/building_map/templates/ign_world.sdf b/rmf_building_map_tools/building_map/templates/ign_world.sdf
index 81444b97f..d7c75108d 100644
--- a/rmf_building_map_tools/building_map/templates/ign_world.sdf
+++ b/rmf_building_map_tools/building_map/templates/ign_world.sdf
@@ -17,6 +17,14 @@
       filename="libignition-gazebo-scene-broadcaster-system.so"
       name="ignition::gazebo::systems::SceneBroadcaster">
     </plugin>
+    <plugin
+      filename="libdoor.so"
+      name="door">
+    </plugin>
+    <plugin
+      filename="liblift.so"
+      name="lift">
+    </plugin>
 
     <scene>
       <ambient>1 1 1</ambient>
@@ -49,120 +57,225 @@
       </window>
 
       <!-- GUI plugins -->
+      <plugin filename="MinimalScene" name="3D View">
+          <gz-gui>
+              <title>3D View</title>
+              <property type="bool" key="showTitleBar">false</property>
+              <property type="string" key="state">docked</property>
+          </gz-gui>
+          <engine>ogre2</engine>
+          <scene>scene</scene>
+          <ambient_light>0.4 0.4 0.4</ambient_light>
+          <background_color>0.8 0.8 0.8</background_color>
+          <camera_pose>-6 0 6 0 0.5 0</camera_pose>
+      </plugin>
 
-      <!-- 3D scene -->
-      <plugin filename="GzScene3D" name="3D View">
-        <ignition-gui>
-          <title>3D View</title>
-          <property type="bool" key="showTitleBar">false</property>
-          <property type="string" key="state">docked</property>
-        </ignition-gui>
+      <!-- Plugins that add functionality to the scene -->
+      <plugin filename="EntityContextMenuPlugin" name="Entity context menu">
+          <gz-gui>
+              <property key="state" type="string">floating</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
 
-        <engine>ogre2</engine>
-        <scene>scene</scene>
-        <ambient_light>0.4 0.4 0.4</ambient_light>
-        <background_color>0.8 0.8 0.8</background_color>
-        <camera_pose>6 0 6 0 0.5 3.14</camera_pose>
+      <plugin filename="GzSceneManager" name="Scene Manager">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
       </plugin>
 
-      <!-- Play / pause / step -->
-      <plugin filename="WorldControl" name="World control">
-        <ignition-gui>
-          <title>World control</title>
-          <property type="bool" key="showTitleBar">false</property>
-          <property type="bool" key="resizable">false</property>
-          <property type="double" key="height">72</property>
-          <property type="double" key="width">121</property>
-          <property type="double" key="z">1</property>
+      <plugin filename="InteractiveViewControl" name="Interactive view control">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
 
-          <property type="string" key="state">floating</property>
-          <anchors target="3D View">
-            <line own="left" target="left"/>
-            <line own="bottom" target="bottom"/>
-          </anchors>
-        </ignition-gui>
+      <plugin filename="CameraTracking" name="Camera Tracking">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
 
-        <play_pause>true</play_pause>
-        <step>true</step>
-        <start_paused>true</start_paused>
+      <plugin filename="MarkerManager" name="Marker manager">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
 
+      <plugin filename="SelectEntities" name="Select Entities">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
       </plugin>
 
-      <!-- Time / RTF -->
-      <plugin filename="WorldStats" name="World stats">
-        <ignition-gui>
-          <title>World stats</title>
-          <property type="bool" key="showTitleBar">false</property>
-          <property type="bool" key="resizable">false</property>
-          <property type="double" key="height">110</property>
-          <property type="double" key="width">290</property>
-          <property type="double" key="z">1</property>
+      <plugin filename="Spawn" name="Spawn Entities">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
+
+      <plugin filename="VisualizationCapabilities" name="Visualization Capabilities">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="width" type="double">5</property>
+              <property key="height" type="double">5</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+          </gz-gui>
+      </plugin>
 
-          <property type="string" key="state">floating</property>
-          <anchors target="3D View">
-            <line own="right" target="right"/>
-            <line own="bottom" target="bottom"/>
-          </anchors>
-        </ignition-gui>
+      <!-- World control -->
+      <plugin filename="WorldControl" name="World control">
+          <gz-gui>
+              <title>World control</title>
+              <property type="bool" key="showTitleBar">false</property>
+              <property type="bool" key="resizable">false</property>
+              <property type="double" key="height">72</property>
+              <property type="double" key="z">1</property>
+              <property type="string" key="state">floating</property>
+              <anchors target="3D View">
+                  <line own="left" target="left"/>
+                  <line own="bottom" target="bottom"/>
+              </anchors>
+          </gz-gui>
+          <play_pause>true</play_pause>
+          <step>true</step>
+          <start_paused>true</start_paused>
+          <use_event>true</use_event>
+      </plugin>
 
-        <sim_time>true</sim_time>
-        <real_time>true</real_time>
-        <real_time_factor>true</real_time_factor>
-        <iterations>true</iterations>
+      <!-- World statistics -->
+      <plugin filename="WorldStats" name="World stats">
+          <gz-gui>
+              <title>World stats</title>
+              <property type="bool" key="showTitleBar">false</property>
+              <property type="bool" key="resizable">false</property>
+              <property type="double" key="height">110</property>
+              <property type="double" key="width">290</property>
+              <property type="double" key="z">1</property>
+              <property type="string" key="state">floating</property>
+              <anchors target="3D View">
+                  <line own="right" target="right"/>
+                  <line own="bottom" target="bottom"/>
+              </anchors>
+          </gz-gui>
+          <sim_time>true</sim_time>
+          <real_time>true</real_time>
+          <real_time_factor>true</real_time_factor>
+          <iterations>true</iterations>
+      </plugin>
+
+      <!-- Insert simple shapes -->
+      <plugin filename="Shapes" name="Shapes">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="x" type="double">0</property>
+              <property key="y" type="double">0</property>
+              <property key="width" type="double">250</property>
+              <property key="height" type="double">50</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+              <property key="cardBackground" type="string">#666666</property>
+          </gz-gui>
+      </plugin>
 
+      <!-- Insert lights -->
+      <plugin filename="Lights" name="Lights">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="x" type="double">250</property>
+              <property key="y" type="double">0</property>
+              <property key="width" type="double">150</property>
+              <property key="height" type="double">50</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+              <property key="cardBackground" type="string">#666666</property>
+          </gz-gui>
       </plugin>
 
       <!-- Translate / rotate -->
       <plugin filename="TransformControl" name="Transform control">
-        <ignition-gui>
-          <title>Transform control</title>
-          <anchors target="3D View">
-            <line own="left" target="left"/>
-            <line own="top" target="top"/>
-          </anchors>
-          <property key="resizable" type="bool">false</property>
-          <property key="width" type="double">230</property>
-          <property key="height" type="double">50</property>
-          <property key="state" type="string">floating</property>
-          <property key="showTitleBar" type="bool">false</property>
-          <property key="cardBackground" type="string">#666666</property>
-        </ignition-gui>
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="x" type="double">0</property>
+              <property key="y" type="double">50</property>
+              <property key="width" type="double">250</property>
+              <property key="height" type="double">50</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+              <property key="cardBackground" type="string">#777777</property>
+          </gz-gui>
       </plugin>
 
-      <!-- Insert simple shapes -->
-      <plugin filename="Shapes" name="Shapes">
-        <ignition-gui>
-          <anchors target="Transform control">
-            <line own="left" target="right"/>
-            <line own="top" target="top"/>
-          </anchors>
-          <property key="resizable" type="bool">false</property>
-          <property key="width" type="double">200</property>
-          <property key="height" type="double">50</property>
-          <property key="state" type="string">floating</property>
-          <property key="showTitleBar" type="bool">false</property>
-          <property key="cardBackground" type="string">#666666</property>
-        </ignition-gui>
+      <!-- Screenshot -->
+      <plugin filename="Screenshot" name="Screenshot">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="x" type="double">250</property>
+              <property key="y" type="double">50</property>
+              <property key="width" type="double">50</property>
+              <property key="height" type="double">50</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+              <property key="cardBackground" type="string">#777777</property>
+          </gz-gui>
+      </plugin>
+
+      <!-- Copy/Paste -->
+      <plugin filename="CopyPaste" name="CopyPaste">
+          <gz-gui>
+              <property key="resizable" type="bool">false</property>
+              <property key="x" type="double">300</property>
+              <property key="y" type="double">50</property>
+              <property key="width" type="double">100</property>
+              <property key="height" type="double">50</property>
+              <property key="state" type="string">floating</property>
+              <property key="showTitleBar" type="bool">false</property>
+              <property key="cardBackground" type="string">#777777</property>
+          </gz-gui>
       </plugin>
 
       <!-- Inspector -->
       <plugin filename="ComponentInspector" name="Component inspector">
-        <ignition-gui>
-          <property type="bool" key="showTitleBar">false</property>
-          <property type="string" key="state">docked</property>
-        </ignition-gui>
+          <gz-gui>
+              <property type="bool" key="showTitleBar">false</property>
+              <property type="string" key="state">docked</property>
+          </gz-gui>
       </plugin>
 
       <!-- Entity tree -->
       <plugin filename="EntityTree" name="Entity tree">
-        <ignition-gui>
-          <property type="bool" key="showTitleBar">false</property>
-          <property type="string" key="state">docked</property>
-        </ignition-gui>
+          <gz-gui>
+              <property type="bool" key="showTitleBar">false</property>
+              <property type="string" key="state">docked</property>
+          </gz-gui>
       </plugin>
-
-      <!-- Toggle charging traffic editor plugin -->
-      <plugin filename="toggle_charging" name="toggle_charging"/>
     </gui>
 
     <light type="directional" name="sun">
@@ -181,3 +294,4 @@
 
   </world>
 </sdf>
+