From 30f3f9437c5ccd9a33f77ec5873cc35995c5a7ee Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Denis=20=C5=A0togl?= <denis@stogl.de>
Date: Fri, 28 May 2021 18:00:34 +0200
Subject: [PATCH] Move all files

---
 .../config/diffbot_diff_drive_controller.yaml |   0
 ...ot_gazebo_forward_controller_position.yaml |   0
 ...t_multi_interface_forward_controllers.yaml |   0
 .../env-hooks/ros2_control_demo_robot.dsv.in  |   0
 .../launch/diffbot_system.launch.py           |   0
 .../launch/rrbot.launch.py                    |  15 +-
 .../rrbot_system_multi_interface.launch.py    |  81 +++++++
 ...rbot_system_position_only_gazebo.launch.py | 105 +++++++++
 .../meshes/hokuyo.dae                         |   0
 .../urdf}/diffbot_system.urdf.xacro           |   0
 ..._system_multi_interface.ros2_control.xacro |  30 ++-
 .../rrbot_system_multi_interface.urdf.xacro   |  46 ++++
 ...ot_system_position_only.ros2_control.xacro |  40 ----
 .../description/rrbot/urdf/rrbot.urdf.xacro   | 204 ------------------
 .../rrbot_system_multi_interface.urdf.xacro   |  38 ----
 .../rrbot_system_multi_interface.launch.py    |  52 -----
 ...rbot_system_position_only_gazebo.launch.py |  77 -------
 17 files changed, 269 insertions(+), 419 deletions(-)
 rename {ros2_control_demo_robot => ros2_control_demo_bringup}/config/diffbot_diff_drive_controller.yaml (100%)
 rename {ros2_control_demo_robot => ros2_control_demo_bringup}/config/rrbot_gazebo_forward_controller_position.yaml (100%)
 rename {ros2_control_demo_robot => ros2_control_demo_bringup}/config/rrbot_multi_interface_forward_controllers.yaml (100%)
 rename {ros2_control_demo_robot => ros2_control_demo_bringup}/env-hooks/ros2_control_demo_robot.dsv.in (100%)
 rename {ros2_control_demo_robot => ros2_control_demo_bringup}/launch/diffbot_system.launch.py (100%)
 create mode 100644 ros2_control_demo_bringup/launch/rrbot_system_multi_interface.launch.py
 create mode 100644 ros2_control_demo_bringup/launch/rrbot_system_position_only_gazebo.launch.py
 rename {ros2_control_demo_robot/description/rrbot => ros2_control_demo_description}/meshes/hokuyo.dae (100%)
 rename {ros2_control_demo_robot/description => ros2_control_demo_description/urdf}/diffbot_system.urdf.xacro (100%)
 rename {ros2_control_demo_robot/description/rrbot => ros2_control_demo_description/urdf/rrbot/ros2_control}/rrbot_system_multi_interface.ros2_control.xacro (59%)
 create mode 100644 ros2_control_demo_description/urdf/rrbot_system_multi_interface.urdf.xacro
 delete mode 100644 ros2_control_demo_robot/description/rrbot/ros2_control/rrbot_system_position_only.ros2_control.xacro
 delete mode 100644 ros2_control_demo_robot/description/rrbot/urdf/rrbot.urdf.xacro
 delete mode 100644 ros2_control_demo_robot/description/rrbot_system_multi_interface.urdf.xacro
 delete mode 100644 ros2_control_demo_robot/launch/rrbot_system_multi_interface.launch.py
 delete mode 100644 ros2_control_demo_robot/launch/rrbot_system_position_only_gazebo.launch.py

diff --git a/ros2_control_demo_robot/config/diffbot_diff_drive_controller.yaml b/ros2_control_demo_bringup/config/diffbot_diff_drive_controller.yaml
similarity index 100%
rename from ros2_control_demo_robot/config/diffbot_diff_drive_controller.yaml
rename to ros2_control_demo_bringup/config/diffbot_diff_drive_controller.yaml
diff --git a/ros2_control_demo_robot/config/rrbot_gazebo_forward_controller_position.yaml b/ros2_control_demo_bringup/config/rrbot_gazebo_forward_controller_position.yaml
similarity index 100%
rename from ros2_control_demo_robot/config/rrbot_gazebo_forward_controller_position.yaml
rename to ros2_control_demo_bringup/config/rrbot_gazebo_forward_controller_position.yaml
diff --git a/ros2_control_demo_robot/config/rrbot_multi_interface_forward_controllers.yaml b/ros2_control_demo_bringup/config/rrbot_multi_interface_forward_controllers.yaml
similarity index 100%
rename from ros2_control_demo_robot/config/rrbot_multi_interface_forward_controllers.yaml
rename to ros2_control_demo_bringup/config/rrbot_multi_interface_forward_controllers.yaml
diff --git a/ros2_control_demo_robot/env-hooks/ros2_control_demo_robot.dsv.in b/ros2_control_demo_bringup/env-hooks/ros2_control_demo_robot.dsv.in
similarity index 100%
rename from ros2_control_demo_robot/env-hooks/ros2_control_demo_robot.dsv.in
rename to ros2_control_demo_bringup/env-hooks/ros2_control_demo_robot.dsv.in
diff --git a/ros2_control_demo_robot/launch/diffbot_system.launch.py b/ros2_control_demo_bringup/launch/diffbot_system.launch.py
similarity index 100%
rename from ros2_control_demo_robot/launch/diffbot_system.launch.py
rename to ros2_control_demo_bringup/launch/diffbot_system.launch.py
diff --git a/ros2_control_demo_bringup/launch/rrbot.launch.py b/ros2_control_demo_bringup/launch/rrbot.launch.py
index 3d54ab41b..1082fed3d 100644
--- a/ros2_control_demo_bringup/launch/rrbot.launch.py
+++ b/ros2_control_demo_bringup/launch/rrbot.launch.py
@@ -62,6 +62,13 @@ def generate_launch_description():
         have to be updated.",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_sim",
+            default_value="false",
+            description="Start robot in Gazebo simulation.",
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "use_fake_hardware",
@@ -96,6 +103,7 @@ def generate_launch_description():
     description_package = LaunchConfiguration("description_package")
     description_file = LaunchConfiguration("description_file")
     prefix = LaunchConfiguration("prefix")
+    use_sim = LaunchConfiguration("use_sim")
     use_fake_hardware = LaunchConfiguration("use_fake_hardware")
     fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
     slowdown = LaunchConfiguration("slowdown")
@@ -106,11 +114,16 @@ def generate_launch_description():
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
-            PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
+            PathJoinSubstitution(
+                [FindPackageShare(description_package), "urdf", description_file]
+            ),
             " ",
             "prefix:=",
             prefix,
             " ",
+            "use_sim:=",
+            use_sim,
+            " ",
             "use_fake_hardware:=",
             use_fake_hardware,
             " ",
diff --git a/ros2_control_demo_bringup/launch/rrbot_system_multi_interface.launch.py b/ros2_control_demo_bringup/launch/rrbot_system_multi_interface.launch.py
new file mode 100644
index 000000000..aa67787f2
--- /dev/null
+++ b/ros2_control_demo_bringup/launch/rrbot_system_multi_interface.launch.py
@@ -0,0 +1,81 @@
+# Copyright 2021 Department of Engineering Cybernetics, NTNU.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "prefix",
+            default_value='""',
+            description="Prefix of the joint names, useful for \
+        multi-robot setup. If changed than also joint names in the controllers' configuration \
+        have to be updated.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="true",
+            description="Start robot with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "fake_sensor_commands",
+            default_value="false",
+            description="Enable fake command interfaces for sensors used for simple simulations. \
+            Used only if 'use_fake_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "slowdown", default_value="3.0", description="Slowdown factor of the RRbot."
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_controller",
+            default_value="forward_position_controller",
+            description="Robot controller to start.",
+        )
+    )
+
+    # Initialize Arguments
+    prefix = LaunchConfiguration("prefix")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    slowdown = LaunchConfiguration("slowdown")
+    robot_controller = LaunchConfiguration("robot_controller")
+
+    base_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot.launch.py"]),
+        launch_arguments={
+            "controllers_file": "rrbot_multi_interface_forward_controllers.yaml",
+            "description_file": "rrbot_system_multi_interface.urdf.xacro",
+            "prefix": prefix,
+            "use_fake_hardware": use_fake_hardware,
+            "fake_sensor_commands": fake_sensor_commands,
+            "slowdown": slowdown,
+            "robot_controller": robot_controller,
+        }.items(),
+    )
+
+    return LaunchDescription(declared_arguments + [base_launch])
diff --git a/ros2_control_demo_bringup/launch/rrbot_system_position_only_gazebo.launch.py b/ros2_control_demo_bringup/launch/rrbot_system_position_only_gazebo.launch.py
new file mode 100644
index 000000000..978ac84f4
--- /dev/null
+++ b/ros2_control_demo_bringup/launch/rrbot_system_position_only_gazebo.launch.py
@@ -0,0 +1,105 @@
+# Copyright 2021 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, ThisLaunchFileDir
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "prefix",
+            default_value='""',
+            description="Prefix of the joint names, useful for \
+        multi-robot setup. If changed than also joint names in the controllers' configuration \
+        have to be updated.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="true",
+            description="Start robot with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "fake_sensor_commands",
+            default_value="false",
+            description="Enable fake command interfaces for sensors used for simple simulations. \
+            Used only if 'use_fake_hardware' parameter is true.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "slowdown", default_value="3.0", description="Slowdown factor of the RRbot."
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_controller",
+            default_value="forward_position_controller",
+            description="Robot controller to start.",
+        )
+    )
+
+    # Initialize Arguments
+    prefix = LaunchConfiguration("prefix")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    slowdown = LaunchConfiguration("slowdown")
+    robot_controller = LaunchConfiguration("robot_controller")
+
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("gazebo_ros"),
+                        "launch",
+                        "/gazebo.launch.py",
+                    ]
+                )
+            ]
+        ),
+        launch_arguments={"verbose": "false"}.items(),
+    )
+
+    spawn_entity = Node(
+        package="gazebo_ros",
+        executable="spawn_entity.py",
+        arguments=["-topic", "robot_description", "-entity", "rrbot_system_position"],
+        output="screen",
+    )
+
+    base_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/rrbot.launch.py"]),
+        launch_arguments={
+            "description_file": "rrbot_system_position_only.urdf.xacro",
+            "prefix": prefix,
+            "use_fake_hardware": use_fake_hardware,
+            "fake_sensor_commands": fake_sensor_commands,
+            "slowdown": slowdown,
+            "robot_controller": robot_controller,
+        }.items(),
+    )
+
+    return LaunchDescription(declared_arguments + [gazebo, base_launch, spawn_entity])
diff --git a/ros2_control_demo_robot/description/rrbot/meshes/hokuyo.dae b/ros2_control_demo_description/meshes/hokuyo.dae
similarity index 100%
rename from ros2_control_demo_robot/description/rrbot/meshes/hokuyo.dae
rename to ros2_control_demo_description/meshes/hokuyo.dae
diff --git a/ros2_control_demo_robot/description/diffbot_system.urdf.xacro b/ros2_control_demo_description/urdf/diffbot_system.urdf.xacro
similarity index 100%
rename from ros2_control_demo_robot/description/diffbot_system.urdf.xacro
rename to ros2_control_demo_description/urdf/diffbot_system.urdf.xacro
diff --git a/ros2_control_demo_robot/description/rrbot/rrbot_system_multi_interface.ros2_control.xacro b/ros2_control_demo_description/urdf/rrbot/ros2_control/rrbot_system_multi_interface.ros2_control.xacro
similarity index 59%
rename from ros2_control_demo_robot/description/rrbot/rrbot_system_multi_interface.ros2_control.xacro
rename to ros2_control_demo_description/urdf/rrbot/ros2_control/rrbot_system_multi_interface.ros2_control.xacro
index 60d7b46d2..5ca0251b6 100644
--- a/ros2_control_demo_robot/description/rrbot/rrbot_system_multi_interface.ros2_control.xacro
+++ b/ros2_control_demo_description/urdf/rrbot/ros2_control/rrbot_system_multi_interface.ros2_control.xacro
@@ -1,15 +1,31 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <xacro:macro name="rrbot_system_multi_interface" params="name prefix">
+  <xacro:macro name="rrbot_system_multi_interface" params="name prefix use_sim:=^|false use_fake_hardware:=^|true fake_sensor_commands:=^|false slowdown:=2.0">
 
     <ros2_control name="${name}" type="system">
-      <hardware>
-        <plugin>ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware</plugin>
-        <param name="example_param_hw_start_duration_sec">2.0</param>
-        <param name="example_param_hw_stop_duration_sec">3.0</param>
-        <param name="example_param_hw_slowdown">2.0</param>
-      </hardware>
+
+      <xacro:if value="$(arg use_sim)">
+        <hardware>
+          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+        </hardware>
+      </xacro:if>
+      <xacro:unless value="$(arg use_sim)">
+        <hardware>
+          <xacro:if value="${use_fake_hardware}">
+            <plugin>fake_components/GenericSystem</plugin>
+            <param name="fake_sensor_commands">${fake_sensor_commands}</param>
+            <param name="state_following_offset">0.0</param>
+          </xacro:if>
+          <xacro:unless value="${use_fake_hardware}">
+            <plugin>ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware</plugin>
+            <param name="example_param_hw_start_duration_sec">2.0</param>
+            <param name="example_param_hw_stop_duration_sec">3.0</param>
+            <param name="example_param_hw_slowdown">${slowdown}</param>
+          </xacro:unless>
+        </hardware>
+      </xacro:unless>
+
       <joint name="joint1">
         <command_interface name="position">
           <param name="min">-1</param>
diff --git a/ros2_control_demo_description/urdf/rrbot_system_multi_interface.urdf.xacro b/ros2_control_demo_description/urdf/rrbot_system_multi_interface.urdf.xacro
new file mode 100644
index 000000000..ce1a243af
--- /dev/null
+++ b/ros2_control_demo_description/urdf/rrbot_system_multi_interface.urdf.xacro
@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<!-- Revolute-Revolute Manipulator -->
+<!--
+Copied and modified from ROS1 example:
+https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="2dof_robot">
+  <xacro:arg name="use_sim" default="false" />
+
+  <!-- Enable setting arguments from the launch file -->
+  <xacro:arg name="use_fake_hardware" default="true" />
+  <xacro:arg name="fake_sensor_commands" default="false" />
+  <xacro:arg name="prefix" default="" />
+  <xacro:arg name="slowdown" default="2.0" />
+
+  <!-- Import RRBot macro -->
+  <xacro:include filename="$(find ros2_control_demo_description)/urdf/rrbot/rrbot.urdf.xacro" />
+
+  <!-- Import all Gazebo-customization elements, including Gazebo colors -->
+  <xacro:include filename="$(find ros2_control_demo_description)/urdf/rrbot/gazebo/rrbot.gazebo.xacro" />
+
+  <!-- Import Rviz colors -->
+  <xacro:include filename="$(find ros2_control_demo_description)/urdf/rrbot/gazebo/rrbot.materials.xacro" />
+
+  <!-- Import RRBot ros2_control description -->
+  <xacro:include filename="$(find ros2_control_demo_description)/urdf/rrbot/ros2_control/rrbot_system_position_only.ros2_control.xacro" />
+
+  <!-- Used for fixing robot -->
+  <link name="world"/>
+  <gazebo reference="world">
+    <static>true</static>
+  </gazebo>
+
+  <xacro:rrbot parent="world" prefix="$(arg prefix)">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </xacro:rrbot>
+
+  <xacro:rrbot_gazebo prefix="$(arg prefix)" />
+
+  <xacro:rrbot_system_multi_interface
+    name="RRBotSystemMultiInterface" prefix="$(arg prefix)"
+    use_fake_hardware="$(arg use_fake_hardware)"
+    fake_sensor_commands="$(arg fake_sensor_commands)"
+    slowdown="$(arg slowdown)" />
+
+</robot>
diff --git a/ros2_control_demo_robot/description/rrbot/ros2_control/rrbot_system_position_only.ros2_control.xacro b/ros2_control_demo_robot/description/rrbot/ros2_control/rrbot_system_position_only.ros2_control.xacro
deleted file mode 100644
index 628dd2c4d..000000000
--- a/ros2_control_demo_robot/description/rrbot/ros2_control/rrbot_system_position_only.ros2_control.xacro
+++ /dev/null
@@ -1,40 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-  <xacro:macro name="rrbot_system_position_only" params="name prefix use_sim:='false' slowdown:=2.0">
-
-    <ros2_control name="${name}" type="system">
-
-      <xacro:if value="$(arg use_sim)">
-        <hardware>
-          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
-        </hardware>
-      </xacro:if>
-      <xacro:unless value="$(arg use_sim)">
-        <hardware>
-          <plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
-          <param name="example_param_hw_start_duration_sec">2.0</param>
-          <param name="example_param_hw_stop_duration_sec">3.0</param>
-          <param name="example_param_hw_slowdown">${slowdown}</param>
-        </hardware>
-      </xacro:unless>
-
-      <joint name="joint1">
-        <command_interface name="position">
-          <param name="min">-1</param>
-          <param name="max">1</param>
-        </command_interface>
-        <state_interface name="position"/>
-      </joint>
-      <joint name="joint2">
-        <command_interface name="position">
-          <param name="min">-1</param>
-          <param name="max">1</param>
-        </command_interface>
-        <state_interface name="position"/>
-      </joint>
-    </ros2_control>
-
-  </xacro:macro>
-
-</robot>
diff --git a/ros2_control_demo_robot/description/rrbot/urdf/rrbot.urdf.xacro b/ros2_control_demo_robot/description/rrbot/urdf/rrbot.urdf.xacro
deleted file mode 100644
index 97031e426..000000000
--- a/ros2_control_demo_robot/description/rrbot/urdf/rrbot.urdf.xacro
+++ /dev/null
@@ -1,204 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
-
-  <xacro:macro name="rrbot" params="parent prefix *origin">
-
-    <!-- Constants for robot dimensions -->
-    <xacro:property name="mass" value="1" /> <!-- arbitrary value for mass -->
-    <xacro:property name="width" value="0.1" /> <!-- Square dimensions (widthxwidth) of beams -->
-    <xacro:property name="height1" value="2" /> <!-- Link 1 -->
-    <xacro:property name="height2" value="1" /> <!-- Link 2 -->
-    <xacro:property name="height3" value="1" /> <!-- Link 3 -->
-    <xacro:property name="camera_link" value="0.05" /> <!-- Size of square 'camera' box -->
-    <xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
-
-    <joint name="${prefix}base_joint" type="fixed">
-      <xacro:insert_block name="origin" />
-      <parent link="${parent}"/>
-      <child link="${prefix}base_link" />
-    </joint>
-
-    <!-- Base Link -->
-    <link name="${prefix}base_link">
-      <collision>
-        <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
-        <geometry>
-          <box size="${width} ${width} ${height1}"/>
-        </geometry>
-      </collision>
-
-      <visual>
-        <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
-        <geometry>
-          <box size="${width} ${width} ${height1}"/>
-        </geometry>
-        <material name="orange"/>
-      </visual>
-
-      <inertial>
-        <origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
-        <mass value="${mass}"/>
-        <inertia
-          ixx="${mass / 12.0 * (width*width + height1*height1)}" ixy="0.0" ixz="0.0"
-          iyy="${mass / 12.0 * (height1*height1 + width*width)}" iyz="0.0"
-          izz="${mass / 12.0 * (width*width + width*width)}"/>
-      </inertial>
-    </link>
-
-    <joint name="${prefix}joint1" type="continuous">
-      <parent link="${prefix}base_link"/>
-      <child link="${prefix}link1"/>
-      <origin xyz="0 ${width} ${height1 - axel_offset}" rpy="0 0 0"/>
-      <axis xyz="0 1 0"/>
-      <dynamics damping="0.7"/>
-    </joint>
-
-    <!-- Middle Link -->
-    <link name="${prefix}link1">
-      <collision>
-        <origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
-        <geometry>
-          <box size="${width} ${width} ${height2}"/>
-        </geometry>
-      </collision>
-
-      <visual>
-        <origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
-        <geometry>
-          <box size="${width} ${width} ${height2}"/>
-        </geometry>
-        <material name="yellow"/>
-      </visual>
-
-      <inertial>
-        <origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
-        <mass value="${mass}"/>
-        <inertia
-          ixx="${mass / 12.0 * (width*width + height2*height2)}" ixy="0.0" ixz="0.0"
-          iyy="${mass / 12.0 * (height2*height2 + width*width)}" iyz="0.0"
-          izz="${mass / 12.0 * (width*width + width*width)}"/>
-      </inertial>
-    </link>
-
-    <joint name="${prefix}joint2" type="continuous">
-      <parent link="${prefix}link1"/>
-      <child link="${prefix}link2"/>
-      <origin xyz="0 ${width} ${height2 - axel_offset*2}" rpy="0 0 0"/>
-      <axis xyz="0 1 0"/>
-      <dynamics damping="0.7"/>
-    </joint>
-
-    <!-- Top Link -->
-    <link name="${prefix}link2">
-      <collision>
-        <origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
-        <geometry>
-          <box size="${width} ${width} ${height3}"/>
-        </geometry>
-      </collision>
-
-      <visual>
-        <origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
-        <geometry>
-          <box size="${width} ${width} ${height3}"/>
-        </geometry>
-        <material name="orange"/>
-      </visual>
-
-      <inertial>
-        <origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
-        <mass value="${mass}"/>
-        <inertia
-          ixx="${mass / 12.0 * (width*width + height3*height3)}" ixy="0.0" ixz="0.0"
-          iyy="${mass / 12.0 * (height3*height3 + width*width)}" iyz="0.0"
-          izz="${mass / 12.0 * (width*width + width*width)}"/>
-      </inertial>
-    </link>
-
-    <joint name="${prefix}tool_joint" type="fixed">
-      <origin xyz="0 0 1" rpy="0 0 0" />
-      <parent link="${prefix}link2"/>
-      <child link="${prefix}tool_link" />
-    </joint>
-
-    <!-- Tool Link -->
-    <link name="${prefix}tool_link">
-    </link>
-
-    <joint name="${prefix}hokuyo_joint" type="fixed">
-      <axis xyz="0 1 0" />
-      <origin xyz="0 0 ${height3 - axel_offset/2}" rpy="0 0 0"/>
-      <parent link="${prefix}link2"/>
-      <child link="${prefix}hokuyo_link"/>
-    </joint>
-
-    <!-- Hokuyo Laser -->
-    <link name="${prefix}hokuyo_link">
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.1 0.1 0.1"/>
-        </geometry>
-      </collision>
-
-      <visual>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <geometry>
-          <mesh filename="package://ros2_control_demo_robot/description/rrbot/meshes/hokuyo.dae"/>
-        </geometry>
-      </visual>
-
-      <inertial>
-        <mass value="1e-5" />
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
-      </inertial>
-    </link>
-
-    <joint name="${prefix}camera_joint" type="fixed">
-      <axis xyz="0 1 0" />
-      <origin xyz="${camera_link} 0 ${height3 - axel_offset*2}" rpy="0 0 0"/>
-      <parent link="${prefix}link2"/>
-      <child link="${prefix}camera_link"/>
-    </joint>
-
-    <!-- Camera -->
-    <link name="${prefix}camera_link">
-      <collision>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <geometry>
-          <box size="${camera_link} ${camera_link} ${camera_link}"/>
-        </geometry>
-      </collision>
-
-      <visual>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <geometry>
-          <box size="${camera_link} ${camera_link} ${camera_link}"/>
-        </geometry>
-        <material name="red"/>
-      </visual>
-
-      <inertial>
-        <mass value="1e-5" />
-        <origin xyz="0 0 0" rpy="0 0 0"/>
-        <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
-      </inertial>
-    </link>
-
-    <!-- generate an optical frame http://www.ros.org/reps/rep-0103.html#suffix-frames
-    so that ros and opencv can operate on the camera frame correctly -->
-    <joint name="${prefix}camera_optical_joint" type="fixed">
-      <!-- these values have to be these values otherwise the gazebo camera image
-      won't be aligned properly with the frame it is supposedly originating from -->
-      <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
-      <parent link="${prefix}camera_link"/>
-      <child link="${prefix}camera_link_optical"/>
-    </joint>
-
-    <link name="${prefix}camera_link_optical">
-    </link>
-
-  </xacro:macro>
-
-</robot>
diff --git a/ros2_control_demo_robot/description/rrbot_system_multi_interface.urdf.xacro b/ros2_control_demo_robot/description/rrbot_system_multi_interface.urdf.xacro
deleted file mode 100644
index 6a330bb4c..000000000
--- a/ros2_control_demo_robot/description/rrbot_system_multi_interface.urdf.xacro
+++ /dev/null
@@ -1,38 +0,0 @@
-<?xml version="1.0"?>
-<!-- Revolute-Revolute Manipulator -->
-<!--
-Copied and modified from ROS1 example:
-https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro
--->
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="2dof_robot">
-
-  <!-- Import RRBot macro -->
-  <xacro:include filename="rrbot/rrbot.urdf.xacro" />
-  <!-- <xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot.urdf.xacro" /> -->
-
-  <!-- Import all Gazebo-customization elements, including Gazebo colors -->
-  <xacro:include filename="rrbot/rrbot.gazebo.xacro" />
-  <!-- <xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot.gazebo.xacro" /> -->
-
-  <!-- Import Rviz colors -->
-  <xacro:include filename="rrbot/rrbot.materials.xacro" />
-  <!-- <xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot.materials.xacro" /> -->
-
-  <!-- Import RRBot ros2_control description -->
-  <xacro:include filename="rrbot/rrbot_system_multi_interface.ros2_control.xacro" />
-
-  <!-- Used for fixing robot -->
-  <link name="world"/>
-  <gazebo reference="world">
-    <static>true</static>
-  </gazebo>
-
-  <xacro:rrbot parent="world" prefix="">
-    <origin xyz="0 0 0" rpy="0 0 0" />
-  </xacro:rrbot>
-
-  <xacro:rrbot_gazebo prefix="" />
-
-  <xacro:rrbot_system_multi_interface name="RRBotSystemMultiInterface" prefix="" />
-
-</robot>
diff --git a/ros2_control_demo_robot/launch/rrbot_system_multi_interface.launch.py b/ros2_control_demo_robot/launch/rrbot_system_multi_interface.launch.py
deleted file mode 100644
index 59f202ebd..000000000
--- a/ros2_control_demo_robot/launch/rrbot_system_multi_interface.launch.py
+++ /dev/null
@@ -1,52 +0,0 @@
-# Copyright 2021 Department of Engineering Cybernetics, NTNU.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-import xacro
-
-
-def generate_launch_description():
-
-    # Get URDF via xacro
-    robot_description_path = os.path.join(
-        get_package_share_directory("ros2_control_demo_robot"),
-        "description",
-        "rrbot_system_multi_interface.urdf.xacro",
-    )
-    robot_description_config = xacro.process_file(robot_description_path)
-    robot_description = {"robot_description": robot_description_config.toxml()}
-
-    rrbot_forward_controller = os.path.join(
-        get_package_share_directory("ros2_control_demo_robot"),
-        "config",
-        "rrbot_multi_interface_forward_controllers.yaml",
-    )
-
-    return LaunchDescription(
-        [
-            Node(
-                package="controller_manager",
-                executable="ros2_control_node",
-                parameters=[robot_description, rrbot_forward_controller],
-                output={"stdout": "screen", "stderr": "screen"},
-            )
-        ]
-    )
diff --git a/ros2_control_demo_robot/launch/rrbot_system_position_only_gazebo.launch.py b/ros2_control_demo_robot/launch/rrbot_system_position_only_gazebo.launch.py
deleted file mode 100644
index 45cc547c7..000000000
--- a/ros2_control_demo_robot/launch/rrbot_system_position_only_gazebo.launch.py
+++ /dev/null
@@ -1,77 +0,0 @@
-# Copyright 2021 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-
-from launch_ros.actions import Node
-
-import xacro
-
-
-def generate_launch_description():
-    gazebo = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [
-                os.path.join(get_package_share_directory("gazebo_ros"), "launch"),
-                "/gazebo.launch.py",
-            ]
-        ),
-        launch_arguments={"verbose": "false"}.items(),
-    )
-
-    robot_description_path = os.path.join(
-        get_package_share_directory("ros2_control_demo_robot"),
-        "description",
-        "rrbot",
-        "rrbot_system_position_only.urdf.xacro",
-    )
-    robot_description_config = xacro.process_file(
-        robot_description_path, mappings={"use_sim": "true"}
-    )
-    robot_description = {"robot_description": robot_description_config.toxml()}
-
-    node_robot_state_publisher = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        output="screen",
-        parameters=[robot_description],
-    )
-
-    spawn_entity = Node(
-        package="gazebo_ros",
-        executable="spawn_entity.py",
-        arguments=["-topic", "robot_description", "-entity", "rrbot_system_position"],
-        output="screen",
-    )
-    spawn_controller = Node(
-        package="controller_manager",
-        executable="spawner.py",
-        arguments=["joint_state_broadcaster"],
-        output="screen",
-    )
-
-    return LaunchDescription(
-        [
-            gazebo,
-            node_robot_state_publisher,
-            spawn_entity,
-            spawn_controller,
-        ]
-    )