From c2d0402e3bb72cc894d9af66faa0c7501c783c38 Mon Sep 17 00:00:00 2001
From: Wiktor Bajor <wiktorbajor1@gmail.com>
Date: Sun, 18 Aug 2024 12:27:06 +0200
Subject: [PATCH] Migrate demo to use modern gazebo

---
 .../grasp_service_parameters.py               | 106 +--
 .../launch/gazebo.launch.py                   |  48 --
 .../manipulator_simulation_bringup.launch.py  |  64 +-
 .../manipulator_simulation/setup.py           |   2 +-
 .../worlds/default_world.sdf                  | 701 ++++++++++++++++++
 .../worlds/default_world.world                |  35 -
 .../urdf/ros2_control.xacro                   |   5 +-
 7 files changed, 804 insertions(+), 157 deletions(-)
 delete mode 100644 ros2_grasp_service_demo/manipulator_simulation/launch/gazebo.launch.py
 create mode 100644 ros2_grasp_service_demo/manipulator_simulation/worlds/default_world.sdf
 delete mode 100644 ros2_grasp_service_demo/manipulator_simulation/worlds/default_world.world

diff --git a/ros2_grasp_service/ros2_grasp_service/grasp_service_parameters.py b/ros2_grasp_service/ros2_grasp_service/grasp_service_parameters.py
index ae2a17f..b045daf 100644
--- a/ros2_grasp_service/ros2_grasp_service/grasp_service_parameters.py
+++ b/ros2_grasp_service/ros2_grasp_service/grasp_service_parameters.py
@@ -1,3 +1,5 @@
+# flake8: noqa
+
 # auto-generated DO NOT EDIT
 
 from rcl_interfaces.msg import ParameterDescriptor
@@ -35,6 +37,7 @@ class __MapPositionsToTargetList:
             def add_entry(self, name):
                 if not hasattr(self, name):
                     setattr(self, name, self.__map_type())
+                return getattr(self, name)
 
             def get_entry(self, name):
                 return getattr(self, name)
@@ -51,6 +54,7 @@ class __MapPositionsToHomeList:
             def add_entry(self, name):
                 if not hasattr(self, name):
                     setattr(self, name, self.__map_type())
+                return getattr(self, name)
 
             def get_entry(self, name):
                 return getattr(self, name)
@@ -85,10 +89,11 @@ def refresh_dynamic_parameters(self):
             # TODO remove any destroyed dynamic parameters
 
             # declare any new dynamic parameters
-            for value in updated_params.positions_to_target_list:
-                updated_params.positions_to_target.add_entry(value)
-                entry = updated_params.positions_to_target.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_target.{value}.positions"
+
+            for value_1 in updated_params.positions_to_target_list:
+                updated_params.positions_to_target.add_entry(value_1)
+                entry = updated_params.positions_to_target.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_target.{value_1}.positions"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(
                         description="List of joints value for given position.", read_only=False
@@ -107,10 +112,11 @@ def refresh_dynamic_parameters(self):
                         + validation_result,
                     )
                 entry.positions = param.value
-            for value in updated_params.positions_to_target_list:
-                updated_params.positions_to_target.add_entry(value)
-                entry = updated_params.positions_to_target.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_target.{value}.time_to_target"
+
+            for value_1 in updated_params.positions_to_target_list:
+                updated_params.positions_to_target.add_entry(value_1)
+                entry = updated_params.positions_to_target.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_target.{value_1}.time_to_target"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(description="Time for given position", read_only=False)
                     descriptor.integer_range.append(IntegerRange())
@@ -130,10 +136,11 @@ def refresh_dynamic_parameters(self):
                         + validation_result,
                     )
                 entry.time_to_target = param.value
-            for value in updated_params.positions_to_home_list:
-                updated_params.positions_to_home.add_entry(value)
-                entry = updated_params.positions_to_home.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_home.{value}.positions"
+
+            for value_1 in updated_params.positions_to_home_list:
+                updated_params.positions_to_home.add_entry(value_1)
+                entry = updated_params.positions_to_home.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_home.{value_1}.positions"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(
                         description="List of joints value for given position.", read_only=False
@@ -152,10 +159,11 @@ def refresh_dynamic_parameters(self):
                         + validation_result,
                     )
                 entry.positions = param.value
-            for value in updated_params.positions_to_home_list:
-                updated_params.positions_to_home.add_entry(value)
-                entry = updated_params.positions_to_home.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_home.{value}.time_to_target"
+
+            for value_1 in updated_params.positions_to_home_list:
+                updated_params.positions_to_home.add_entry(value_1)
+                entry = updated_params.positions_to_home.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_home.{value_1}.time_to_target"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(description="Time for given position", read_only=False)
                     descriptor.integer_range.append(IntegerRange())
@@ -241,42 +249,44 @@ def update(self, parameters):
 
             # update dynamic parameters
             for param in parameters:
-                for value in updated_params.positions_to_target_list:
-                    param_name = f"{self.prefix_}positions_to_target.{value}.positions"
+                for value_1 in updated_params.positions_to_target_list:
+                    param_name = f"{self.prefix_}positions_to_target{value_1}.positions"
                     if param.name == param_name:
                         validation_result = ParameterValidators.size_gt(param, 0)
                         if validation_result:
                             return SetParametersResult(successful=False, reason=validation_result)
-                        updated_params.positions_to_target.positions_to_target_list_map[value].positions = param.value
+
+                        updated_params.positions_to_target.get_entry(value_1).positions = param.value
                         self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
 
-                for value in updated_params.positions_to_target_list:
-                    param_name = f"{self.prefix_}positions_to_target.{value}.time_to_target"
+                for value_1 in updated_params.positions_to_target_list:
+                    param_name = f"{self.prefix_}positions_to_target{value_1}.time_to_target"
                     if param.name == param_name:
                         validation_result = ParameterValidators.gt(param, 0)
                         if validation_result:
                             return SetParametersResult(successful=False, reason=validation_result)
-                        updated_params.positions_to_target.positions_to_target_list_map[value].time_to_target = (
-                            param.value
-                        )
+
+                        updated_params.positions_to_target.get_entry(value_1).time_to_target = param.value
                         self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
 
-                for value in updated_params.positions_to_home_list:
-                    param_name = f"{self.prefix_}positions_to_home.{value}.positions"
+                for value_1 in updated_params.positions_to_home_list:
+                    param_name = f"{self.prefix_}positions_to_home{value_1}.positions"
                     if param.name == param_name:
                         validation_result = ParameterValidators.size_gt(param, 0)
                         if validation_result:
                             return SetParametersResult(successful=False, reason=validation_result)
-                        updated_params.positions_to_home.positions_to_home_list_map[value].positions = param.value
+
+                        updated_params.positions_to_home.get_entry(value_1).positions = param.value
                         self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
 
-                for value in updated_params.positions_to_home_list:
-                    param_name = f"{self.prefix_}positions_to_home.{value}.time_to_target"
+                for value_1 in updated_params.positions_to_home_list:
+                    param_name = f"{self.prefix_}positions_to_home{value_1}.time_to_target"
                     if param.name == param_name:
                         validation_result = ParameterValidators.gt(param, 0)
                         if validation_result:
                             return SetParametersResult(successful=False, reason=validation_result)
-                        updated_params.positions_to_home.positions_to_home_list_map[value].time_to_target = param.value
+
+                        updated_params.positions_to_home.get_entry(value_1).time_to_target = param.value
                         self.logger_.debug(param.name + ": " + param.type_.name + " = " + str(param.value))
 
             updated_params.stamp_ = self.clock_.now()
@@ -432,10 +442,11 @@ def declare_params(self):
             updated_params.gripper_open = param.value
 
             # declare and set all dynamic parameters
-            for value in updated_params.positions_to_target_list:
-                updated_params.positions_to_target.add_entry(value)
-                entry = updated_params.positions_to_target.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_target.{value}.positions"
+
+            for value_1 in updated_params.positions_to_target_list:
+                updated_params.positions_to_target.add_entry(value_1)
+                entry = updated_params.positions_to_target.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_target.{value_1}.positions"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(
                         description="List of joints value for given position.", read_only=False
@@ -454,10 +465,11 @@ def declare_params(self):
                         + validation_result,
                     )
                 entry.positions = param.value
-            for value in updated_params.positions_to_target_list:
-                updated_params.positions_to_target.add_entry(value)
-                entry = updated_params.positions_to_target.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_target.{value}.time_to_target"
+
+            for value_1 in updated_params.positions_to_target_list:
+                updated_params.positions_to_target.add_entry(value_1)
+                entry = updated_params.positions_to_target.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_target.{value_1}.time_to_target"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(description="Time for given position", read_only=False)
                     descriptor.integer_range.append(IntegerRange())
@@ -477,10 +489,11 @@ def declare_params(self):
                         + validation_result,
                     )
                 entry.time_to_target = param.value
-            for value in updated_params.positions_to_home_list:
-                updated_params.positions_to_home.add_entry(value)
-                entry = updated_params.positions_to_home.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_home.{value}.positions"
+
+            for value_1 in updated_params.positions_to_home_list:
+                updated_params.positions_to_home.add_entry(value_1)
+                entry = updated_params.positions_to_home.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_home.{value_1}.positions"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(
                         description="List of joints value for given position.", read_only=False
@@ -499,10 +512,11 @@ def declare_params(self):
                         + validation_result,
                     )
                 entry.positions = param.value
-            for value in updated_params.positions_to_home_list:
-                updated_params.positions_to_home.add_entry(value)
-                entry = updated_params.positions_to_home.get_entry(value)
-                param_name = f"{self.prefix_}positions_to_home.{value}.time_to_target"
+
+            for value_1 in updated_params.positions_to_home_list:
+                updated_params.positions_to_home.add_entry(value_1)
+                entry = updated_params.positions_to_home.get_entry(value_1)
+                param_name = f"{self.prefix_}positions_to_home.{value_1}.time_to_target"
                 if not self.node_.has_parameter(self.prefix_ + param_name):
                     descriptor = ParameterDescriptor(description="Time for given position", read_only=False)
                     descriptor.integer_range.append(IntegerRange())
diff --git a/ros2_grasp_service_demo/manipulator_simulation/launch/gazebo.launch.py b/ros2_grasp_service_demo/manipulator_simulation/launch/gazebo.launch.py
deleted file mode 100644
index 07869bf..0000000
--- a/ros2_grasp_service_demo/manipulator_simulation/launch/gazebo.launch.py
+++ /dev/null
@@ -1,48 +0,0 @@
-import os
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.conditions import IfCondition
-
-
-def generate_launch_description():
-    os.environ["GAZEBO_MODEL_PATH"] = os.path.join(get_package_share_directory("manipulator_simulation"), "models")
-    manipulator_simulation_package = get_package_share_directory("manipulator_simulation")
-    gazebo_ros_package_dir = get_package_share_directory("gazebo_ros")
-
-    return LaunchDescription(
-        [
-            DeclareLaunchArgument(
-                name="world",
-                default_value="default_world.world",
-                description="Name of the world model file to load in ariadna_simulation_bringup/worlds",
-            ),
-            DeclareLaunchArgument(name="gui", default_value="true", description='Set to "false" to run headless.'),
-            DeclareLaunchArgument(
-                name="verbose",
-                default_value="false",
-                description='Set to "true" to run verbose logging.',
-            ),
-            DeclareLaunchArgument(
-                name="server",
-                default_value="true",
-                description='Set to "false" not to run gzserver.',
-            ),
-            IncludeLaunchDescription(
-                PythonLaunchDescriptionSource([os.path.join(gazebo_ros_package_dir, "launch", "gzserver.launch.py")]),
-                launch_arguments={
-                    "world": PathJoinSubstitution(
-                        [manipulator_simulation_package, "worlds", LaunchConfiguration("world")]
-                    ),
-                    "verbose": LaunchConfiguration("verbose"),
-                }.items(),
-                condition=IfCondition(LaunchConfiguration("server")),
-            ),
-            IncludeLaunchDescription(
-                PythonLaunchDescriptionSource([os.path.join(gazebo_ros_package_dir, "launch", "gzclient.launch.py")]),
-                condition=IfCondition(LaunchConfiguration("gui")),
-            ),
-        ]
-    )
diff --git a/ros2_grasp_service_demo/manipulator_simulation/launch/manipulator_simulation_bringup.launch.py b/ros2_grasp_service_demo/manipulator_simulation/launch/manipulator_simulation_bringup.launch.py
index 7a7ab62..79a7157 100644
--- a/ros2_grasp_service_demo/manipulator_simulation/launch/manipulator_simulation_bringup.launch.py
+++ b/ros2_grasp_service_demo/manipulator_simulation/launch/manipulator_simulation_bringup.launch.py
@@ -4,45 +4,59 @@
 from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
 
 
 def generate_launch_description():
-    manipulator_simulation = get_package_share_directory("manipulator_simulation")
-
-    manipulator_spawner = Node(
-        package="gazebo_ros",
-        executable="spawn_entity.py",
-        arguments=["-entity", "manipulator", "-topic", "robot_description"],
-        output="screen",
-        emulate_tty=True,
-    )
-
     open_manipulator_bringup = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             [
                 get_package_share_directory("open_manipulator_bringup"),
                 "/launch/open_manipulator_bringup.launch.py",
             ]
+        )
+    )
+
+    world_argument = DeclareLaunchArgument(
+        "world",
+        default_value=os.path.join(
+            get_package_share_directory("manipulator_simulation"), "worlds", "default_world.sdf"
         ),
+        description="Robot controller to start.",
+    )
+
+    gazebo = IncludeLaunchDescription(
+        os.path.join(get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py"),
+        launch_arguments=[
+            (
+                "gz_args",
+                ["-r -v 4 ", LaunchConfiguration("world"), " --physics-engine gz-physics-bullet-featherstone-plugin"],
+            )
+        ],
+    )
+
+    ign_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        name="gz_bridge",
+        arguments=["/clock" + "@rosgraph_msgs/msg/Clock" + "[ignition.msgs.Clock"],
+        output="screen",
+    )
+
+    gazebo_spawn_robot = Node(
+        package="ros_gz_sim",
+        executable="create",
+        name="spawn_manipulator",
+        arguments=["-name", "manipulator", "-topic", "robot_description"],
+        output="screen",
     )
 
     return LaunchDescription(
         [
-            DeclareLaunchArgument(
-                name="world",
-                default_value=os.path.join(manipulator_simulation, "worlds", "default_world.world"),
-                description="Full path to the world model file to load",
-            ),
-            DeclareLaunchArgument(name="gui", default_value="true", description='Set to "false" to run headless.'),
-            DeclareLaunchArgument(
-                name="server",
-                default_value="true",
-                description='Set to "false" not to run gzserver.',
-            ),
-            IncludeLaunchDescription(
-                PythonLaunchDescriptionSource([manipulator_simulation, "/launch/gazebo.launch.py"])
-            ),
-            manipulator_spawner,
+            world_argument,
+            gazebo,
+            ign_bridge,
+            gazebo_spawn_robot,
             open_manipulator_bringup,
         ]
     )
diff --git a/ros2_grasp_service_demo/manipulator_simulation/setup.py b/ros2_grasp_service_demo/manipulator_simulation/setup.py
index a3cbe1d..6115a32 100644
--- a/ros2_grasp_service_demo/manipulator_simulation/setup.py
+++ b/ros2_grasp_service_demo/manipulator_simulation/setup.py
@@ -12,7 +12,7 @@
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
         ("share/" + package_name, ["package.xml"]),
         (os.path.join("share", package_name, "launch"), glob("launch/*.py")),
-        (os.path.join("share", package_name, "worlds"), glob("worlds/*.world")),
+        (os.path.join("share", package_name, "worlds"), glob("worlds/*.sdf")),
         (
             os.path.join("share", package_name, "models/ground_plane"),
             glob("models/ground_plane/model.sdf"),
diff --git a/ros2_grasp_service_demo/manipulator_simulation/worlds/default_world.sdf b/ros2_grasp_service_demo/manipulator_simulation/worlds/default_world.sdf
new file mode 100644
index 0000000..6b01308
--- /dev/null
+++ b/ros2_grasp_service_demo/manipulator_simulation/worlds/default_world.sdf
@@ -0,0 +1,701 @@
+<sdf version='1.10'>
+  <world name='world_demo'>
+    <physics name='1ms' type='ignored'>
+      <max_step_size>0.01</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <plugin name='ignition::gazebo::systems::Physics' filename='libignition-gazebo-physics-system.so'/>
+    <plugin name='ignition::gazebo::systems::UserCommands' filename='libignition-gazebo-user-commands-system.so'/>
+    <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='libignition-gazebo-scene-broadcaster-system.so'/>
+    <scene>
+      <ambient>1 1 1 1</ambient>
+      <background>0.300000012 0.699999988 0.899999976 1</background>
+      <shadows>false</shadows>
+      <grid>true</grid>
+    </scene>
+    <gravity>0 0 -9.8000000000000007</gravity>
+    <magnetic_field>5.5644999999999998e-06 2.2875799999999999e-05 -4.2388400000000002e-05</magnetic_field>
+    <atmosphere type='adiabatic'/>
+    <model name='ground_plane'>
+      <static>true</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>1 1</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <pose>0 0 0 0 0 0</pose>
+        <inertial>
+          <pose>0 0 0 0 0 0</pose>
+          <mass>1</mass>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+        </inertial>
+        <enable_wind>false</enable_wind>
+      </link>
+      <pose>0 0 0 0 0 0</pose>
+      <self_collide>false</self_collide>
+    </model>
+    <include>
+      <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Depot</uri>
+      <name>Depot</name>
+      <pose>7.1900899999999996 1.09982 0 0 0 0</pose>
+    </include>
+    <model name='depot_collision'>
+      <static>true</static>
+      <pose>7.1900899999999996 1.09982 0 0 0 0</pose>
+      <link name='collision_link'>
+        <pose>0 0 0 0 0 0</pose>
+        <collision name='wall1'>
+          <pose>0 -7.6128999999999998 4.5 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>30.167000000000002 0.080000000000000002 9</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='wall2'>
+          <pose>0 7.2874999999999996 4.5 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>30.167000000000002 0.080000000000000002 9</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='wall3'>
+          <pose>-15 0 4.5 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.080000000000000002 15.359999999999999 9</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='wall4'>
+          <pose>15 0 4.5 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.080000000000000002 15.359999999999999 9</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='boxes1'>
+          <pose>0.22267999999999999 -4.7267999999999999 0.68506 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.288 1.4219999999999999 1.288</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='boxes2'>
+          <pose>3.1726999999999999 -4.7267999999999999 0.68506 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.288 1.4219999999999999 1.288</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='boxes3'>
+          <pose>5.95268 -4.7267999999999999 0.68506 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.288 1.4219999999999999 1.288</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='boxes4'>
+          <pose>8.5588700000000006 -4.7267999999999999 0.68506 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.288 1.4219999999999999 1.288</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='boxes5'>
+          <pose>11.326000000000001 -4.7267999999999999 0.68506 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.288 1.4219999999999999 1.288</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='boxes6'>
+          <pose>0.22267999999999999 -2.3744800000000001 0.68506 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.288 1.4219999999999999 1.288</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='boxes7'>
+          <pose>3.1726999999999999 -2.3744800000000001 0.68506 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.288 1.4219999999999999 1.288</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='boxes8'>
+          <pose>5.95268 -2.3744800000000001 0.68506 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.288 1.4219999999999999 1.288</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='boxes9'>
+          <pose>8.5588700000000006 -2.3744800000000001 0.68506 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.288 1.4219999999999999 1.288</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='boxes10'>
+          <pose>11.326000000000001 -2.3744800000000001 0.68506 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.288 1.4219999999999999 1.288</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='boxes11'>
+          <pose>-1.2267999999999999 4.1557000000000004 0.68506 0 0 -1.0279989300000001</pose>
+          <geometry>
+            <box>
+              <size>1.288 1.4219999999999999 1.288</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='pilar1'>
+          <pose>-7.5401999999999996 3.6151 1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.46500000000000002 0.46500000000000002 2</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='pilar2'>
+          <pose>7.4574999999999996 3.6151 1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.46500000000000002 0.46500000000000002 2</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='pilar3'>
+          <pose>-7.5401999999999996 -3.8856999999999999 1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.46500000000000002 0.46500000000000002 2</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='pilar4'>
+          <pose>7.4574999999999996 -3.8856999999999999 1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.46500000000000002 0.46500000000000002 2</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='pallet_mover1'>
+          <pose>-0.61439999999999995 -2.3889999999999998 0.41837999999999997 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.36299999999999999 0.44 0.71899999999999997</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='pallet_mover2'>
+          <pose>-1.6004 4.8224999999999998 0.41837999999999997 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.36299999999999999 0.24399999999999999 0.71899999999999997</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='stairs'>
+          <pose>13.018000000000001 3.1652 0.25 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.2989999999999999 0.59999999999999998 0.5</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs1'>
+          <pose>1.4661999999999999 -0.017559000000000002 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs2'>
+          <pose>2.6482999999999999 -0.017559000000000002 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs3'>
+          <pose>5.3247 -0.017559000000000002 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs4'>
+          <pose>6.5063000000000004 -0.017559000000000002 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs5'>
+          <pose>9.0757999999999992 -0.017559000000000002 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs6'>
+          <pose>10.257999999999999 -0.017559000000000002 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs7'>
+          <pose>1.4661999999999999 2.5663999999999998 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs8'>
+          <pose>2.6482999999999999 2.5663999999999998 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs9'>
+          <pose>5.3247 2.5663999999999998 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs10'>
+          <pose>6.5063000000000004 2.5663999999999998 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs11'>
+          <pose>9.0757999999999992 2.5663999999999998 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs12'>
+          <pose>10.257999999999999 2.5663999999999998 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs13'>
+          <pose>1.4661999999999999 5.1497000000000002 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs14'>
+          <pose>2.6482999999999999 5.1497000000000002 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs15'>
+          <pose>5.3247 5.1497000000000002 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs16'>
+          <pose>6.5063000000000004 5.1497000000000002 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs17'>
+          <pose>9.0757999999999992 5.1497000000000002 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <collision name='shelfs18'>
+          <pose>10.257999999999999 5.1497000000000002 0.5 0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.029999999999999999</radius>
+              <length>1</length>
+            </cylinder>
+          </geometry>
+          <surface>
+            <friction>
+              <ode/>
+            </friction>
+            <bounce/>
+            <contact/>
+          </surface>
+        </collision>
+        <inertial>
+          <pose>0 0 0 0 0 0</pose>
+          <mass>1</mass>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+        </inertial>
+        <enable_wind>false</enable_wind>
+      </link>
+      <self_collide>false</self_collide>
+    </model>
+  </world>
+</sdf>
diff --git a/ros2_grasp_service_demo/manipulator_simulation/worlds/default_world.world b/ros2_grasp_service_demo/manipulator_simulation/worlds/default_world.world
deleted file mode 100644
index 10cdf3f..0000000
--- a/ros2_grasp_service_demo/manipulator_simulation/worlds/default_world.world
+++ /dev/null
@@ -1,35 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.5">
-  <world name="default">
-    <gui>
-      <camera name="user_camera">
-        <pose>-2.0 0.0 3.0 0.0 0.8 0.0</pose>
-      </camera>
-    </gui>
-    <include>
-      <scale>3.0</scale>
-      <uri>model://ground_plane</uri>
-    </include>
-    <include>
-      <scale>3.0</scale>
-      <uri>model://asphalt_plane</uri>
-    </include>
-    <include>
-      <uri>model://sun</uri>
-    </include>
-
-    <include>
-      <scale>1.0</scale>
-      <uri>model://big_box</uri>
-      <name>big_box_0</name>
-      <pose>-7.0 -2.0 0.0 0 0 0</pose>
-    </include>
-
-    <include>
-      <scale>1.0</scale>
-      <uri>model://big_box</uri>
-      <name>big_box_1</name>
-      <pose>-2.0 -2.0 0.0 0 0 0</pose>
-    </include>
-  </world>
-</sdf>
diff --git a/ros2_grasp_service_demo/open_manipulator/open_manipulator_description/urdf/ros2_control.xacro b/ros2_grasp_service_demo/open_manipulator/open_manipulator_description/urdf/ros2_control.xacro
index 4731419..596acc3 100644
--- a/ros2_grasp_service_demo/open_manipulator/open_manipulator_description/urdf/ros2_control.xacro
+++ b/ros2_grasp_service_demo/open_manipulator/open_manipulator_description/urdf/ros2_control.xacro
@@ -2,7 +2,7 @@
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
 
   <gazebo>
-    <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+    <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
       <parameters>$(find open_manipulator_control)/config/controllers.yaml</parameters>
     </plugin>
   </gazebo>
@@ -19,7 +19,7 @@
 
   <ros2_control name="open_manipulator" type="system">
     <hardware>
-      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
     </hardware>
 
     <xacro:manipulator_joint_command_by_position name="joint1" initial_value="0"/>
@@ -34,6 +34,7 @@
     <joint name="gripper_sub">
         <param name="mimic">gripper</param>
         <param name="multiplier">1</param>
+        <command_interface name="position"/>
         <state_interface name="position"/>
         <state_interface name="velocity"/>
     </joint>