diff --git a/scripts/setup-robot-bringup.bash b/scripts/setup-robot-bringup.bash
index 91b70432..1b4c464a 100755
--- a/scripts/setup-robot-bringup.bash
+++ b/scripts/setup-robot-bringup.bash
@@ -88,16 +88,20 @@ cp -n $ROS2_CONTROL_TEMPLATES/test_goal_publishers_config.yaml $ROBOT_FPC_PUB_YA
 for file_type in "${LAUNCH_FILE_TYPES[@]}"; do
   # Construct the file paths
   ROBOT_LAUNCH="launch/${ROBOT_NAME}.launch${file_type}"
+  ROBOT_CONTROL_LAUNCH="launch/${ROBOT_NAME}_control.launch${file_type}"
   TEST_FWD_POS_CTRL_LAUNCH="launch/test_forward_position_controller.launch${file_type}"
   TEST_JTC_LAUNCH="launch/test_joint_trajectory_controller.launch${file_type}"
 
   # Copy the templates to the destination with the specified file type
-  cp -n "$ROS2_CONTROL_TEMPLATES/robot_ros2_control.launch${file_type}" "${ROBOT_LAUNCH}"
+  if [ -f "$ROS2_CONTROL_TEMPLATES/robot.launch${file_type}" ]; then  # only .xml for now
+    cp -n "$ROS2_CONTROL_TEMPLATES/robot.launch${file_type}" "${ROBOT_LAUNCH}"
+  fi
+  cp -n "$ROS2_CONTROL_TEMPLATES/robot_ros2_control.launch${file_type}" "${ROBOT_CONTROL_LAUNCH}"
   cp -n "$ROS2_CONTROL_TEMPLATES/test_forward_position_controller.launch${file_type}" "${TEST_FWD_POS_CTRL_LAUNCH}"
   cp -n "$ROS2_CONTROL_TEMPLATES/test_joint_trajectory_controller.launch${file_type}" "${TEST_JTC_LAUNCH}"
 
   # sed all needed files
-  FILES_TO_SED=($ROBOT_LAUNCH $TEST_FWD_POS_CTRL_LAUNCH $TEST_JTC_LAUNCH)
+  FILES_TO_SED=($ROBOT_LAUNCH $ROBOT_CONTROL_LAUNCH $TEST_FWD_POS_CTRL_LAUNCH $TEST_JTC_LAUNCH)
 
   for SED_FILE in "${FILES_TO_SED[@]}"; do
     sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" $SED_FILE
@@ -123,14 +127,26 @@ done
 prepend_to_string="if(BUILD_TESTING)"
 sed -i "s/$prepend_to_string/install\(\\n  DIRECTORY config launch\\n  DESTINATION share\/\$\{PROJECT_NAME\}\\n\)\\n\\n$prepend_to_string/g" CMakeLists.txt
 
-# extend README with general instructions
+# extend main README with general instructions
+if [ -f ../README.md ]; then
+  cat $ROS2_CONTROL_TEMPLATES/append_to_MAIN_README.md >> ../README.md
+  sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" ../README.md
+  sed -i "s/\\\$ROBOT_NAME\\\$/${ROBOT_NAME}/g" ../README.md
+  sed -i "s/\\\$DESCR_PKG_NAME\\\$/${DESCR_PKG_NAME}/g" ../README.md
+else
+  echo "No README.md in parent directory found, skipping adding short bringup info."
+fi
+
+# extend package README with general instructions
 if [ -f README.md ]; then
-  cat $ROS2_CONTROL_TEMPLATES/append_to_README.md >>README.md
-  sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" README.md
-  sed -i "s/\\\$ROBOT_NAME\\\$/${ROBOT_NAME}/g" README.md
-  sed -i "s/\\\$DESCR_PKG_NAME\\\$/${DESCR_PKG_NAME}/g" $SED_FILE
+  echo "#${PKG_NAME}\n\n" > README.md
 fi
 
+cat $ROS2_CONTROL_TEMPLATES/append_to_README.md >>README.md
+sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" README.md
+sed -i "s/\\\$ROBOT_NAME\\\$/${ROBOT_NAME}/g" README.md
+sed -i "s/\\\$DESCR_PKG_NAME\\\$/${DESCR_PKG_NAME}/g" README.md
+
 # TODO: Add license checks
 
 # Compile and add new package the to the path
diff --git a/scripts/setup-robot-description.bash b/scripts/setup-robot-description.bash
index 9ac2530f..40d574f9 100755
--- a/scripts/setup-robot-description.bash
+++ b/scripts/setup-robot-description.bash
@@ -90,11 +90,15 @@ cp -n "$ROBOT_DESCRIPTION_TEMPLATES/materials.xacro" urdf/common/materials.xacro
 # Copy launch files for testing the description
 for file_type in "${LAUNCH_FILE_TYPES[@]}"; do
   mkdir -p launch
+
+  ROBOT_DESCRIPTION_LAUNCH="launch/${ROBOT_NAME}_description.launch${file_type}"
+  cp -n "$ROBOT_DESCRIPTION_TEMPLATES/robot_description.launch${file_type}" $ROBOT_DESCRIPTION_LAUNCH
+
   VIEW_ROBOT_LAUNCH="launch/view_${ROBOT_NAME}.launch${file_type}"
   cp -n "$ROBOT_DESCRIPTION_TEMPLATES/view_robot.launch${file_type}" $VIEW_ROBOT_LAUNCH
 
   # sed all needed files
-  FILES_TO_SED=($ROBOT_URDF_XACRO $ROBOT_MACRO $ROBOT_MACRO_ROS2_CONTROL $VIEW_ROBOT_LAUNCH)
+  FILES_TO_SED=($ROBOT_URDF_XACRO $ROBOT_MACRO $ROBOT_MACRO_ROS2_CONTROL $ROBOT_DESCRIPTION_LAUNCH $VIEW_ROBOT_LAUNCH)
 
   for SED_FILE in "${FILES_TO_SED[@]}"; do
     sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" $SED_FILE
@@ -188,7 +192,7 @@ sed -i "/$pattern/a$lines_to_append" CMakeLists.txt
 
 # extend README with general instructions
 if [ ! -f README.md ]; then
-  echo "${PKG_NAME}\n\n" > README.md
+  echo "#${PKG_NAME}\n\n" > README.md
 fi
 
 cat $ROBOT_DESCRIPTION_TEMPLATES/append_to_README.md >> README.md
diff --git a/templates/robot_description/robot_description.launch.py b/templates/robot_description/robot_description.launch.py
new file mode 100644
index 00000000..a326a9a4
--- /dev/null
+++ b/templates/robot_description/robot_description.launch.py
@@ -0,0 +1,104 @@
+# Copyright (c) 2024, 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.
+
+#
+# Source of this file is https://github.com/StoglRobotics/ros_team_workspace repository.
+#
+# Author: Dr. Denis
+#
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_package",
+            default_value="$PKG_NAME$",
+            description="Description package of the $ROBOT_NAME$. Usually the argument is not set, \
+        it enables use of a custom description.",
+        )
+    )
+    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(
+            "launch_rviz",
+            default_value="false",
+            description="Launch Rviz?",
+        )
+    )
+
+    # Initialize Arguments
+    description_package = LaunchConfiguration("description_package")
+    prefix = LaunchConfiguration("prefix")
+    launch_rviz = LaunchConfiguration("launch_rviz")
+
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare(description_package), "urdf", "$ROBOT_NAME$.urdf.xacro"]
+            ),
+            " ",
+            "prefix:=",
+            prefix,
+            " ",
+        ]
+    )
+
+    robot_description = {"robot_description": robot_description_content}
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare(description_package), "rviz", "$ROBOT_NAME$.rviz"]
+    )
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        condition=IfCondition(launch_rviz),
+        arguments=["-d", rviz_config_file],
+    )
+
+    return LaunchDescription(
+        declared_arguments
+        + [
+            robot_state_publisher_node,
+            rviz_node,
+        ]
+    )
diff --git a/templates/robot_description/robot_description.launch.xml b/templates/robot_description/robot_description.launch.xml
new file mode 100644
index 00000000..d6f24cd6
--- /dev/null
+++ b/templates/robot_description/robot_description.launch.xml
@@ -0,0 +1,53 @@
+<!--
+Copyright (c) 2024, 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.
+
+
+Source of this file are templates in https://github.com/StoglRobotics/ros_team_workspace repository.
+-->
+
+<launch>
+  <arg name="description_package"
+       default="$PKG_NAME$"
+       description="Description package of the dte_ea_1000. Usually the argument is not set, it enables use of a custom description."/>
+  <arg name="prefix"
+       default=""
+       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."/>
+  <arg name="use_mock_hardware"
+       default="false"
+       description="Start robot with mock hardware mirroring command to its states."/>
+  <arg name="mock_sensor_commands"
+       default="false"
+       description="Enable mock command interfaces for sensors used for simple simulations. Used only if 'use_mock_hardware' parameter is true."/>
+  <arg name="sim_gazebo"
+       default="false"
+       description="Start robot in Gazebo simulation."/>
+  <arg name="simulation_controllers"
+       default=""
+       description="Path to the yaml file with controllers used in the simulation."/>
+  <arg name="launch_rviz"
+       default="false"
+       description="Launch RViz?"/>
+
+  <let name="robot_description_content" value="$(command '$(find-exec xacro) $(find-pkg-share $(var description_package))/urdf/$ROBOT_NAME$.urdf.xacro prefix:=$(var prefix) use_mock_hardware:=$(var use_mock_hardware) mock_sensor_commands:=$(var mock_sensor_commands) sim_gazebo:=$(var sim_gazebo) simulation_controllers:=$(var simulation_controllers)')" />
+
+  <node pkg="robot_state_publisher" exec="robot_state_publisher" output="both">
+    <param name="robot_description" value="$(var robot_description_content)" />
+    <param name="use_sim_time" value="$(var sim_gazebo)" />
+  </node>
+
+  <node if="$(var launch_rviz)" pkg="rviz2" exec="rviz2" output="log" args="-d $(find-pkg-share $(var description_package))/rviz/$ROBOT_NAME$.rviz">
+    <param name="use_sim_time" value="$(var sim_gazebo)" />
+  </node>
+</launch>
diff --git a/templates/robot_description/robot_macro.ros2_control.xacro b/templates/robot_description/robot_macro.ros2_control.xacro
index e7c7d075..a6cfbe7b 100644
--- a/templates/robot_description/robot_macro.ros2_control.xacro
+++ b/templates/robot_description/robot_macro.ros2_control.xacro
@@ -16,6 +16,7 @@
         <xacro:if value="${use_mock_hardware}">
           <plugin>mock_components/GenericSystem</plugin>
           <param name="mock_sensor_commands">${mock_sensor_commands}</param>
+          <param name="calculate_dynamics">true</param>
         </xacro:if>
         <xacro:if value="${sim_gazebo_classic}">
           <plugin>gazebo_ros2_control/GazeboSystem</plugin>
@@ -35,8 +36,12 @@
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="acceleration"/>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="acceleration">
+          <param name="initial_value">0.0</param>
+        </state_interface>
       </joint>
       <joint name="${prefix}joint2">
         <command_interface name="position">     <!-- Add/Remove interfaces as needed -->
@@ -46,8 +51,12 @@
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="acceleration"/>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="acceleration">
+          <param name="initial_value">0.0</param>
+        </state_interface>
       </joint>
       <joint name="${prefix}joint3">
         <command_interface name="position">     <!-- Add/Remove interfaces as needed -->
@@ -57,8 +66,12 @@
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="acceleration"/>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="acceleration">
+          <param name="initial_value">0.0</param>
+        </state_interface>
       </joint>
       <joint name="${prefix}joint4">
         <command_interface name="position">     <!-- Add/Remove interfaces as needed -->
@@ -68,8 +81,12 @@
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="acceleration"/>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="acceleration">
+          <param name="initial_value">0.0</param>
+        </state_interface>
       </joint>
       <joint name="${prefix}joint5">
         <command_interface name="position">     <!-- Add/Remove interfaces as needed -->
@@ -79,8 +96,12 @@
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="acceleration"/>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="acceleration">
+          <param name="initial_value">0.0</param>
+        </state_interface>
       </joint>
       <joint name="${prefix}joint6">
         <command_interface name="position">     <!-- Add/Remove interfaces as needed -->
@@ -90,16 +111,32 @@
         <state_interface name="position">
           <param name="initial_value">0.0</param>
         </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="acceleration"/>
+        <state_interface name="velocity">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="acceleration">
+          <param name="initial_value">0.0</param>
+        </state_interface>
       </joint>
       <sensor name="tcp_fts_sensor">            <!-- Remove/change sensors as needed -->
-        <state_interface name="fx"/>
-        <state_interface name="fy"/>
-        <state_interface name="fz"/>
-        <state_interface name="tx"/>
-        <state_interface name="ty"/>
-        <state_interface name="tz"/>
+        <state_interface name="fx">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="fy">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="fz">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="tx">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="ty">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="tz">
+          <param name="initial_value">0.0</param>
+        </state_interface>
         <param name="frame_id">tool0</param>    <!-- Change sensors as needed -->
         <param name="min_fx">-100</param>
         <param name="max_fx">100</param>
diff --git a/templates/robot_description/robot_macro.xacro b/templates/robot_description/robot_macro.xacro
index a99729eb..afd3ecd6 100644
--- a/templates/robot_description/robot_macro.xacro
+++ b/templates/robot_description/robot_macro.xacro
@@ -4,6 +4,10 @@
   <xacro:include filename="$(find $PKG_NAME$)/urdf/common/inertials.xacro" />
   <xacro:include filename="$(find $PKG_NAME$)/urdf/common/materials.xacro" />
 
+
+  <!-- ATTTTTTEEEEENNNNNNNTTTTTIIIIIIOOOOOOONNNNNN -->
+  <!-- If you are dealing with a mobile robot, remove fixing it to the world!! -->
+
   <xacro:macro name="$ROBOT_NAME$" params="prefix parent *origin">
     <!-- LINKS -->
     <!-- base link -->
@@ -15,7 +19,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.3" radius="0.3" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/visual/base_link.dae" /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/visual/base_link.dae" /> -->
         </geometry>
         <material name="black"/>
       </visual>
@@ -23,7 +27,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.3" radius="0.3" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/collision/base_link.stl" /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/collision/base_link.stl" /> -->
         </geometry>
       </collision>
     </link>
@@ -36,7 +40,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.3" radius="0.15" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/visual/link1.dae" /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/visual/link1.dae" /> -->
         </geometry>
         <material name="blue"/>
       </visual>
@@ -44,7 +48,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.3" radius="0.15" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/collision/link1.stl" /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/collision/link1.stl" /> -->
         </geometry>
         <material name="green"/>
       </collision>
@@ -58,7 +62,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.8" radius="0.15" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/visual/link2.dae" /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/visual/link2.dae" /> -->
         </geometry>
         <material name="grey"/>
       </visual>
@@ -66,7 +70,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.8" radius="0.15" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/collision/link2.stl"  /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/collision/link2.stl"  /> -->
         </geometry>
       </collision>
     </link>
@@ -79,7 +83,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.5" radius="0.10" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/visual/link3.dae" /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/visual/link3.dae" /> -->
         </geometry>
         <material name="orange"/>
       </visual>
@@ -87,7 +91,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.5" radius="0.10" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/collision/link3.stl"  /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/collision/link3.stl"  /> -->
         </geometry>
       </collision>
     </link>
@@ -100,7 +104,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.1" radius="0.2" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/visual/link4.dae" /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/visual/link4.dae" /> -->
         </geometry>
         <material name="brown"/>
       </visual>
@@ -108,7 +112,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.1" radius="0.2" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/collision/link4.stl"  /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/collision/link4.stl"  /> -->
         </geometry>
       </collision>
     </link>
@@ -121,7 +125,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.2" radius="0.1" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/visual/link5.dae" /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/visual/link5.dae" /> -->
         </geometry>
         <material name="red"/>
       </visual>
@@ -129,7 +133,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.2" radius="0.1" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/collision/link5.stl" /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/collision/link5.stl" /> -->
         </geometry>
       </collision>
     </link>
@@ -142,7 +146,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.05" radius="0.1" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/visual/link6.dae" /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/visual/link6.dae" /> -->
         </geometry>
         <material name="white"/>
       </visual>
@@ -150,7 +154,7 @@
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
           <cylinder length="0.05" radius="0.1" />
-<!--           <mesh filename="package://$PKG_NAME$/meshes/$ROBOT_NAME$/collision/link6.stl" /> -->
+<!--           <mesh filename="file://$(find $PKG_NAME$)/meshes/$ROBOT_NAME$/collision/link6.stl" /> -->
         </geometry>
       </collision>
     </link>
diff --git a/templates/robot_description/robot_moveit.launch.py b/templates/robot_description/robot_moveit.launch.py
new file mode 100644
index 00000000..d03bd548
--- /dev/null
+++ b/templates/robot_description/robot_moveit.launch.py
@@ -0,0 +1,188 @@
+# Copyright (c) 2024, 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.
+
+#
+# Source of this file are templates in
+# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
+#
+# Authors: Nikola Banović, Dr. Denis
+#
+import os
+import yaml
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import OpaqueFunction, DeclareLaunchArgument
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path) as file:
+            return yaml.safe_load(file)
+    except OSError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def launch_setup(context, *args, **kwargs):
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    launch_rviz = LaunchConfiguration("launch_rviz")
+    only_rviz = LaunchConfiguration("only_rviz")
+
+    # Get SRDF via xacro
+    robot_srdf_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("$PKG_NAME$"), "srdf", "$ROBOT_NAME$.srdf.xacro"]
+            ),
+        ]
+    )
+    robot_description_semantic = {
+        "robot_description_semantic": robot_srdf_content.perform(context)
+    }
+
+    moveit_config_package_name = "$ROBOT_NAME$_moveit_config"
+
+    controllers_yaml = load_yaml(
+        moveit_config_package_name, "config/$ROBOT_NAME$/controllers.yaml"
+    )
+    ompl_planning_yaml = load_yaml(
+        moveit_config_package_name, "config/$ROBOT_NAME$/ompl_planning.yaml"
+    )
+    kinematics_yaml = load_yaml(moveit_config_package_name, "config/$ROBOT_NAME$/kinematics.yaml")
+    robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
+    limits_yaml = load_yaml(moveit_config_package_name, "config/$ROBOT_NAME$/joint_limits.yaml")
+    robot_description_planning = {"robot_description_planning": limits_yaml}
+
+    # Planning Configuration
+    ompl_planning_pipeline_config = {
+        "default_planning_pipeline": "ompl",
+        "planning_pipelines": ["ompl"],
+        "ompl": {
+            "planning_plugin": "ompl_interface/OMPLPlanner",
+            "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
+            "start_state_max_bounds_error": 0.1,
+        },
+    }
+    ompl_planning_pipeline_config["ompl"].update(ompl_planning_yaml)
+
+    # Moveit controllers Configuration
+    moveit_controllers = {
+        "moveit_simple_controller_manager": controllers_yaml,
+        "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
+    }
+
+    # Trajectory Execution Configuration
+    trajectory_execution = {
+        "moveit_manage_controllers": True,
+        "trajectory_execution.allowed_execution_duration_scaling": 1.2,
+        "trajectory_execution.allowed_goal_duration_margin": 0.5,
+        "trajectory_execution.allowed_start_tolerance": 0.01,
+        "trajectory_execution.execution_duration_monitoring": False,
+    }
+
+    plan_execution = {
+        "plan_execution.record_trajectory_state_frequency": 10.0,
+    }
+
+    planning_scene_monitor_parameters = {
+        "publish_planning_scene": True,
+        "publish_geometry_updates": True,
+        "publish_state_updates": True,
+        "publish_transforms_updates": True,
+    }
+
+    move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[
+            robot_description_semantic,
+            robot_description_kinematics,
+            robot_description_planning,
+            ompl_planning_pipeline_config,
+            trajectory_execution,
+            plan_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+            # PathJoinSubstitution(
+            #     [FindPackageShare("$PKG_NAME$"), "$ROBOT_NAME$_moveit", "moveit_execution_and_control.yaml"]
+            # ),
+            # PathJoinSubstitution(
+            #     [FindPackageShare("$PKG_NAME$"), "$ROBOT_NAME$_moveit", "planning_pipelines.yaml"]
+            # ),
+            {"use_sim_time": use_sim_time},
+        ],
+        condition=UnlessCondition(only_rviz),
+    )
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("$PKG_NAME$"), "rviz", "$ROBOT_NAME$_moveit.rviz"]
+    )
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        parameters=[
+            robot_description_semantic,
+            robot_description_kinematics,
+            # PathJoinSubstitution(
+            #     [FindPackageShare("$PKG_NAME$"), "$ROBOT_NAME$_moveit", "moveit_execution_and_control.yaml"]
+            # ),
+            # PathJoinSubstitution(
+            #     [FindPackageShare("$PKG_NAME$"), "$ROBOT_NAME$_moveit", "planning_pipelines.yaml"]
+            # ),
+            {"use_sim_time": use_sim_time},
+        ],
+        condition=IfCondition(launch_rviz or only_rviz),
+    )
+
+    return [move_group_node, rviz_node]
+
+
+def generate_launch_description():
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="false",
+            description="Use simulation clock instead of world clock",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "launch_rviz",
+            default_value="false",
+            description="Launch Rviz?",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "only_rviz",
+            default_value="false",
+            description="Start only rviz with MoveIt setup. This is useful when MoveIt is running on another machine, e.g., on the robot.",
+        )
+    )
+
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])
diff --git a/templates/robot_description/view_robot.launch.py b/templates/robot_description/view_robot.launch.py
index d8274a87..55efbec9 100755
--- a/templates/robot_description/view_robot.launch.py
+++ b/templates/robot_description/view_robot.launch.py
@@ -19,81 +19,31 @@
 #
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import IncludeLaunchDescription
+from launch.substitutions import PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
 
 def generate_launch_description():
-    # Declare arguments
-    declared_arguments = []
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "description_package",
-            default_value="$PKG_NAME$",
-            description="Description package of the $ROBOT_NAME$. Usually the argument is not set, \
-        it enables use of a custom description.",
-        )
-    )
-    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.",
-        )
-    )
-
-    # Initialize Arguments
-    description_package = LaunchConfiguration("description_package")
-    prefix = LaunchConfiguration("prefix")
-
-    # Get URDF via xacro
-    robot_description_content = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
+    robot_description_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare(description_package), "urdf", "$ROBOT_NAME$.urdf.xacro"]
-            ),
-            " ",
-            "prefix:=",
-            prefix,
-            " ",
-        ]
-    )
-
-    robot_description = {"robot_description": robot_description_content}
-
-    rviz_config_file = PathJoinSubstitution(
-        [FindPackageShare(description_package), "rviz", "$ROBOT_NAME$.rviz"]
+                [FindPackageShare("$PKG_NAME$"), "launch", "$ROBOT_NAME$_description.launch.py"]
+            )
+        ),
+        launch_arguments={"launch_rviz": "true"}.items(),
     )
 
     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_node = Node(
-        package="rviz2",
-        executable="rviz2",
-        name="rviz2",
-        output="log",
-        arguments=["-d", rviz_config_file],
-    )
 
     return LaunchDescription(
-        declared_arguments
-        + [
+        [
+            robot_description_launch,
             joint_state_publisher_node,
-            robot_state_publisher_node,
-            rviz_node,
         ]
     )
diff --git a/templates/robot_description/view_robot.launch.xml b/templates/robot_description/view_robot.launch.xml
index d7076547..ba7a6162 100644
--- a/templates/robot_description/view_robot.launch.xml
+++ b/templates/robot_description/view_robot.launch.xml
@@ -18,20 +18,11 @@ Source of this file are templates in https://github.com/StoglRobotics/ros_team_w
 -->
 
 <launch>
-  <arg name="description_package"
-       default="$PKG_NAME$"
-       description="Description package of the dte_ea_1000. Usually the argument is not set, it enables use of a custom description."/>
-  <arg name="prefix"
-       default=""
-       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."/>
 
-  <let name="robot_description_content" value="$(command '$(find-exec xacro) $(find-pkg-share $(var description_package))/urdf/$ROBOT_NAME$.urdf.xacro prefix:=$(var prefix)')" />
+  <include file="$(find-pkg-share $PKG_NAME$)/launch/$ROBOT_NAME$_description.launch.xml" >
+    <arg name="launch_rviz" value="true" />
+  </include>
 
   <node pkg="joint_state_publisher_gui" exec="joint_state_publisher_gui"/>
 
-  <node pkg="robot_state_publisher" exec="robot_state_publisher" output="both">
-    <param name="robot_description" value="$(var robot_description_content)" />
-  </node>
-
-  <node pkg="rviz2" exec="rviz2" output="log" args="-d $(find-pkg-share $(var description_package))/rviz/$ROBOT_NAME$.rviz"/>
 </launch>
diff --git a/templates/ros2_control/append_to_MAIN_README.md b/templates/ros2_control/append_to_MAIN_README.md
index d459ea87..4b75a312 100644
--- a/templates/ros2_control/append_to_MAIN_README.md
+++ b/templates/ros2_control/append_to_MAIN_README.md
@@ -5,11 +5,11 @@
 
 To test the mock hardware with standard controllers we first start the bringup with the mock hardware enabled:
 ```
-ros2 launch dte_ea_bringup dte_ea_1000.launch.xml use_mock_hardware:=true
+ros2 launch $PKG_NAME$ $ROBOT_NAME$_control.launch.xml use_mock_hardware:=true
 ```
 In a second terminal we then publish goals for the jtc:
 ```
-ros2 launch dte_ea_bringup test_joint_trajectory_controller.launch.xml
+ros2 launch $PKG_NAME$ test_joint_trajectory_controller.launch.xml
 ```
 
 After stopping the goal publisher, you can deactivate `JointTrajectoryController`, and activate `ForwardPositionController`:
@@ -18,5 +18,5 @@ ros2 control switch_controllers --activate forward_position_controller --deactiv
 ```
 After successful controller switch, start goal publisher for another controller:
 ```
-ros2 launch dte_ea_bringup test_forward_position_controller.launch.xml
+ros2 launch $PKG_NAME$ test_forward_position_controller.launch.xml
 ```
diff --git a/templates/ros2_control/append_to_README.md b/templates/ros2_control/append_to_README.md
index e21a1888..28c1fdf7 100644
--- a/templates/ros2_control/append_to_README.md
+++ b/templates/ros2_control/append_to_README.md
@@ -17,9 +17,9 @@ The general package structure is the following:
 │   ├── <robot_name>_forward_position_publisher.yaml  # Setup test publisher for forward position controller
 │   └── <robot_name>_joint_trajectory_publisher.yaml  # Setup test publisher for joint trajectory controller
 └── launch/
-    ├── <robot_name>.launch.py                     # Robot's default launch file
-    ├── test_forward_position_controller.launch.py # Start test publisher for forward position controller
-    └── test_joint_trajectory_controller.launch.py # Start test publisher for joint trajectory controller
+    ├── <robot_name>.launch.xml                      # Robot's default launch file
+    ├── test_forward_position_controller.launch.xml  # Start test publisher for forward position controller
+    └── test_joint_trajectory_controller.launch.xml  # Start test publisher for joint trajectory controller
 
 ```
 
@@ -33,7 +33,7 @@ Consult the repository and [ros2_control documentation](https://ros-controls.git
 
 1. Start robot's hardware and load controllers (default configuration starts mock hardware)
    ```
-   ros2 launch $PKG_NAME$ $ROBOT_NAME$.launch.xml
+   ros2 launch $PKG_NAME$ $ROBOT_NAME$_control.launch.xml
    ```
 
 2. Open another terminal and check if your hardware is loaded properly:
@@ -55,12 +55,37 @@ Consult the repository and [ros2_control documentation](https://ros-controls.git
 ## Loading Controllers
 
 To move the robot you should load and start controllers.
-To get feedback about robot's state `JointStateController` is used.
-to send command to the robot `ForwardCommandController` (direct goals) or `JointTrajectoryController` (interpolates trajectory).
+
+Check below the controller's setup for different components.
+
+### Mobile base
+
+To move the *differential drive* mobile robot [`DiffDriveController`](https://control.ros.org/master/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html) controller is used.
+
+#### Differential Drive Controller
+Diff Drive Controller provides calculation of inverse kinematics for differential drive kinematics, by receiving velocity commands in Cartesian space and sending them to the hardware in joint space.
+In general it provides support for input velocities with (`geometry_msgs/TwistStamped`) and without (`geometry_msgs/Twist`) time stamp.
+
+  **NOTE**: As of today, February 2024, when the controller is used with Nav2, the input without timestamp should be used and you should set `use_stamped_vel` parameter to false. (It is expected that from ROS 2 Jazzy stamped version and new message `VelocityStamped` will be used!).
+
+To test the configuration of the DiffDriveController and movement of the robot use either.
+
+1. `teleop twist keyboard`:
+   ```
+   ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap /cmd_vel:=/diff_drive_controller/cmd_vel_unstamped
+   ```
+
+2. RQT-plugin `robot steering`. When you open it set the publish topic to `/diff_drive_controller/cmd_vel_unstamped`
+
+
+### Arms
+
+To get feedback about robot's state [`JointStateBroadcaster`](https://control.ros.org/master/doc/ros2_controllers/joint_state_broadcaster/doc/userdoc.html) is used.
+To send command to the robot [`ForwardCommandController`](https://control.ros.org/master/doc/ros2_controllers/forward_command_controller/doc/userdoc.html) (direct goals) or [`JointTrajectoryController`](https://control.ros.org/master/doc/ros2_controllers/joint_trajectory_controller/doc/userdoc.html) (interpolates trajectory).
 The sections below describe their usage.
 
-### Joint State Controller
-Joint state Controllers provides output of robot's internal states to `/joint_states` and `/dynamic_joint_states` ROS 2-topics.
+#### Joint State Broadcaster
+Joint State Broadcaster provides output of robot's internal states to `/joint_states` and `/dynamic_joint_states` ROS 2-topics.
 
 In a new terminal with sourced ROS 2 environment load, configure and start `joint_state_broadcaster`:
   ```
@@ -77,7 +102,7 @@ You should get similar response to:
 
 Now you should also see your robot represented correctly in the `rviz2`.
 
-### Using Forward Command Controllers
+#### Using Forward Command Controllers
 
 1. If you want to test hardware with `ForwardCommandController` first load and configure it. Controller types are e.g., "position", "velocity", and depend on configured names in the [`config/$ROBOT_NAME$_controllers.yaml`](config/$ROBOT_NAME$_controllers.yaml):
    ```
@@ -123,7 +148,7 @@ Now you should also see your robot represented correctly in the `rviz2`.
    ```
    b. Or you can start demo node which sends two goals every 5 seconds in a loop (**Only with position controller!**):
    ```
-   ros2 launch $PKG_NAME$ test_forward_position_controller.launch.py
+   ros2 launch $PKG_NAME$ test_forward_position_controller.launch.xml
    ```
 
 ### Using JointTrajectoryController (**Not working yet!**)
@@ -158,7 +183,7 @@ Now you should also see your robot represented correctly in the `rviz2`.
 
 3. Send command to the controller using test node:
    ```
-   ros2 launch ros2_control_demo_robot test_joint_trajectory_controller.launch.py
+   ros2 launch ros2_control_demo_robot test_joint_trajectory_controller.launch.xml
    ```
 
 **NOTE**: You can switch controllers (step 1 and 2) also with one command:
diff --git a/templates/ros2_control/empty.sdf b/templates/ros2_control/empty.sdf
new file mode 100644
index 00000000..5c95c05e
--- /dev/null
+++ b/templates/ros2_control/empty.sdf
@@ -0,0 +1,67 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="empty">
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-contact-system"
+      name="gz::sim::systems::Contact">
+    </plugin>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+  </world>
+</sdf>
diff --git a/templates/ros2_control/robot.launch.xml b/templates/ros2_control/robot.launch.xml
new file mode 100644
index 00000000..ab967cbc
--- /dev/null
+++ b/templates/ros2_control/robot.launch.xml
@@ -0,0 +1,43 @@
+<!--
+Copyright (c) 2024, 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.
+
+
+Source of this file are templates in
+[RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
+
+Authors: Dr. Denis
+-->
+
+<launch>
+  <arg name="use_mock_hardware"
+       default="false"
+       description="Starting MockHW for offline testing instead of the real HW."/>
+
+  <include file="$(find-pkg-share $RUNTIME_CONFIG_PKG_NAME$)/launch/$ROBOT_NAME$_control.launch.xml" >
+     <arg name="use_mock_hardware" value="$(var use_mock_hardware)" />
+     <arg name="mock_sensor_commands" value="false" />
+     <arg name="sim_gazebo" value="false" />
+     <arg name="launch_rviz" value="false" />
+  </include>
+
+  <!-- Activate rviz only if working with MockHW, usually on the development PC -->
+  <include file="$(find-pkg-share $RUNTIME_CONFIG_PKG_NAME$)/launch/$ROBOT_NAME$_moveit.launch.py" >
+     <arg name="sim_time" value="false" />
+     <arg name="launch_rviz" value="$(var use_mock_hardware)" />
+  </include>
+
+  <!-- ADD HERE OTHER COMPONENTS FOR THE SCENARIO -->
+
+</launch>
diff --git a/templates/ros2_control/robot_controllers.yaml b/templates/ros2_control/robot_controllers.yaml
index 3c984fbd..d4b7d279 100644
--- a/templates/ros2_control/robot_controllers.yaml
+++ b/templates/ros2_control/robot_controllers.yaml
@@ -35,6 +35,10 @@ controller_manager:
     joint_trajectory_controller:              # delete entry if controller is not applicable
       type: joint_trajectory_controller/JointTrajectoryController
 
+    diff_drive_controller:                    # delete entry if controller is not applicable
+      type: diff_drive_controller/DiffDriveController
+
+
 forward_position_controller:                  # delete entry if controller is not applicable
   ros__parameters:
     joints:
@@ -46,6 +50,7 @@ forward_position_controller:                  # delete entry if controller is no
       - joint6
     interface_name: position
 
+
 forward_velocity_controller:                  # delete entry if controller is not applicable
   ros__parameters:
     joints:
@@ -57,6 +62,7 @@ forward_velocity_controller:                  # delete entry if controller is no
       - joint6
     interface_name: velocity
 
+
 joint_trajectory_controller:                  # delete entry if controller is not applicable
   ros__parameters:
     joints:
@@ -79,3 +85,52 @@ joint_trajectory_controller:                  # delete entry if controller is no
     constraints:
       stopped_velocity_tolerance: 0.01 # Defaults to 0.01
       goal_time: 0.0 # Defaults to 0.0 (start immediately)
+
+
+diff_drive_controller:
+  ros__parameters:
+    left_wheel_names: ["left_wheel_joint"]
+    right_wheel_names: ["right_wheel_joint"]
+
+    wheel_separation: 0.20
+    #wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
+    wheel_radius: 0.035
+
+    wheel_separation_multiplier: 1.0
+    left_wheel_radius_multiplier: 1.0
+    right_wheel_radius_multiplier: 1.0
+
+    publish_rate: 50.0
+    odom_frame_id: odom
+    base_frame_id: base_link
+    pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+
+    open_loop: false
+    enable_odom_tf: true
+
+    cmd_vel_timeout: 0.5
+    #publish_limited_velocity: true
+    use_stamped_vel: false
+    #velocity_rolling_window_size: 10
+
+    # Velocity and acceleration limits
+    # Whenever a min_* is unspecified, default to -max_*
+    linear.x.has_velocity_limits: true
+    linear.x.has_acceleration_limits: true
+    linear.x.has_jerk_limits: false
+    linear.x.max_velocity: 1.0
+    linear.x.min_velocity: -1.0
+    linear.x.max_acceleration: 1.0
+    linear.x.max_jerk: 0.0
+    linear.x.min_jerk: 0.0
+
+    angular.z.has_velocity_limits: true
+    angular.z.has_acceleration_limits: true
+    angular.z.has_jerk_limits: false
+    angular.z.max_velocity: 1.0
+    angular.z.min_velocity: -1.0
+    angular.z.max_acceleration: 1.0
+    angular.z.min_acceleration: -1.0
+    angular.z.max_jerk: 0.0
+    angular.z.min_jerk: 0.0
diff --git a/templates/ros2_control/robot_ros2_control.launch.py b/templates/ros2_control/robot_ros2_control.launch.py
index b2506ca8..76e6d9a2 100644
--- a/templates/ros2_control/robot_ros2_control.launch.py
+++ b/templates/ros2_control/robot_ros2_control.launch.py
@@ -20,9 +20,15 @@
 #
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction
+from launch.launch_description_sources import AnyLaunchDescriptionSource
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    RegisterEventHandler,
+    TimerAction,
+)
 from launch.event_handlers import OnProcessExit, OnProcessStart
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
@@ -53,13 +59,6 @@ def generate_launch_description():
         is not set, it enables use of a custom description.",
         )
     )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "description_file",
-            default_value="$ROBOT_NAME$.urdf.xacro",
-            description="URDF/XACRO description file with the robot.",
-        )
-    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "prefix",
@@ -97,35 +96,29 @@ def generate_launch_description():
     runtime_config_package = LaunchConfiguration("runtime_config_package")
     controllers_file = LaunchConfiguration("controllers_file")
     description_package = LaunchConfiguration("description_package")
-    description_file = LaunchConfiguration("description_file")
     prefix = LaunchConfiguration("prefix")
     use_mock_hardware = LaunchConfiguration("use_mock_hardware")
     mock_sensor_commands = LaunchConfiguration("mock_sensor_commands")
     robot_controller = LaunchConfiguration("robot_controller")
 
-    # Get URDF via xacro
-    robot_description_content = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
+    # Load robot description
+    robot_description_launch = IncludeLaunchDescription(
+        AnyLaunchDescriptionSource(
             PathJoinSubstitution(
-                [FindPackageShare(description_package), "urdf", description_file]
-            ),
-            " ",
-            "prefix:=",
-            prefix,
-            " ",
-            "use_mock_hardware:=",
-            use_mock_hardware,
-            " ",
-            "mock_sensor_commands:=",
-            mock_sensor_commands,
-            " ",
-        ]
+                [
+                    FindPackageShare(description_package),
+                    "launch",
+                    "$ROBOT_NAME$_description.launch.xml",
+                ]
+            )
+        ),
+        launch_arguments={
+            "prefix": prefix,
+            "use_mock_hardware": use_mock_hardware,
+            "mock_sensor_commands": mock_sensor_commands,
+        }.items(),
     )
 
-    robot_description = {"robot_description": robot_description_content}
-
     robot_controllers = PathJoinSubstitution(
         [FindPackageShare(runtime_config_package), "config", controllers_file]
     )
@@ -137,13 +130,8 @@ def generate_launch_description():
         package="controller_manager",
         executable="ros2_control_node",
         output="both",
-        parameters=[robot_description, robot_controllers],
-    )
-    robot_state_pub_node = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        output="both",
-        parameters=[robot_description],
+        parameters=[robot_controllers],
+        remappings=[("~/robot_description", "/robot_description")],
     )
     rviz_node = Node(
         package="rviz2",
@@ -229,8 +217,8 @@ def generate_launch_description():
     return LaunchDescription(
         declared_arguments
         + [
+            robot_description_launch,
             control_node,
-            robot_state_pub_node,
             rviz_node,
             delay_joint_state_broadcaster_spawner_after_ros2_control_node,
         ]
diff --git a/templates/ros2_control/robot_ros2_control.launch.xml b/templates/ros2_control/robot_ros2_control.launch.xml
index ff75915a..2e152a11 100644
--- a/templates/ros2_control/robot_ros2_control.launch.xml
+++ b/templates/ros2_control/robot_ros2_control.launch.xml
@@ -42,6 +42,12 @@ Authors: Riyan Jose, Manuel Muth, Dr. Denis
   <arg name="mock_sensor_commands"
        default="false"
        description="Enable mock command interfaces for sensors used for simple simulations. Used only if 'use_mock_hardware' parameter is true."/>
+  <arg name="sim_gazebo"
+       default="false"
+       description="Use Gazebo simulation or not."/>
+  <arg name="launch_rviz"
+       default="true"
+       description="Launch RViz?"/>
   <arg name="robot_controller"
        default="joint_trajectory_controller"
        description="Robot controller to start. Choices are: [forward_position_controller, joint_trajectory_controller]."/>
@@ -49,30 +55,45 @@ Authors: Riyan Jose, Manuel Muth, Dr. Denis
        default="forward_position_controller"
        description="Robot controller to start inactive. Choices are: [forward_position_controller, joint_trajectory_controller]."/>
 
-  <let name="robot_description_content" value="$(command '$(find-exec xacro) $(find-pkg-share $(var description_package))/urdf/$(var description_file).urdf.xacro prefix:=$(var prefix) use_mock_hardware:=$(var use_mock_hardware) mock_sensor_commands:=$(var mock_sensor_commands)')"/>
+  <let name="robot_controllers" value="$(find-pkg-share $(var runtime_config_package))/config/$(var controllers_file).yaml" />
 
-
-  <!--robot_state_pub_node-->
-  <node pkg="robot_state_publisher" exec="robot_state_publisher" output="both">
-    <param name="robot_description" value="$(var robot_description_content)"/>
-  </node>
+  <include file="$(find-pkg-share $(var description_package))/launch/$ROBOT_NAME$_description.launch.xml" >
+     <arg name="prefix" value="$(var prefix)" />
+     <arg name="use_mock_hardware" value="$(var use_mock_hardware)" />
+     <arg name="mock_sensor_commands" value="$(var mock_sensor_commands)" />
+     <arg name="sim_gazebo" value="$(var sim_gazebo)" />
+     <arg name="simulation_controllers" value="$(var robot_controllers)" />
+     <arg name="launch_rviz" value="false" />
+  </include>
 
   <!--control_node-->
-  <node pkg="controller_manager" exec="ros2_control_node" output="both">
-    <param name="robot_description" value="$(var robot_description_content)" />
-    <param from="$(find-pkg-share $(var runtime_config_package))/config/$(var controllers_file).yaml"/>
+  <let name="start_controller_manager" value="false" if="$(var sim_gazebo)"/>
+  <let name="start_controller_manager" value="true" unless="$(var sim_gazebo)"/>
+  <node if="$(var start_controller_manager)" pkg="controller_manager" exec="ros2_control_node" output="both">
+       <param from="$(var robot_controllers)"/>
+       <remap from="~/robot_description" to="/robot_description"/>
   </node>
 
   <!--rviz_node-->
-  <node pkg="rviz2" exec="rviz2" output="log" args="-d $(find-pkg-share $(var description_package))/rviz/$ROBOT_NAME$.rviz"/>
+  <let name="start_rviz" value="true" if="$(var launch_rviz)"/>
+  <let name="start_rviz" value="false" unless="$(var launch_rviz)"/>
+  <node if="$(var start_rviz)" pkg="rviz2" exec="rviz2" output="log" args="-d $(find-pkg-share $(var description_package))/rviz/$ROBOT_NAME$.rviz">
+       <param name="use_sim_time" value="$(var sim_gazebo)" />
+  </node>
 
   <!--joint_state_braodcaster_spawner-->
-  <node pkg="controller_manager" exec="spawner" args="joint_state_broadcaster"/>
+  <node pkg="controller_manager" exec="spawner" args="joint_state_broadcaster">
+   <param name="use_sim_time" value="$(var sim_gazebo)" />
+  </node>
 
-  <!--robot_controller_spawner-->
-  <node pkg="controller_manager" exec="spawner" args="$(var robot_controller)"/>
+  <!-- active robot controller spawner -->
+  <node pkg="controller_manager" exec="spawner" args="$(var robot_controller)">
+     <param name="use_sim_time" value="$(var sim_gazebo)" />
+  </node>
 
-  <!--robot_controller_spawner-->
-  <node pkg="controller_manager" exec="spawner" args="$(var inactive_robot_controller) --inactive"/>
+  <!-- inactive robot controller spawner -->
+  <node pkg="controller_manager" exec="spawner" args="$(var inactive_robot_controller) --inactive">
+     <param name="use_sim_time" value="$(var sim_gazebo)" />
+  </node>
 
 </launch>
diff --git a/templates/ros2_control/robot_ros2_control_sim.launch.py b/templates/ros2_control/robot_ros2_control_gazebo.launch.py
similarity index 100%
rename from templates/ros2_control/robot_ros2_control_sim.launch.py
rename to templates/ros2_control/robot_ros2_control_gazebo.launch.py
diff --git a/templates/ros2_control/robot_ros2_control_gazebo.launch.xml b/templates/ros2_control/robot_ros2_control_gazebo.launch.xml
new file mode 100644
index 00000000..6f20475d
--- /dev/null
+++ b/templates/ros2_control/robot_ros2_control_gazebo.launch.xml
@@ -0,0 +1,77 @@
+<!--
+Copyright (c) 2024, 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.
+
+
+Source of this file are templates in [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository.
+
+Author: Dr. Denis
+-->
+
+<launch>
+  <arg name="runtime_config_package"
+       default="$RUNTIME_CONFIG_PKG_NAME$"
+       description="Package with the controller's configuration in 'config' folder. Usually the argument is not set, it enables use of a custom setup."/>
+  <arg name="controllers_file"
+       default="$ROBOT_NAME$_controllers"
+       description="YAML file with the controllers configuration."/>
+  <arg name="prefix"
+       default=""
+       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."/>
+  <arg name="simulation_controllers"
+       default="$(find-pkg-share $(var runtime_config_package))/config/$(var controllers_file).yaml"
+       description="Path to the yaml file with controllers used in the simulation."/>
+  <arg name="description_package"
+       default="$DESCR_PKG_NAME$"
+       description="Description package with robot URDF/xacro files. Usually the argument is not set, it enables use of a custom description."/>
+  <arg name="prefix"
+       default=""
+       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."/>
+  <arg name="robot_controller"
+       default="diffbot_base_controller"
+       description="Robot controller to start. Choices are: [diffbot_base_controller]."/>
+
+  <include file="$(find-pkg-share $(var description_package))/launch/$ROBOT_NAME$_description.launch.xml" >
+       <arg name="prefix" value="$(var prefix)" />
+       <arg name="sim_gazebo" value="true" />
+       <arg name="simulation_controllers" value="$(var simulation_controllers)" />
+       <arg name="launch_rviz" value="false" />
+  </include>
+
+  <node pkg="rviz2" exec="rviz2" output="log" args="-d $(find-pkg-share $(var description_package))/rviz/$ROBOT_NAME$.rviz" >
+      <param name="use_sim_time" value="true" />
+  </node>
+
+  <!--joint_state_braodcaster_spawner-->
+  <node pkg="controller_manager" exec="spawner" args="joint_state_broadcaster">
+      <param name="use_sim_time" value="true" />
+  </node>
+
+  <!--robot_controller_spawner-->
+  <node pkg="controller_manager" exec="spawner" args="$(var robot_controller)">
+      <param name="use_sim_time" value="true" />
+  </node>
+
+  <!-- Start Gazebo -->
+  <let name="gazebo_world" value="$(find-pkg-share $(var runtime_config_package))/environments/empty.sdf" />
+  <include file="$(find-pkg-share ros_gz_sim)/launch/gz_sim.launch.py" >
+      <arg name="gz_args" value=" -r -v 4 $(var gazebo_world)"/>
+  </include>
+
+  <node pkg="ros_gz_bridge" exec="parameter_bridge" args="/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock" output="screen"/>
+
+  <!-- Create robot in Gazebo -->
+  <node pkg="ros_gz_sim" exec="create" output="screen" args="-topic /robot_description"/>
+
+</launch>