diff --git a/.github/workflows/reusable_ici.yml b/.github/workflows/reusable_ici.yml
index 3a39b4c92..a0ee3843d 100644
--- a/.github/workflows/reusable_ici.yml
+++ b/.github/workflows/reusable_ici.yml
@@ -28,6 +28,16 @@ on:
         default: ''
         required: false
         type: string
+      ccache_dir:
+        description: 'Local path to store cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed'
+        default: '.ccache'
+        required: false
+        type: string
+      basedir:
+        description: 'Local path to workspace base directory to cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed'
+        default: '.work'
+        required: false
+        type: string
 
 jobs:
   reusable_ici:
@@ -35,6 +45,9 @@ jobs:
     runs-on: ubuntu-latest
     env:
       DOCKER_RUN_OPTS: '-v /var/run/docker.sock:/var/run/docker.sock --network ursim_net'
+      CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }}
+      BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }}
+      CACHE_PREFIX: ${{ inputs.ros_distro }}-${{ inputs.upstream_workspace }}-${{ inputs.ros_repo }}-${{ github.job }}
     steps:
       - name: Checkout ${{ inputs.ref }} when build is not scheduled
         if: ${{ github.event_name != 'schedule' }}
@@ -45,6 +58,22 @@ jobs:
         with:
           ref: ${{ inputs.ref_for_scheduled_build }}
       - run: docker network create --subnet=192.168.56.0/24 ursim_net
+      - name: cache target_ws
+        if: ${{ ! matrix.env.CCOV }}
+        uses: pat-s/always-upload-cache@v3.0.11
+        with:
+          path: ${{ env.BASEDIR }}/target_ws
+          key: target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}-${{ github.run_id }}
+          restore-keys: |
+            target_ws-${{ env.CACHE_PREFIX }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml') }}
+      - name: cache ccache
+        uses: pat-s/always-upload-cache@v3.0.11
+        with:
+          path: ${{ env.CCACHE_DIR }}
+          key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
+          restore-keys: |
+            ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
+            ccache-${{ env.CACHE_PREFIX }}
       - uses: 'ros-industrial/industrial_ci@master'
         env:
           UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }}
@@ -52,3 +81,10 @@ jobs:
           ROS_REPO: ${{ inputs.ros_repo }}
           CMAKE_ARGS: -DUR_ROBOT_DRIVER_BUILD_INTEGRATION_TESTS=ON
           ADDITIONAL_DEBS: docker.io netcat-openbsd  # Needed for integration tests
+      - name: prepare target_ws for cache
+        if: ${{ always() && ! matrix.env.CCOV }}
+        run: |
+          du -sh ${{ env.BASEDIR }}/target_ws
+          sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete
+          sudo rm -rf ${{ env.BASEDIR }}/target_ws/src
+          du -sh ${{ env.BASEDIR }}/target_ws
\ No newline at end of file
diff --git a/Universal_Robots_ROS2_Driver.humble.repos b/Universal_Robots_ROS2_Driver.humble.repos
index 61ee8ef3a..456c5f25b 100644
--- a/Universal_Robots_ROS2_Driver.humble.repos
+++ b/Universal_Robots_ROS2_Driver.humble.repos
@@ -27,3 +27,12 @@ repositories:
     type: git
     url: https://github.com/ros-controls/control_msgs.git
     version: humble
+  moveit2:
+    type: git
+    url: https://github.com/ros-planning/moveit2.git
+    version: humble
+
+  moveit_msgs:
+    type: git
+    url: https://github.com/ros-planning/moveit_msgs.git
+    version: humble
diff --git a/Universal_Robots_ROS2_Driver.iron.repos b/Universal_Robots_ROS2_Driver.iron.repos
index f59c5d0e5..3270a7d02 100644
--- a/Universal_Robots_ROS2_Driver.iron.repos
+++ b/Universal_Robots_ROS2_Driver.iron.repos
@@ -27,3 +27,12 @@ repositories:
     type: git
     url: https://github.com/ros-controls/control_msgs.git
     version: master
+  moveit2:
+    type: git
+    url: https://github.com/ros-planning/moveit2.git
+    version: iron
+
+  moveit_msgs:
+    type: git
+    url: https://github.com/ros-planning/moveit_msgs.git
+    version: iron
diff --git a/Universal_Robots_ROS2_Driver.rolling.repos b/Universal_Robots_ROS2_Driver.rolling.repos
index f7dd6bee1..371895282 100644
--- a/Universal_Robots_ROS2_Driver.rolling.repos
+++ b/Universal_Robots_ROS2_Driver.rolling.repos
@@ -27,3 +27,12 @@ repositories:
     type: git
     url: https://github.com/ros-controls/control_msgs.git
     version: master
+  moveit2:
+    type: git
+    url: https://github.com/ros-planning/moveit2.git
+    version: main
+
+  moveit_msgs:
+    type: git
+    url: https://github.com/ros-planning/moveit_msgs.git
+    version: ros2
diff --git a/ci_status.md b/ci_status.md
index 376cad650..a364b0b55 100644
--- a/ci_status.md
+++ b/ci_status.md
@@ -115,5 +115,5 @@ red pipeline there should be a corresponding issue labeled with [ci-failure](htt
 
 Each of these stages also performs integration tests using ursim. In order to execute these tests locally, they have to be enabled:
   ```
-  colcon build --packages-select ur_robot_driver --cmake-args -DUR_ROBOT_DRIVER_BUILD_INTEGRATION_TESTS=On
+  colcon build --packages-select ur_robot_driver ur_moveit_config --cmake-args -DUR_ROBOT_DRIVER_BUILD_INTEGRATION_TESTS=On
   ```
diff --git a/ur_moveit_config/CMakeLists.txt b/ur_moveit_config/CMakeLists.txt
index e47ea7642..164777280 100644
--- a/ur_moveit_config/CMakeLists.txt
+++ b/ur_moveit_config/CMakeLists.txt
@@ -13,3 +13,15 @@ ament_python_install_package(${PROJECT_NAME})
 ament_python_install_module(${PROJECT_NAME}/launch_common.py)
 
 ament_package()
+
+if(BUILD_TESTING)
+  find_package(ur_robot_driver REQUIRED)
+  find_package(launch_testing_ament_cmake)
+
+  if(${UR_ROBOT_DRIVER_BUILD_INTEGRATION_TESTS})
+    add_launch_test(test/startup_test.py
+      TIMEOUT
+        180
+    )
+  endif()
+endif()
diff --git a/ur_moveit_config/config/ur_servo.yaml b/ur_moveit_config/config/ur_servo.yaml
index 767481a8a..ef2c79d3d 100644
--- a/ur_moveit_config/config/ur_servo.yaml
+++ b/ur_moveit_config/config/ur_servo.yaml
@@ -52,7 +52,7 @@ num_outgoing_halt_msgs_to_publish: 4
 ## Configure handling of singularities and joint limits
 lower_singularity_threshold:  100.0  # Start decelerating when the condition number hits this (close to singularity)
 hard_stop_singularity_threshold: 200.0 # Stop when the condition number hits this
-joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
 
 ## Topic names
 cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands
diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py
index a73bd73be..a261200ee 100644
--- a/ur_moveit_config/launch/ur_moveit.launch.py
+++ b/ur_moveit_config/launch/ur_moveit.launch.py
@@ -256,6 +256,7 @@ def launch_setup(context, *args, **kwargs):
             servo_params,
             robot_description,
             robot_description_semantic,
+            robot_description_kinematics,
         ],
         output="screen",
     )
diff --git a/ur_moveit_config/package.xml b/ur_moveit_config/package.xml
index 2b6a026fa..457dc27b8 100644
--- a/ur_moveit_config/package.xml
+++ b/ur_moveit_config/package.xml
@@ -32,6 +32,9 @@
   <exec_depend>warehouse_ros_sqlite</exec_depend>
   <exec_depend>xacro</exec_depend>
 
+  <test_depend>launch_testing_ament_cmake</test_depend>
+  <test_depend>ur_robot_driver</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>
diff --git a/ur_moveit_config/test/startup_test.py b/ur_moveit_config/test/startup_test.py
new file mode 100644
index 000000000..35effc499
--- /dev/null
+++ b/ur_moveit_config/test/startup_test.py
@@ -0,0 +1,178 @@
+#!/usr/bin/env python
+# Copyright 2024, FZI Forschungszentrum Informatik
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the {copyright_holder} nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import pytest
+import unittest
+
+from launch import LaunchDescription
+import launch_testing
+from launch_testing.actions import ReadyToTest
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    RegisterEventHandler,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackagePrefix, FindPackageShare
+from launch.event_handlers import OnProcessExit
+
+import rclpy
+import rclpy.node
+from moveit_msgs.srv import GetPlanningScene
+
+TIMEOUT_WAIT_SERVICE_INITIAL = 120  # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable.
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    controller_spawner_timeout = TIMEOUT_WAIT_SERVICE_INITIAL
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_type",
+            default_value="ur5e",
+            description="Type/series of used UR robot.",
+            choices=[
+                "ur3",
+                "ur3e",
+                "ur5",
+                "ur5e",
+                "ur10",
+                "ur10e",
+                "ur16e",
+                "ur20",
+                "ur30",
+            ],
+        )
+    )
+
+    ur_type = LaunchConfiguration("ur_type")
+
+    robot_driver = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"]
+            )
+        ),
+        launch_arguments={
+            "robot_ip": "192.168.56.101",
+            "ur_type": ur_type,
+            "launch_rviz": "false",
+            "controller_spawner_timeout": str(controller_spawner_timeout),
+            "initial_joint_controller": "scaled_joint_trajectory_controller",
+            "headless_mode": "true",
+            "launch_dashboard_client": "false",
+            "start_joint_controller": "false",
+        }.items(),
+    )
+    wait_dashboard_server = ExecuteProcess(
+        cmd=[
+            PathJoinSubstitution(
+                [
+                    FindPackagePrefix("ur_robot_driver"),
+                    "bin",
+                    "wait_dashboard_server.sh",
+                ]
+            )
+        ],
+        name="wait_dashboard_server",
+        output="screen",
+    )
+    driver_starter = RegisterEventHandler(
+        OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver)
+    )
+    moveit_setup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("ur_moveit_config"), "launch", "ur_moveit.launch.py"]
+            )
+        ),
+        launch_arguments={
+            "ur_type": ur_type,
+            "launch_rviz": "false",
+            "launch_servo": "false",  # the servo node currently doesn't startup correctly,
+            # it waits for the robot state until the robot has actually
+            # executed a plan. Therefore, it doesn't exit cleanly which
+            # breaks this test.
+        }.items(),
+    )
+
+    return LaunchDescription(
+        declared_arguments
+        + [ReadyToTest(), wait_dashboard_server, _ursim_action(), driver_starter, moveit_setup]
+    )
+
+
+def _ursim_action():
+    ur_type = LaunchConfiguration("ur_type")
+
+    return ExecuteProcess(
+        cmd=[
+            PathJoinSubstitution(
+                [
+                    FindPackagePrefix("ur_client_library"),
+                    "lib",
+                    "ur_client_library",
+                    "start_ursim.sh",
+                ]
+            ),
+            " ",
+            "-m ",
+            ur_type,
+        ],
+        name="start_ursim",
+        output="screen",
+    )
+
+
+class TestReadyForPlanning(unittest.TestCase):
+    def test_read_stdout(self, proc_output):
+        """Check if 'You can start planning now!' was found in the stdout."""
+        proc_output.assertWaitFor("You can start planning now!", timeout=120, stream="stdout")
+        proc_output.assertWaitFor(
+            "Dashboard server connections are possible.", timeout=120, stream="stdout"
+        )
+
+    def testCallingMoveItService(self):
+        # Dummy test to make sure MoveIt lives long enough...
+        rclpy.init()
+        node = rclpy.node.Node("ur_moveit_test")
+
+        cli = node.create_client(GetPlanningScene, "/get_planning_scene")
+        while not cli.wait_for_service(timeout_sec=120.0):
+            node.get_logger().info("service not available, waiting again...")
+
+
+@launch_testing.post_shutdown_test()
+class TestProcessExitCode(unittest.TestCase):
+    def test_exit_code(self, proc_info):
+        launch_testing.asserts.assertExitCodes(proc_info)