diff --git a/kuka_experimental/CMakeLists.txt b/kuka_experimental/CMakeLists.txt
index 8dbd255ce..2c84d20f4 100644
--- a/kuka_experimental/CMakeLists.txt
+++ b/kuka_experimental/CMakeLists.txt
@@ -1,4 +1,7 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.16)
+
 project(kuka_experimental)
-find_package(catkin REQUIRED)
-catkin_metapackage()
+
+find_package(ament_cmake REQUIRED)
+
+ament_package()
diff --git a/kuka_experimental/package.xml b/kuka_experimental/package.xml
index c9c20c0ce..811f572ff 100644
--- a/kuka_experimental/package.xml
+++ b/kuka_experimental/package.xml
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>kuka_experimental</name>
   <version>0.1.0</version>
   <description>Experimental packages for KUKA manipulators within ROS-Industrial.</description>
@@ -11,22 +11,14 @@
   <url type="bugtracker">https://github.com/ros-industrial/kuka_experimental/issues</url>
   <url type="repository">https://github.com/ros-industrial/kuka_experimental</url>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <exec_depend>kuka_kr10_support</exec_depend>
-  <exec_depend>kuka_kr120_support</exec_depend>
-  <exec_depend>kuka_kr150_support</exec_depend>
-  <exec_depend>kuka_kr16_support</exec_depend>
-  <exec_depend>kuka_kr210_support</exec_depend>
-  <exec_depend>kuka_kr5_support</exec_depend>
   <exec_depend>kuka_kr6_support</exec_depend>
-  <exec_depend>kuka_lbr_iiwa_support</exec_depend>
   <exec_depend>kuka_resources</exec_depend>
-  <exec_depend>kuka_eki_hw_interface</exec_depend>
-  <exec_depend>kuka_rsi_hw_interface</exec_depend>
+  <!--<exec_depend>kuka_rsi_hw_interface</exec_depend>-->
   <exec_depend>kuka_rsi_simulator</exec_depend>
 
   <export>
-    <metapackage/>
+	<build_type>ament_cmake</build_type>
   </export>
 </package>
diff --git a/kuka_kr16_support/CMakeLists.txt b/kuka_kr16_support/CMakeLists.txt
index 2e76269e4..770599dc6 100644
--- a/kuka_kr16_support/CMakeLists.txt
+++ b/kuka_kr16_support/CMakeLists.txt
@@ -1,17 +1,16 @@
-cmake_minimum_required(VERSION 3.16)
+cmake_minimum_required(VERSION 3.5)
 
-project(kuka_kr16_support LANGUAGES CXX)
-
-if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
-  add_compile_options(-Wall -Wextra)
-endif()
+project(kuka_kr16_support)
 
 find_package(ament_cmake REQUIRED)
 
+if (CATKIN_ENABLE_TESTING)
+  find_package(roslaunch REQUIRED)
+  roslaunch_add_file_check(test/roslaunch_test.xml)
+endif()
 
-install(
-  DIRECTORY config launch meshes urdf
+install(DIRECTORY config launch meshes urdf
   DESTINATION share/${PROJECT_NAME}
-  )
+)
 
-  ament_package()
+ament_package()
\ No newline at end of file
diff --git a/kuka_kr16_support/config/rviz/view_robot_kr16_2.rviz b/kuka_kr16_support/config/rviz/view_robot_kr16_2.rviz
new file mode 100644
index 000000000..77b895b52
--- /dev/null
+++ b/kuka_kr16_support/config/rviz/view_robot_kr16_2.rviz
@@ -0,0 +1,249 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 745
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base:
+          Value: true
+        base_link:
+          Value: true
+        flange:
+          Value: true
+        link_1:
+          Value: true
+        link_2:
+          Value: true
+        link_3:
+          Value: true
+        link_4:
+          Value: true
+        link_5:
+          Value: true
+        link_6:
+          Value: true
+        tool0:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        base_link:
+          base:
+            {}
+          link_1:
+            link_2:
+              link_3:
+                link_4:
+                  link_5:
+                    link_6:
+                      flange:
+                        tool0:
+                          {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        flange:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tool0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 4.382266521453857
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.4607224464416504
+        Y: 0.24497589468955994
+        Z: 0.1281789243221283
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.155398428440094
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 5.303589820861816
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1036
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000372fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000372000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000372fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000372000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077c0000003efc0100000002fb0000000800540069006d006501000000000000077c0000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000050b0000037200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1916
+  X: 0
+  Y: 20
diff --git a/kuka_kr16_support/launch/view_kr16_2.launch.py b/kuka_kr16_support/launch/view_kr16_2.launch.py
new file mode 100644
index 000000000..7ef0cb4eb
--- /dev/null
+++ b/kuka_kr16_support/launch/view_kr16_2.launch.py
@@ -0,0 +1,71 @@
+# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt)
+#
+# 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 RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("kuka_kr16_support"), "urdf",
+                 "kr16_2.xacro"]
+            ),
+        ]
+    )
+
+    robot_description = {"robot_description": robot_description_content}
+
+    joint_state_publisher_node = Node(
+        package="joint_state_publisher_gui",
+        executable="joint_state_publisher_gui",
+    )
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("kuka_kr16_support"), "config/rviz",
+         "view_robot_kr16_2.rviz"]
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file]
+    )
+
+    nodes = [
+        joint_state_publisher_node,
+        robot_state_publisher_node,
+        rviz_node
+    ]
+
+    return LaunchDescription(nodes)
diff --git a/kuka_kr16_support/package.xml b/kuka_kr16_support/package.xml
index 7cae6bd27..fcf9a147c 100644
--- a/kuka_kr16_support/package.xml
+++ b/kuka_kr16_support/package.xml
@@ -1,5 +1,4 @@
 <?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>kuka_kr16_support</name>
   <version>0.1.0</version>
@@ -40,14 +39,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <test_depend>roslaunch</test_depend>
-
-  <exec_depend>industrial_robot_client</exec_depend>
-  <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>kuka_resources</exec_depend>
-  <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>rviz</exec_depend>
-  <exec_depend>xacro</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/kuka_kr16_support/urdf/kr16_2.xacro b/kuka_kr16_support/urdf/kr16_2.xacro
index 8a7d6f66a..893a73044 100644
--- a/kuka_kr16_support/urdf/kr16_2.xacro
+++ b/kuka_kr16_support/urdf/kr16_2.xacro
@@ -1,13 +1,6 @@
 <?xml version="1.0"?>
 <!--Generates a urdf from the macro in kr16_2_macro.xacro -->
 <robot name="kuka_kr16_2" xmlns:xacro="http://wiki.ros.org/xacro">
-  <xacro:arg name="prefix" default="" />
-  <xacro:arg name="use_mock_hardware" default="false" />
-
   <xacro:include filename="$(find kuka_kr16_support)/urdf/kr16_2_macro.xacro" />
-  <xacro:include filename="$(find kuka_resources)/urdf/common_macro.ros2_control.xacro"/>
-
-  <xacro:kuka_common_ros2_control_macro name="kr16_2_mock" prefix="$(arg prefix)" use_mock_hardware="$(arg use_mock_hardware)"/>
-
   <xacro:kuka_kr16_2 prefix=""/>
 </robot>
diff --git a/kuka_kr6_support/CMakeLists.txt b/kuka_kr6_support/CMakeLists.txt
index 8fdd61091..8e412f826 100644
--- a/kuka_kr6_support/CMakeLists.txt
+++ b/kuka_kr6_support/CMakeLists.txt
@@ -8,10 +8,9 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 
-
 install(
   DIRECTORY config launch meshes urdf
   DESTINATION share/${PROJECT_NAME}
-  )
+)
 
-  ament_package()
+ament_package()
diff --git a/kuka_kr6_support/config/rviz/view_robot.rviz b/kuka_kr6_support/config/rviz/view_robot.rviz
new file mode 100644
index 000000000..6a83d7c07
--- /dev/null
+++ b/kuka_kr6_support/config/rviz/view_robot.rviz
@@ -0,0 +1,244 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 749
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base:
+          Value: true
+        base_link:
+          Value: true
+        link_1:
+          Value: true
+        link_2:
+          Value: true
+        link_3:
+          Value: true
+        link_4:
+          Value: true
+        link_5:
+          Value: true
+        link_6:
+          Value: true
+        tool0:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        base_link:
+          base:
+            {}
+          link_1:
+            link_2:
+              link_3:
+                link_4:
+                  link_5:
+                    link_6:
+                      tool0:
+                        {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tool0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 3.1647839546203613
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.33039820194244385
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 5.143582820892334
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1040
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000376fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000376000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000376fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000376000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077c0000003efc0100000002fb0000000800540069006d006501000000000000077c0000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000050b0000037600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1916
+  X: 0
+  Y: 18
diff --git a/kuka_kr6_support/launch/view_kr6.launch.py b/kuka_kr6_support/launch/view_kr6.launch.py
new file mode 100644
index 000000000..ec685cf84
--- /dev/null
+++ b/kuka_kr6_support/launch/view_kr6.launch.py
@@ -0,0 +1,71 @@
+# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt)
+#
+# 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 RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("kuka_kr6_support"), "urdf",
+                 "kr6r900sixx.xacro"]
+            ),
+        ]
+    )
+
+    robot_description = {"robot_description": robot_description_content}
+
+    joint_state_publisher_node = Node(
+        package="joint_state_publisher_gui",
+        executable="joint_state_publisher_gui",
+    )
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("kuka_kr6_support"), "config/rviz",
+         "view_robot.rviz"]
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file]
+    )
+
+    nodes = [
+        joint_state_publisher_node,
+        robot_state_publisher_node,
+        rviz_node
+    ]
+
+    return LaunchDescription(nodes)
diff --git a/kuka_kr6_support/package.xml b/kuka_kr6_support/package.xml
index 81baead4a..1d8b1b0c4 100644
--- a/kuka_kr6_support/package.xml
+++ b/kuka_kr6_support/package.xml
@@ -48,8 +48,6 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <test_depend>roslaunch</test_depend>
-
   <exec_depend>industrial_robot_client</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>kuka_kr10_support</exec_depend>
diff --git a/kuka_resources/CMakeLists.txt b/kuka_resources/CMakeLists.txt
index 25a9644d4..ecaaa9604 100644
--- a/kuka_resources/CMakeLists.txt
+++ b/kuka_resources/CMakeLists.txt
@@ -12,6 +12,6 @@ find_package(ament_cmake REQUIRED)
 install(
   DIRECTORY urdf config launch
   DESTINATION share/${PROJECT_NAME}
-  )
+)
 
-  ament_package()
+ament_package()
diff --git a/kuka_resources/package.xml b/kuka_resources/package.xml
index cade58a7a..83d9db37a 100644
--- a/kuka_resources/package.xml
+++ b/kuka_resources/package.xml
@@ -22,8 +22,6 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <test_depend>roslaunch</test_depend>
-
   <exec_depend>industrial_robot_client</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
diff --git a/kuka_rsi_hw_interface/package.xml b/kuka_rsi_hw_interface/package.xml
index 1e6547c56..9b77ba7d0 100644
--- a/kuka_rsi_hw_interface/package.xml
+++ b/kuka_rsi_hw_interface/package.xml
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>kuka_rsi_hw_interface</name>
   <version>0.1.0</version>
   <description>A ROS-Control hardware interface for use with KUKA RSI</description>
@@ -12,14 +12,15 @@
   <url type="bugtracker">https://github.com/ros-industrial/kuka_experimental/issues</url>
   <url type="repository">https://github.com/ros-industrial/kuka_experimental</url>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <build_depend>cmake_modules</build_depend>
-
-  <depend>controller_manager</depend>
-  <depend>hardware_interface</depend>
-  <depend>joint_limits_interface</depend>
-  <depend>realtime_tools</depend>
-  <depend>roscpp</depend>
+  <!--<depend>controller_manager</depend>-->
+  <!--<depend>hardware_interface</depend>-->
+  <!--<depend>joint_limits_interface</depend>-->
+  <!--<depend>realtime_tools</depend>-->
+  <depend>rclcpp</depend>
   <depend>std_msgs</depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>
diff --git a/kuka_rsi_simulator/kuka_rsi_simulator/__init__.py b/kuka_rsi_simulator/kuka_rsi_simulator/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/kuka_rsi_simulator/kuka_rsi_simulator/kuka_rsi_simulator.py b/kuka_rsi_simulator/kuka_rsi_simulator/kuka_rsi_simulator.py
new file mode 100755
index 000000000..c7f3b5704
--- /dev/null
+++ b/kuka_rsi_simulator/kuka_rsi_simulator/kuka_rsi_simulator.py
@@ -0,0 +1,110 @@
+#!/usr/bin/env python3
+
+import sys
+import socket
+import numpy as np
+import time
+import xml.etree.ElementTree as ET
+
+import errno
+# import rospy
+# from std_msgs.msg import String
+
+def create_rsi_xml_rob(act_joint_pos, setpoint_joint_pos, timeout_count, ipoc):
+    q = act_joint_pos
+    qd = setpoint_joint_pos
+    root = ET.Element('Rob', {'TYPE':'KUKA'})
+    ET.SubElement(root, 'RIst', {'X':'0.0', 'Y':'0.0', 'Z':'0.0',
+                                 'A':'0.0', 'B':'0.0', 'C':'0.0'})
+    ET.SubElement(root, 'RSol', {'X':'0.0', 'Y':'0.0', 'Z':'0.0',
+                                 'A':'0.0', 'B':'0.0', 'C':'0.0'})
+    ET.SubElement(root, 'AIPos', {'A1':str(q[0]), 'A2':str(q[1]), 'A3':str(q[2]),
+                                  'A4':str(q[3]), 'A5':str(q[4]), 'A6':str(q[5])})
+    ET.SubElement(root, 'ASPos', {'A1':str(qd[0]), 'A2':str(qd[1]), 'A3':str(qd[2]),
+                                  'A4':str(qd[3]), 'A5':str(qd[4]), 'A6':str(qd[5])})
+    ET.SubElement(root, 'Delay', {'D':str(timeout_count)})
+    ET.SubElement(root, 'IPOC').text=str(ipoc)
+    return ET.tostring(root)
+
+def parse_rsi_xml_sen(data):
+    root = ET.fromstring(data)
+    AK = root.find('AK').attrib
+    desired_joint_correction = np.array([AK['A1'], AK['A2'], AK['A3'],
+                                         AK['A4'], AK['A5'], AK['A6']]).astype(np.float64)
+    IPOC = root.find('IPOC').text
+    return desired_joint_correction, int(IPOC)
+
+
+# if __name__ == '__main__':
+def main():
+    node_name = 'kuka_rsi_simulation'
+# rsi_act_pub = rospy.Publisher(node_name + '/rsi/state', String, queue_size=1)
+# rsi_cmd_pub = rospy.Publisher(node_name + '/rsi/command', String, queue_size=1)
+
+    cycle_time = 0.004
+    act_joint_pos = np.array([0, -90, 90, 0, 90, 0]).astype(np.float64)
+    cmd_joint_pos = act_joint_pos.copy()
+    des_joint_correction_absolute = np.zeros(6)
+    timeout_count = 0
+    ipoc = 0
+
+    # import argparse
+    # parser = argparse.ArgumentParser(description='KUKA RSI Simulation')
+    # parser.add_argument('--rsi_hw_iface_ip', default="127.0.0.1", help='The ip address of the RSI control interface (default=127.0.0.1)')
+    # parser.add_argument('--rsi_hw_iface_port', default=49152, help='The port of the RSI control interface (default=49152)')
+    # parser.add_argument('--sen', default='ImFree', help='Type attribute in RSI XML doc. E.g. <Sen Type:"ImFree">')
+    # # Only parse known arguments
+    # args, _ = parser.parse_known_args()
+    # host = args.rsi_hw_iface_ip
+    # port = int(args.rsi_hw_iface_port)
+    # sen_type = args.sen
+
+    host = "127.0.0.1"
+    port = 49152
+    sen_type = 'ImFree'
+
+
+    # rospy.init_node(node_name)
+    # rospy.loginfo('{}: Started'.format(node_name))
+
+    try:
+        s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        # rospy.loginfo('{}, Successfully created socket'.format(node_name))
+        print('{}, Successfully created socket'.format(node_name))
+        s.settimeout(1)
+    except socket.error as e:
+        # rospy.logfatal('{}Could not create socket'.format(node_name))
+        print('{}Could not create socket'.format(node_name))
+        sys.exit()
+
+    def shutdown_hook():
+        # rospy.loginfo('{}: Shutting down'.format(node_name))
+        print('{}: Shutting down'.format(node_name))
+        s.close()
+
+    # rospy.on_shutdown(shutdown_hook)
+
+    # while not rospy.is_shutdown():
+    while True:
+        time.sleep(0.001)  # this is a hack, make this a ros2 node
+        try:
+            msg = create_rsi_xml_rob(act_joint_pos, cmd_joint_pos, timeout_count, ipoc)
+            # rsi_act_pub.publish(str(msg))
+            s.sendto(msg, (host, port))
+            recv_msg, addr = s.recvfrom(1024)
+            # rsi_cmd_pub.publish(str(recv_msg))
+            des_joint_correction_absolute, ipoc_recv = parse_rsi_xml_sen(recv_msg)
+            act_joint_pos = cmd_joint_pos + des_joint_correction_absolute
+            ipoc += 1
+            time.sleep(cycle_time / 2)
+        except socket.timeout:
+            # rospy.logwarn('{}: Socket timed out'.format(node_name))
+            print('{}: Socket timed out'.format(node_name))
+            timeout_count += 1
+        except socket.error as e:
+            if e.errno != errno.EINTR:
+                raise
+
+
+if __name__ == '__main__':
+    main()
diff --git a/kuka_rsi_simulator/package.xml b/kuka_rsi_simulator/package.xml
index e32385ba2..de196cd7f 100644
--- a/kuka_rsi_simulator/package.xml
+++ b/kuka_rsi_simulator/package.xml
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>kuka_rsi_simulator</name>
   <version>0.1.0</version>
   <description>Python node that implements a minimal RSI interface simulator.</description>
@@ -12,8 +12,10 @@
   <url type="bugtracker">https://github.com/ros-industrial/kuka_experimental/issues</url>
   <url type="repository">https://github.com/ros-industrial/kuka_experimental</url>
 
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <exec_depend>rospy</exec_depend>
+  <exec_depend>rclpy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
 </package>
diff --git a/kuka_rsi_simulator/resource/kuka_rsi_simulator b/kuka_rsi_simulator/resource/kuka_rsi_simulator
new file mode 100644
index 000000000..e69de29bb
diff --git a/kuka_rsi_simulator/scripts/kuka_rsi_simulator b/kuka_rsi_simulator/scripts/kuka_rsi_simulator
index 15131e167..db1b47f34 100755
--- a/kuka_rsi_simulator/scripts/kuka_rsi_simulator
+++ b/kuka_rsi_simulator/scripts/kuka_rsi_simulator
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 import sys
 import socket
@@ -7,7 +7,7 @@ import time
 import xml.etree.ElementTree as ET
 
 import errno
-import rospy
+import rclpy
 from std_msgs.msg import String
 
 def create_rsi_xml_rob(act_joint_pos, setpoint_joint_pos, timeout_count, ipoc):
@@ -34,9 +34,8 @@ def parse_rsi_xml_sen(data):
     IPOC = root.find('IPOC').text
     return desired_joint_correction, int(IPOC)
 
+
 node_name = 'kuka_rsi_simulation'
-rsi_act_pub = rospy.Publisher(node_name + '/rsi/state', String, queue_size=1)
-rsi_cmd_pub = rospy.Publisher(node_name + '/rsi/command', String, queue_size=1)
 
 cycle_time = 0.004
 act_joint_pos = np.array([0, -90, 90, 0, 90, 0]).astype(np.float64)
@@ -48,8 +47,8 @@ ipoc = 0
 if __name__ == '__main__':
     import argparse
     parser = argparse.ArgumentParser(description='KUKA RSI Simulation')
-    parser.add_argument('rsi_hw_iface_ip', help='The ip address of the RSI control interface')
-    parser.add_argument('rsi_hw_iface_port', help='The port of the RSI control interface')
+    parser.add_argument('--rsi_hw_iface_ip', default="127.0.0.1", help='The ip address of the RSI control interface (default=127.0.0.1)')
+    parser.add_argument('--rsi_hw_iface_port', default=49152, help='The port of the RSI control interface (default=49152)')
     parser.add_argument('--sen', default='ImFree', help='Type attribute in RSI XML doc. E.g. <Sen Type:"ImFree">')
     # Only parse known arguments
     args, _ = parser.parse_known_args()
@@ -57,38 +56,43 @@ if __name__ == '__main__':
     port = int(args.rsi_hw_iface_port)
     sen_type = args.sen
 
-    rospy.init_node(node_name)
-    rospy.loginfo('{}: Started'.format(node_name))
+    rclpy.init(args=args)
+    node = rclpy.create_node(node_name)
+
+    node.get_logger().info(f"Started '{node_name}'")
+
+    rsi_act_pub = node.create_publisher(String, '~/rsi/state')
+    rsi_cmd_pub = node.create_publisher(String, '~/rsi/command')
 
     try:
         s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-        rospy.loginfo('{}, Successfully created socket'.format(node_name))
+        node.get_logger().info(f"[{node_name}] Successfully created socket.")
         s.settimeout(1)
     except socket.error as e:
-        rospy.logfatal('{}Could not create socket'.format(node_name))
+        node.get_logger().fatal(f"[{node_name}] Could not create socket.")
         sys.exit()
 
-    def shutdown_hook():
-        rospy.loginfo('{}: Shutting down'.format(node_name))
-        s.close()
-
-    rospy.on_shutdown(shutdown_hook)
-
-    while not rospy.is_shutdown():
+    while rclpy.ok():
+        time.sleep(0.001)  # this is a hack, make this a ros2 node
         try:
             msg = create_rsi_xml_rob(act_joint_pos, cmd_joint_pos, timeout_count, ipoc)
-            rsi_act_pub.publish(msg)
+            rsi_act_pub.publish(str(msg))
             s.sendto(msg, (host, port))
             recv_msg, addr = s.recvfrom(1024)
-            rsi_cmd_pub.publish(recv_msg)
+            rsi_cmd_pub.publish(str(recv_msg))
             des_joint_correction_absolute, ipoc_recv = parse_rsi_xml_sen(recv_msg)
             act_joint_pos = cmd_joint_pos + des_joint_correction_absolute
             ipoc += 1
+            rclpy.spin_once(node)
             time.sleep(cycle_time / 2)
-        except socket.timeout, msg:
-            rospy.logwarn('{}: Socket timed out'.format(node_name))
+        except socket.timeout:
+            node.get_logger().warn(f"[{node_name}] Socket timed out.")
             timeout_count += 1
-        except socket.error, e:
+        except socket.error as e:
             if e.errno != errno.EINTR:
                 raise
 
+    node.get_logger().info(f"Shutting down '{node_name}'")
+    node.destroy_node()
+    rclpy.shutdown()
+    s.close()
diff --git a/kuka_rsi_simulator/setup.cfg b/kuka_rsi_simulator/setup.cfg
new file mode 100644
index 000000000..5cb6c94a3
--- /dev/null
+++ b/kuka_rsi_simulator/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/kuka_rsi_simulator
+[install]
+install-scripts=$base/lib/kuka_rsi_simulator
diff --git a/kuka_rsi_simulator/setup.py b/kuka_rsi_simulator/setup.py
new file mode 100644
index 000000000..cf4d2de54
--- /dev/null
+++ b/kuka_rsi_simulator/setup.py
@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'kuka_rsi_simulator'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Denis Štogl',
+    maintainer_email='denis@stoglrobotics.de',
+    description='Python node that implements a minimal RSI interface simulator.',
+    license='BSD',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+	          'kuka_rsi_simulator = kuka_rsi_simulator.kuka_rsi_simulator:main',
+        ],
+    },
+)