diff --git a/Jenkinsfile b/Jenkinsfile
deleted file mode 100644
index 29ee0efa..00000000
--- a/Jenkinsfile
+++ /dev/null
@@ -1,13 +0,0 @@
-@Library('bitbots_jenkins_library') import de.bitbots.jenkins.*;
-
-defineProperties()
-
-def pipeline = new BitbotsPipeline(this, env, currentBuild, scm)
-pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_bringup")))
-pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_ceiling_cam")).withoutDocumentation())
-pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_convenience_frames")))
-pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_live_tool_rqt")))
-pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_teleop")))
-pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("bitbots_time_constraint")))
-pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("system_monitor")))
-pipeline.execute()
diff --git a/bitbots_bringup/.rdmanifest b/bitbots_bringup/.rdmanifest
deleted file mode 100644
index ce11239f..00000000
--- a/bitbots_bringup/.rdmanifest
+++ /dev/null
@@ -1,12 +0,0 @@
----
-# See http://doku.bit-bots.de/meta/manual/software/ci.html#make-package-resolvable-in-ci
-check-presence-script: '#!/bin/bash
-
-  test -d $BITBOTS_CATKIN_WORKSPACE/src/bitbots_bringup'
-depends:
-- bitbots_docs
-exec-path: bitbots_misc-master/bitbots_bringup
-install-script: '#!/bin/bash
-
-  cp -r . $BITBOTS_CATKIN_WORKSPACE/src/bitbots_bringup'
-uri: https://github.com/bit-bots/bitbots_misc/archive/refs/heads/master.tar.gz
diff --git a/bitbots_bringup/README b/bitbots_bringup/README
deleted file mode 100644
index 2a1b778c..00000000
--- a/bitbots_bringup/README
+++ /dev/null
@@ -1,377 +0,0 @@
-pylon 5 Camera Software Suite for Linux for Use with Basler
-Gigabit Ethernet(GigE) and Basler USB 3.0 Cameras (U3V)
-================================================================
-
-Note:
-On some embedded platforms, the pylon Viewer and the IP Configurator
-applications may not be available.
-
-System Requirements
-===================
-
-GigE
-----
-A GigE network adapter that supports jumbo frames is recommended. Concerning
-performance and reliability we made best experiences with the Intel PRO 1000,
-I210, I340 and I350 series.
-Although the pylon software will work with any GigE network adapter, we would
-recommend to use one of these adapters.
-
-USB
----
-For U3V devices a USB3-capable USB controller is necessary. For best
-performance and stability we highly recommend a kernel version >= 3.13.x
-
-
-Installation
-=============
-
-The installation of pylon for Linux is described in the INSTALL text document.
-
-
-
-Performance Optimization
-========================
-
-To increase performance and to minimize CPU usage when grabbing images, the
-following settings should be considered:
-
-GigE Devices
-------------
-* Enable Jumbo Frames.
-  Many GigE network adapters support so-called jumbo frames, i.e., network
-  packets larger than the usual 1500 bytes. To enable jumbo frames, the maximum
-  transfer unit (MTU) size of the PC's network adapter must be set to a high
-  value (see the description in the INSTALL document). We recommend using a
-  value of 8192.
-
-* Increase the packet size.
-  When jumbo frames are enabled, the camera's packet size must be increased to
-  benefit from the larger packets. The 'Optimizing Packet Size' section of the
-  INSTALL document describes how to set the packet size.
-
-* Real-time Priority
-  The GigE Vision implementation of Basler pylon software uses a thread for
-  receiving image data. Basler pylon tries to set the thread priority for the
-  receive thread to real-time thread priority. This requires certain
-  permissions. The 'Permissions for Real-time Thread Priorities' section of the
-  INSTALL document describes how to grant the required permissions.
-
-U3V Devices
------------
-* Increasing Packet Size
-  For faster USB transfers you should increase the packet size. You can do this
-  by changing the "Stream Parameters" -> "Maximum Transfer Size" value from
-  inside the pylon Viewer or by setting the corresponding value via the API.
-  After increasing the package size you will likely run out of kernel space
-  and see corresponding error messages on the console. The default value set
-  by the kernel is 16 MB. To set the value (in this example to 1000 MB) you can
-  execute as root:
-    echo 1000 > /sys/module/usbcore/parameters/usbfs_memory_mb
-  This would assign a maximum of 1000 MB to the USB stack.
-
-
-Documentation
-=============
-
-The installation includes a "share/doc/C++" sub-folder containing the
-pylon Programmer's Guide and API reference documentation. Open the
-index.html file with an internet browser.
-
-
-Sample Programs
-===============
-
-The installation archive includes a set of sample programs. These sample
-programs are simple command line programs showing the basic pylon use cases.
-They are located in the "Samples/C++" and "Samples/C" folder. Each folder contains
-a top-level Makefile that can be used to build the different sample programs.
-
-* C++ samples:
-  If you installed pylon in /opt/pylon5, run:
-    cd Samples/C++
-    make
-    ./Grab/Grab
-
-  If pylon is installed in a non-standard location, an additional step is required:
-    cd Samples/C++
-    source <path to pylon>/bin/pylon-setup-env.sh <path to pylon>
-    make
-    ./Grab/Grab
-
-* C samples:
-  If you installed pylon in /opt/pylon5, run:
-    cd Samples/C
-    make
-    ./SimpleGrab/SimpleGrab
-
-  If pylon is installed in a non-standard location, an additional step is required:
-    cd Samples/C
-    source <path to pylon>/bin/pylon-setup-env.sh <path to pylon>
-    make
-    ./SimpleGrab/SimpleGrab
-
-
-Environment Variables
-=====================
-
-From pylon 5.0 upwards, no additional environment variables are required to
-run pylon-based applications.
-
-For development, though, the compiler must know where pylon is installed.
-The pylon samples use the environment variable PYLON_ROOT to find the relevant information.
-
-For your convenience, we created the pylon-setup-env.sh script located in the
-pylon5/bin directory which can carry out the complete setup.
-
-To setup the environment for a pylon installation in <path> execute:
-  source <path>/bin/pylon-setup-env.sh <path>
-
-If you want the environment for the standard installation to be persistent, you
-can add
-   source /path/to/your/pylon5/bin/pylon-setup-env.sh /path/to/your/pylon5
-to ~/.bashrc
-
-For special use cases, you can do a manual environment setup as follows:
-
-   export PYLON_ROOT=/path/to/your/pylon5
-
-Even though there is currently only one variable needed, using pylon-setup-env.sh
-is still the preferred way to set up your environment, as we might add more variables
-in the future.
-
-
-Camera Emulator
-===============
-
-In addition to camera transport layers like GigE Vision or USB3 Vision, pylon offers
-a transport layer that can create simple camera emulation devices. This allows you to
-develop applications without the need for a physical camera. It is also useful if you
-want to develop a multi-camera application and don't have enough cameras at hand.
-Besides emulating image acquisition and standard camera features, camera emulation
-devices also offer features that a physical camera does not offer:
-- You can display custom test images, e.g., to optimize your image processing algorithms.
-- You can generate failed buffers, e.g., to test your exception handling routines.
-
-The number of available emulator devices can be controlled by exporting the
-PYLON_CAMEMU environment variable. For example,
-
-   export PYLON_CAMEMU=2
-
-will provide two emulator devices. These devices are accessible both by using
-the pylon API and the pylon Viewer program.
-
-When PYLON_CAMEMU is not set, no emulator devices are provided.
-Note: A maximum of 256 emulator devices are supported.
-
-For more information on the camera emulator refer to the Basler Product Documentation,
-which is included in this package (press F1 in the pylon Viewer).
-
-
-GenTL producers
-===============
-
-In addition to the pylon 5 Camera Software Suite for Linux, this release contains
-GenTL 1.5 compliant producers for USB3 Vision and GigE Vision standards.
-
-To use the Basler GenTL producers you need to setup your environment as follows
-
-   export GENICAM_GENTL32_PATH=/path/to/your/pylon5/lib/gentlproducer/gtl
-   export GENICAM_GENTL64_PATH=/path/to/your/pylon5/lib64/gentlproducer/gtl
-
-
-Troubleshooting
-===============
-
-* Problem: When trying to grab frames from multiple cameras, the error message
-  "PrepareGrab (StartStreaming) failed for device" or "Insufficient system
-  resources exist to complete the API" are displayed.
-
-  Solution: This condition can occur when the number of available open file
-  descriptors is exhausted. The limit can often be raised for the current
-  process with the command: ulimit -n <number>
-
-
-GigE Devices
-------------
-* Problem: I can't see my camera in the pylon Viewer, even after waiting for
-  more than one minute. (Note: Depending on the camera's and adapter's
-  IP configuration it can take up to one minute until a valid IP address is
-  assigned to the camera.)
-
-  Solution: Start the Basler IP Configurator (/opt/pylon5/bin/IpConfigurator).
-  Is the camera shown by the IP Configurator?
-  If "yes", the camera's IP address is in a different subnet than the network
-  adapter's IP address. Both, the camera and the network adapter, must use IP
-  addresses within the same subnet. Refer to the INSTALL document for the IP
-  address setup.
-
-  If "no", the most likely reason is an enabled firewall. As described in the
-  INSTALL document, disable the firewall for those network adapters cameras
-  will be connected to. If you don't have a firewall, enabled reverse-path
-  filtering in the kernel may prevent detection of the camera. Refer to the
-  next problem description for further details.
-
-* Problem: The IP Configurator can't see my camera. I'm not able to
-  reconfigure the camera to make it visible again.
-
-  Solution: First make sure you don't have a firewall enabled on your network
-  interface the camera is connected to.
-  If you still can't see the camera, reverse path filtering in the kernel may
-  prevent the IP Configurator to detect the camera. On some Linux
-  distributions reverse path filtering may prevent the discovery of GigE Vision
-  cameras. This can happen if the camera's IP address is not within the same
-  subnet as the network adapter the camera is attached to. Normally the IP
-  Configurator can handle this by using broadcasts to discover the camera on
-  any subnet. Reverse-path filtering may prevent the IP Configurator to receive
-  the answer from the broadcast which in turn prevents the IP Configurator
-  from detecting the camera. To check whether filtering is turned on, run the
-  following command:
-    sysctl -a 2>/dev/null | grep '\.rp_filter'
-
-  in the output look for the following lines:
-   net.ipv4.conf.all.rp_filter=1
-   net.ipv4.conf.eth1.rp_filter=1
-
-  where "eth1" is the network adapter the camera is connected to.
-  The "net.ipv4.conf.all.rp_filter" is a global switch which must be turned off.
-  The "net.ipv4.conf.eth1.rp_filter" tells whether filtering for the specified
-  network adapter is activated. To disable filtering, you must first turn off
-  filtering for "all" and the specific network interface (in this
-  sample "eth1"). Use the following commands to change the filtering behavior
-  at runtime:
-    sudo sysctl net.ipv4.conf.all.rp_filter=0
-    sudo sysctl net.ipv4.conf.eth1.rp_filter=0
-
-  Restart the IP Configurator and check whether the camera(s) are
-  detected. Reconfigure the camera(s) and use "Write Configuration" to make
-  your changes persistent. You can re-enable filtering by executing the same
-  commands but set a value of 1. If you want to turn off filtering permanently,
-  you can edit the same values in /etc/sysctl.conf.
-
-  From Linux kernel version 2.6.32 onwards, the rp_filter settings allow strict
-  and loose filtering. To accept asymmetrically routed packets, modify /etc/sysctl.conf:
-    net.ipv4.conf.default.rp_filter = 2
-    net.ipv4.conf.all.rp_filter = 2
-
-* Problem: When grabbing images, the CPU load is higher than expected.
-
-  Solution: Ensure that jumbo frames are enabled and large network packets are
-  used as described above in the 'Performance Optimization' section.
-
-
-* Problem: Grabbing images leads to errors with error code 0x81010014.
-  This error code indicates that the PC received incomplete images, i.e.,
-  network packets have been dropped.
-
-  Solution 1: Ensure that jumbo frames are enabled as described above in the
-  'Performance Optimization' section. Also make sure that the camera's packet
-  size is set to a high value, if possible 8192.
-
-  Solution 2: Increase the maximum UDP receive buffer size to a value that is
-  large enough, e.g. by issuing the sudo sysctl net.core.rmem_max=2097152
-  command. This allows pylon to increase the socket buffer size to 2 MB to
-  ensure a stable image acquisition. To make this setting persistent, you can
-  add the net.core.rmem_max setting to the /etc/sysctl.conf file.
-
-  Solution 3: Ensure that the application has the required permissions to set
-  the pylon receive thread's priority to real-time thread priority as described
-  in the INSTALL document.
-
-  Solution 4: Check the cable. For GigE, Cat 5e cables are recommended. Poor
-  cable quality or damaged cables can lead to unrecoverable transmission errors.
-
-  Solution 5: The amount of data produced by the camera(s) exceeds the amount
-  of bandwidth the network adapter(s) or the PC can provide. Reduce the camera
-  frame rate by increasing the camera's "Inter Packet Delay" parameter. When
-  using the pylon API, the inter packet delay is controlled via the GevSCPD
-  parameter.
-
-
-* Problem: The pylon Viewer seems to acquire images but no images are displayed.
-
-  Solution 1: Start the pylon Viewer from the command line to see if the viewer
-  prints out any messages.
-
-  Solution 2: The viewer only receives incomplete frames. In that case, error
-  messages such as "Failed to grab image: GX status 0x81010014" are displayed.
-  Please refer to the previous problem for tips about how to solve this issue.
-
-  Solution 3: No error messages are printed out. All data packets transmitted
-  from the camera seem to be discarded. Make sure that the camera's current
-  packet size doesn't exceed the network adapter's MTU size.
-
-  If the packet size is less than or equal to the MTU size, but greater than
-  1500, stop image acquisition and set the camera's packet size to 1500.
-  Restart image acquisition. If an image is displayed using this packet size,
-  either jumbo frames are not enabled on the network adapter or the adapter
-  doesn't support jumbo frames.
-
-  Solution 3: Grabbing images is failing for other reasons. Please report the
-  error messages printed out by the viewer to Basler technical support.
-
-
-U3V Devices
------------
-* Problem: The camera is not found during enumeration.
-  Solution: Check the user permissions on the device. You must have read/write
-  permissions. See the output of
-    ls -lR /dev/bus/usb
-  to check if you have read/write permissions. To setup the correct udev rules,
-  execute:
-    ./setup-usb.sh
-  from within the directory containing this README.
-
-
-Known Issues
-============
-
-* Same IP address for camera and network interface.
-  If the camera has been assigned a permanent IP address that equals the address
-  assigned to the network adapter, the camera will not be accessible, even when
-  using the IP Configurator.
-
-  Either change the network adapter's address or use a different PC to assign a
-  different IP address to the camera.
-
-
-* Error messages are printed by the pylon Viewer when image acquisition is stopped.
-  When the pylon Viewer is started from a command line and image acquisition is
-  stopped, the Viewer prints out error messages such as
-  "Failed to grab image: GX status 0x81010017". This diagnostic output
-  indicates that image buffers have been cancelled by the pylon GigE vision
-  library on user request instead of being filled with image data.
-  This is expected behavior.
-
-
-Version Infos
-=============
-
-pylon has been built using the following tools.
-
-Linux x86 32bit/64bit:
-    Build system: Ubuntu 14.04
-    Architecture: i686/x86_64
-    libc-2.19.so
-    libstdc++.so.6.0.19
-    libxcb.so.1.1.0
-    libfontconfig.so.1.8.0
-    libfreetype.so.6.11.1
-
-Linux armhf:
-    Build system: Ubuntu 14.04
-    Architecture: armv7l
-    libc-2.19.so
-    libstdc++.so.6.0.22
-    libxcb.so.1.1.0
-    libfontconfig.so.1.8.0
-    libfreetype.so.6.11.1
-
-Linux arm64:
-    Build system: Ubuntu 14.04
-    Architecture: aarch64 (armv8)
-    libc-2.19.so
-    libstdc++.so.6.0.22
-    libxcb.so.1.1.0
-    libfontconfig.so.1.8.0
-    libfreetype.so.6.11.1
diff --git a/bitbots_utils/launch/README.md b/bitbots_bringup/launch/README.md
similarity index 88%
rename from bitbots_utils/launch/README.md
rename to bitbots_bringup/launch/README.md
index 4eb68704..87ead6c0 100644
--- a/bitbots_utils/launch/README.md
+++ b/bitbots_bringup/launch/README.md
@@ -3,7 +3,7 @@
 Launch these commands in parallel:
 
 ```bash
-ros2 launch bitbots_utils audio.launch
+ros2 launch bitbots_bringup audio.launch
 ros2 bag record /audio/audio /audio/audio_info
 ```
 
diff --git a/bitbots_utils/launch/audio.launch b/bitbots_bringup/launch/audio.launch
similarity index 100%
rename from bitbots_utils/launch/audio.launch
rename to bitbots_bringup/launch/audio.launch
diff --git a/bitbots_bringup/launch/game_start.launch b/bitbots_bringup/launch/game_start.launch
deleted file mode 100644
index 01f2e66b..00000000
--- a/bitbots_bringup/launch/game_start.launch
+++ /dev/null
@@ -1,4 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <node name="game_settings" pkg="bitbots_utils" exec="game_settings.py" output="screen"/>
-</launch>
diff --git a/bitbots_bringup/launch/highlevel.launch b/bitbots_bringup/launch/highlevel.launch
index db750d70..c89d2f1c 100644
--- a/bitbots_bringup/launch/highlevel.launch
+++ b/bitbots_bringup/launch/highlevel.launch
@@ -50,7 +50,7 @@
             <arg name="sim" value="$(var sim)"/>
         </include>
     </group>
-    
+
     <!-- launch localization or fake localization -->
     <group if="$(var localization)">
         <include file="$(find-pkg-share bitbots_localization)/launch/localization.launch">
@@ -81,7 +81,7 @@
 
     <!-- launch audio processing -->
     <group if="$(var audio)">
-        <include file="$(find-pkg-share bitbots_utils)/launch/audio.launch">
+        <include file="$(find-pkg-share bitbots_bringup)/launch/audio.launch">
             <arg name="sim" value="$(var sim)" />
         </include>
     </group>
diff --git a/bitbots_utils/launch/monitoring.launch b/bitbots_bringup/launch/monitoring.launch
similarity index 100%
rename from bitbots_utils/launch/monitoring.launch
rename to bitbots_bringup/launch/monitoring.launch
diff --git a/bitbots_bringup/launch/motion.launch b/bitbots_bringup/launch/motion.launch
index e6d38a63..75cf5f94 100644
--- a/bitbots_bringup/launch/motion.launch
+++ b/bitbots_bringup/launch/motion.launch
@@ -5,6 +5,17 @@
     <arg name="walking" default="true" description="start the walking" />
     <arg name="torqueless_mode" default="false" description="start without torque, for example for testing the falling detection"/>
 
+    <!-- launch the base footprint -->
+    <node pkg="humanoid_base_footprint" exec="base_footprint" output="screen">
+      <param name="support_state_topics" value="[walk_support_state, dynamic_kick_support_state]"/>
+      <param name="use_sim_time" value="$(var sim)"/>
+    </node>
+
+    <!-- launch the odometry -->
+    <include file="$(find-pkg-share bitbots_odometry)/launch/odometry.launch">
+      <arg name="sim" value="$(var sim)"/>
+    </include>
+
     <!-- if not in simulation or visualization, launch ros_control -->
     <group unless="$(var sim)">
         <group unless="$(var viz)">
diff --git a/bitbots_bringup/launch/motion_standalone.launch b/bitbots_bringup/launch/motion_standalone.launch
index 1c152072..01cd78d0 100644
--- a/bitbots_bringup/launch/motion_standalone.launch
+++ b/bitbots_bringup/launch/motion_standalone.launch
@@ -3,8 +3,12 @@
     <arg name="sim" default="false"/>
     <arg name="viz" default="false"/>
     <arg name="torqueless_mode" default="false" description="start without torque, for example for testing the falling detection"/>
-    
-    <include file="$(find-pkg-share bitbots_utils)/launch/base.launch">
+
+    <include file="$(find-pkg-share bitbots_utils)/launch/parameter_blackboard.launch">
+        <arg name="sim" value="$(var sim)"/>
+    </include>
+
+    <include file="$(find-pkg-share bitbots_robot_description)/launch/load_robot_description.launch">
         <arg name="sim" value="$(var sim)"/>
     </include>
 
diff --git a/bitbots_utils/launch/rosbag_record.launch.py b/bitbots_bringup/launch/rosbag_record.launch.py
similarity index 93%
rename from bitbots_utils/launch/rosbag_record.launch.py
rename to bitbots_bringup/launch/rosbag_record.launch.py
index 1d2363bf..ccf84b64 100644
--- a/bitbots_utils/launch/rosbag_record.launch.py
+++ b/bitbots_bringup/launch/rosbag_record.launch.py
@@ -6,9 +6,7 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction
-from launch.conditions import IfCondition
-from launch.substitutions import EnvironmentVariable, EqualsSubstitution, LaunchConfiguration, NotEqualsSubstitution, PathJoinSubstitution
-from launch_ros.actions import Node
+from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution
 
 
 TOPICS_TO_RECORD: List[str] = [
@@ -47,7 +45,6 @@
     '/joint_states',
     '/motion_odometry',
     '/move_base/current_goal',
-    '/obstacles_relative',
     '/pose_with_covariance',
     '/robot_state',
     '/robots_relative',
diff --git a/bitbots_bringup/launch/rviz_multirobot.launch b/bitbots_bringup/launch/rviz_multirobot.launch
deleted file mode 100644
index f8499827..00000000
--- a/bitbots_bringup/launch/rviz_multirobot.launch
+++ /dev/null
@@ -1,10 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <node pkg="tf2_ros" exec="static_transform_publisher" name="amy_map" args="0 0 0 0 0 0 1 map amy/map" />
-    <node pkg="tf2_ros" exec="static_transform_publisher" name="rory_map" args="0 0 0 0 0 0 1 map rory/map" />
-    <node pkg="tf2_ros" exec="static_transform_publisher" name="jack_map" args="0 0 0 0 0 0 1 map jack/map" />
-    <node pkg="tf2_ros" exec="static_transform_publisher" name="donna_map" args="0 0 0 0 0 0 1 map donna/map" />
-    <node pkg="tf2_ros" exec="static_transform_publisher" name="melody_map" args="0 0 0 0 0 0 1 map melody/map" />
-
-    <node pkg="rviz" exec="rviz" name="rviz" args="-d $(find-pkg-share bitbots_utils)/config/multirobot.rviz" required="true"/>
-</launch>
diff --git a/bitbots_bringup/launch/teamplayer.launch b/bitbots_bringup/launch/teamplayer.launch
index 7697647a..c6525172 100644
--- a/bitbots_bringup/launch/teamplayer.launch
+++ b/bitbots_bringup/launch/teamplayer.launch
@@ -14,11 +14,16 @@
     <arg name="vision" default="true" description="Whether the vision system should be started" />
     <arg name="world_model" default="true" description="Whether the world model should be started"/>
 
-    <!-- load the robot description -->
-    <include file="$(find-pkg-share bitbots_utils)/launch/base.launch">
+    <!-- load the global parameters -->
+    <include file="$(find-pkg-share bitbots_utils)/launch/parameter_blackboard.launch">
         <arg name="sim" value="$(var sim)" />
     </include>
 
+    <!-- load the robot description -->
+    <include file="$(find-pkg-share bitbots_robot_description)/launch/load_robot_description.launch">
+        <arg name="sim" value="$(var sim)"/>
+    </include>
+
     <!-- load the motion -->
     <group if="$(var motion)">
         <include file="$(find-pkg-share bitbots_bringup)/launch/motion.launch">
diff --git a/bitbots_utils/launch/tools.launch b/bitbots_bringup/launch/tools.launch
similarity index 74%
rename from bitbots_utils/launch/tools.launch
rename to bitbots_bringup/launch/tools.launch
index ca61966c..464deed6 100644
--- a/bitbots_utils/launch/tools.launch
+++ b/bitbots_bringup/launch/tools.launch
@@ -5,11 +5,11 @@
     <arg name="sim" default="false" description="Whether the robot is running in simulation or on real hardware" />
 
     <group if="$(var monitoring)">
-        <include file="$(find-pkg-share bitbots_utils)/launch/monitoring.launch" />
+        <include file="$(find-pkg-share bitbots_bringup)/launch/monitoring.launch" />
     </group>
 
     <group if="$(var record)">
-        <include file="$(find-pkg-share bitbots_utils)/launch/rosbag_record.launch.py">
+        <include file="$(find-pkg-share bitbots_bringup)/launch/rosbag_record.launch.py">
             <arg name="sim" value="$(var sim)" />
         </include>
     </group>
diff --git a/bitbots_bringup/launch/vision_standalone.launch b/bitbots_bringup/launch/vision_standalone.launch
index dc0fc8d8..38839102 100644
--- a/bitbots_bringup/launch/vision_standalone.launch
+++ b/bitbots_bringup/launch/vision_standalone.launch
@@ -5,8 +5,8 @@
     <arg name="camera" default="true" description="true: launches an image provider to get images from a camera (unless sim:=true)" />
     <arg name="debug" default="false" description="true: activates publishing of several debug images" />
 
-    <!-- Add base -->
-    <include file="$(find-pkg-share bitbots_utils)/launch/base.launch">
+    <!-- Load the global parameters-->
+    <include file="$(find-pkg-share bitbots_utils)/launch/parameter_blackboard.launch">
         <arg name="sim" value="$(var sim)" />
     </include>
 
diff --git a/bitbots_bringup/launch/visualization.launch b/bitbots_bringup/launch/visualization.launch
index 73091c00..fc9f3bea 100644
--- a/bitbots_bringup/launch/visualization.launch
+++ b/bitbots_bringup/launch/visualization.launch
@@ -3,15 +3,11 @@
     <!-- arguments -->
     <arg name="behavior" default="true" description="if the behavior should be started" />
     <arg name="ipm" default="false" description="if the soccer ipm should be used" />
-    <arg name="localization" default="false" description="if the localization should be used" />
     <arg name="motion" default="true" description="if the motion should be started" />
     <arg name="game_controller" default="false" description="if the game controller node should be started" />
 
-    <!-- set visualization parameter -->
-    <!--param name="visualization_active" value="true" /-->
-
-    <!-- load the robot description -->
-    <include file="$(find-pkg-share bitbots_utils)/launch/base.launch" />
+    <!-- load the global parameters -->
+    <include file="$(find-pkg-share bitbots_utils)/launch/parameter_blackboard.launch" />
 
     <!-- publish dummy imu -->
     <node name="dummy_imu" pkg="bitbots_utils" exec="dummy_imu.py" />
@@ -25,12 +21,15 @@
     <include file="$(find-pkg-share bitbots_bringup)/launch/highlevel.launch">
         <arg name="behavior" value="$(var behavior)" />
         <arg name="ipm" value="$(var ipm)" />
-        <arg name="localization" value="$(var localization)" />
+        <arg name="localization" value="false" />
         <arg name="game_controller" value="$(var game_controller)" />
         <arg name="vision" value="false" />
         <arg name="ipm" value="false" />
     </include>
 
+    <!-- simulate localization -->
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="static_map2odom_tf" args="--x -0.0 --y -0.0 --z 0.0 --qx 0.0 --qy 0.0 --qz 0.0 --qw 1.0 --frame-id map --child-frame-id odom" />
+
     <!-- translate joint goals to joint states -->
     <node name="motor_goals_viz_helper" pkg="bitbots_utils" exec="motor_goals_viz_helper.py"/>
 
@@ -41,5 +40,4 @@
     <include file="$(find-pkg-share humanoid_league_team_communication)/launch/team_comm_test_marker.launch">
         <arg name="rviz" value="false" />
     </include>
-    <!-- <node name="show_world_model_objects" pkg="bitbots_body_behavior" exec="show_world_model_objects.py" output="screen"/> -->
 </launch>
diff --git a/bitbots_bringup/package.xml b/bitbots_bringup/package.xml
index 9fa1d747..bb4e99a6 100644
--- a/bitbots_bringup/package.xml
+++ b/bitbots_bringup/package.xml
@@ -14,6 +14,8 @@
 
     <depend>bitbots_docs</depend>
 
+    <exec_depend condition="$IS_ROBOT != True">wolfgang_webots_sim</exec_depend>
+    <exec_depend>audio_common</exec_depend>
     <exec_depend>bitbots_basler_camera</exec_depend>
     <exec_depend>bitbots_body_behavior</exec_depend>
     <exec_depend>bitbots_dynamic_kick</exec_depend>
@@ -21,14 +23,18 @@
     <exec_depend>bitbots_hcm</exec_depend>
     <exec_depend>bitbots_head_mover</exec_depend>
     <exec_depend>bitbots_localization</exec_depend>
+    <exec_depend>bitbots_odometry</exec_depend>
     <exec_depend>bitbots_quintic_walk</exec_depend>
+    <exec_depend>bitbots_robot_description</exec_depend>
     <exec_depend>bitbots_ros_control</exec_depend>
     <exec_depend>bitbots_utils</exec_depend>
     <exec_depend>bitbots_vision</exec_depend>
-    <exec_depend>robot_state_publisher</exec_depend>
-    <exec_depend condition="$IS_ROBOT != True">wolfgang_webots_sim</exec_depend>
+    <exec_depend>humanoid_base_footprint</exec_depend>
     <exec_depend>soccer_ipm</exec_depend>
-    <exec_depend>xacro</exec_depend>
+    <exec_depend>system_monitor</exec_depend>
+    <exec_depend>udp_bridge</exec_depend>
+    <exec_depend>wolfgang_description</exec_depend>
+    <exec_depend>wolfgang_moveit_config</exec_depend>
 
     <export>
         <bitbots_documentation>
diff --git a/bitbots_bringup/repair.sh b/bitbots_bringup/repair.sh
deleted file mode 100755
index 91b551ff..00000000
--- a/bitbots_bringup/repair.sh
+++ /dev/null
@@ -1,15 +0,0 @@
-touch devel/lib/python2.7/dist-packages/bitbots_cm730/lowlevel/controller/__init__.py
-
-touch devel/lib/python2.7/dist-packages/bitbots_cm730/lowlevel/__init__.py
-
-touch devel/lib/python2.7/dist-packages/bitbots_cm730/__init__.py
-
-touch devel/lib/python2.7/dist-packages/bitbots_walking/__init__.py
-
-touch devel/lib/python2.7/dist-packages/bitbots_bringup/pose/__init__.py
-
-touch devel/lib/python2.7/dist-packages/bitbots_bringup/utilCython/__init__.py
-
-cp src/bitbots_misc/humanoid_league_speaker/src/humanoid_league_speaker/cfg/speaker_paramsConfig.py* ./devel/lib/python2.7/dist-packages/humanoid_league_speaker/cfg
-
-cp src/bitbots_lowlevel/bitbots_hcm/src/bitbots_hcm/cfg/motion_paramsConfig.py* ./devel/lib/python2.7/dist-packages/bitbots_hcm/cfg
diff --git a/bitbots_ceiling_cam/.rdmanifest b/bitbots_ceiling_cam/.rdmanifest
deleted file mode 100644
index 059279fc..00000000
--- a/bitbots_ceiling_cam/.rdmanifest
+++ /dev/null
@@ -1,11 +0,0 @@
----
-# See http://doku.bit-bots.de/meta/manual/software/ci.html#make-package-resolvable-in-ci
-check-presence-script: '#!/bin/bash
-
-  test -d $BITBOTS_CATKIN_WORKSPACE/src/bitbots_ceiling_cam'
-depends: null
-exec-path: bitbots_misc-master/bitbots_ceiling_cam
-install-script: '#!/bin/bash
-
-  cp -r . $BITBOTS_CATKIN_WORKSPACE/src/bitbots_ceiling_cam'
-uri: https://github.com/bit-bots/bitbots_misc/archive/refs/heads/master.tar.gz
diff --git a/bitbots_ceiling_cam/launch/ceiling_cam.launch b/bitbots_ceiling_cam/launch/ceiling_cam.launch
index 3c0ae281..1a3ef673 100644
--- a/bitbots_ceiling_cam/launch/ceiling_cam.launch
+++ b/bitbots_ceiling_cam/launch/ceiling_cam.launch
@@ -20,13 +20,11 @@
             <remap from="camera_info" to="/ceiling_cam/ceiling_cam_publisher/camera_info"/>
             <remap from="image_rect" to="/ceiling_cam/ceiling_cam_publisher/image_color_rect"/>
         </composable_node>
-        <!--
         <composable_node pkg="apriltag_ros" plugin="AprilTagNode" name="apriltag">
             <remap from="/apriltag/image" to="/ceiling_cam/ceiling_cam_publisher/image_color_rect"/>
             <remap from="/apriltag/camera_info" to="/ceiling_cam/ceiling_cam_publisher/camera_info"/>
             <param from="$(find-pkg-share bitbots_ceiling_cam)/config/april_tags.yaml"/>
         </composable_node>
-        -->
     </node_container>
 
 
diff --git a/bitbots_convenience_frames/.rdmanifest b/bitbots_convenience_frames/.rdmanifest
deleted file mode 100644
index 812eeb55..00000000
--- a/bitbots_convenience_frames/.rdmanifest
+++ /dev/null
@@ -1,20 +0,0 @@
----
-# See http://doku.bit-bots.de/meta/manual/software/ci.html#make-package-resolvable-in-ci
-check-presence-script: '#!/bin/bash
-
-  test -d $BITBOTS_CATKIN_WORKSPACE/src/bitbots_convenience_frames'
-depends:
-- bitbots_docs
-- geometry_msgs
-- humanoid_league_msgs
-- roscpp
-- rospy
-- std_msgs
-- tf2
-- tf2_geometry_msgs
-- tf2_ros
-exec-path: bitbots_misc-master/bitbots_convenience_frames
-install-script: '#!/bin/bash
-
-  cp -r . $BITBOTS_CATKIN_WORKSPACE/src/bitbots_convenience_frames'
-uri: https://github.com/bit-bots/bitbots_misc/archive/refs/heads/master.tar.gz
diff --git a/bitbots_convenience_frames/CMakeLists.txt b/bitbots_convenience_frames/CMakeLists.txt
deleted file mode 100644
index 4b694b82..00000000
--- a/bitbots_convenience_frames/CMakeLists.txt
+++ /dev/null
@@ -1,47 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(bitbots_convenience_frames)
-
-# Add support for C++17
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
-endif()
-
-find_package(geometry_msgs REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(ament_cmake REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(biped_interfaces REQUIRED)
-find_package(humanoid_league_msgs REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-find_package(bitbots_docs REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(rclpy REQUIRED)
-
-include_directories(include)
-
-add_compile_options(-Wall -Werror -Wno-unused)
-
-add_executable(convenience_frames src/convenience_frames.cpp)
-
-ament_target_dependencies(convenience_frames
-  ament_cmake
-  bitbots_docs
-  biped_interfaces
-  humanoid_league_msgs
-  rclcpp
-  std_msgs
-  tf2
-  tf2_geometry_msgs
-  tf2_ros
-)
-
-enable_bitbots_docs()
-
-install(TARGETS convenience_frames
-        DESTINATION lib/${PROJECT_NAME})
-
-install(DIRECTORY launch
-        DESTINATION share/${PROJECT_NAME})
-
-ament_package()
diff --git a/bitbots_convenience_frames/include/bitbots_convenience_frames/convenience_frames.h b/bitbots_convenience_frames/include/bitbots_convenience_frames/convenience_frames.h
deleted file mode 100644
index 0fe4042b..00000000
--- a/bitbots_convenience_frames/include/bitbots_convenience_frames/convenience_frames.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#include <rclcpp/rclcpp.hpp>
-#include <std_msgs/msg/char.hpp>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#include <tf2_ros/buffer.h>
-#include <tf2/utils.h>
-#include <humanoid_league_msgs/msg/pose_with_certainty.hpp>
-#include <humanoid_league_msgs/msg/pose_with_certainty_array.hpp>
-#include <biped_interfaces/msg/phase.hpp>
-
-#include <utility>
-using std::placeholders::_1;
-
-class ConvenienceFramesBroadcaster : public rclcpp::Node {
- public:
-  ConvenienceFramesBroadcaster();
-  void loop();
- private:
-  std::unique_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
-  geometry_msgs::msg::TransformStamped tf_{geometry_msgs::msg::TransformStamped()};
-  std::unique_ptr<tf2_ros::Buffer>
-      tfBuffer_{std::make_unique<tf2_ros::Buffer>(this->get_clock())};
-  tf2_ros::TransformListener tfListener_{*tfBuffer_, this};
-  std::string base_link_frame_, r_sole_frame_, l_sole_frame_, r_toe_frame_, l_toe_frame_, approach_frame_,
-      ball_frame_, right_post_frame_, left_post_frame_, general_post_frame_;
-
-  bool is_left_support{false};
-  bool got_support_foot_{false};
-  void publishTransform(std::string header_frame_id, std::string child_frame_id, double x, double y, double z);
-  void supportFootCallback(const biped_interfaces::msg::Phase::SharedPtr msg);
-  void ballsCallback(const humanoid_league_msgs::msg::PoseWithCertaintyArray::SharedPtr msg);
-  void goalCallback(const humanoid_league_msgs::msg::PoseWithCertaintyArray::SharedPtr msg);
-  void goalPostsCallback(const humanoid_league_msgs::msg::PoseWithCertaintyArray::SharedPtr msg);
-};
diff --git a/bitbots_convenience_frames/launch/convenience_frames.launch b/bitbots_convenience_frames/launch/convenience_frames.launch
deleted file mode 100644
index 34864162..00000000
--- a/bitbots_convenience_frames/launch/convenience_frames.launch
+++ /dev/null
@@ -1,18 +0,0 @@
-<launch>
-    <arg name="sim" default="false"/>
-    <arg name="tf_prefix" default="$(eval '\'$(env ROS_NAMESPACE /)\'')"/>
-
-    <node pkg="bitbots_convenience_frames" exec="convenience_frames" name="convenience_frames">
-        <param name="use_sim_time" value="$(var sim)"/>
-        <param name="base_link_frame" value="$(var tf_prefix)base_link"/>
-        <param name="r_sole_frame" value="$(var tf_prefix)r_sole"/>
-        <param name="l_sole_frame" value="$(var tf_prefix)l_sole"/>
-        <param name="r_toe_frame" value="$(var tf_prefix)r_toe"/>
-        <param name="l_toe_frame" value="$(var tf_prefix)l_toe"/>
-        <param name="approach_frame" value="$(var tf_prefix)approach_frame"/>
-        <param name="ball_frame" value="$(var tf_prefix)ball"/>
-        <param name="right_post_frame" value="$(var tf_prefix)right_post"/>
-        <param name="left_post_frame" value="$(var tf_prefix)left_post"/>
-        <param name="general_post_frame" value="$(var tf_prefix)post_"/>
-    </node>
-</launch>
\ No newline at end of file
diff --git a/bitbots_convenience_frames/package.xml b/bitbots_convenience_frames/package.xml
deleted file mode 100644
index a3286c54..00000000
--- a/bitbots_convenience_frames/package.xml
+++ /dev/null
@@ -1,33 +0,0 @@
-<?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>bitbots_convenience_frames</name>
-    <version>2.1.0</version>
-    <description>Publishes convenience frames for the Hamburg-BitBots code base.</description>
-
-    <maintainer email="bestmann@informatik.uni-hamburg.de">Marc Bestmann</maintainer>
-    <maintainer email="info@bit-bots.de">Hamburg Bit-Bots</maintainer>
-    <author email="bestmann@informatik.uni-hamburg.de">Marc Bestmann</author>
-
-    <license>MIT</license>
-
-    <buildtool_depend>ament_cmake</buildtool_depend>
-
-    <depend>bitbots_docs</depend>
-    <depend>biped_interfaces</depend>
-    <depend>std_msgs</depend>
-    <depend>tf2</depend>
-    <depend>tf2_geometry_msgs</depend>
-    <depend>tf2_ros</depend>
-    <depend>geometry_msgs</depend>
-    <depend>humanoid_league_msgs</depend>
-
-    <export>
-        <bitbots_documentation>
-            <language>c++</language>
-            <status>tested_robot</status>
-        </bitbots_documentation>
-        <rosdoc config="rosdoc.yaml"/>
-        <build_type>ament_cmake</build_type>
-    </export>
-</package>
diff --git a/bitbots_convenience_frames/src/convenience_frames.cpp b/bitbots_convenience_frames/src/convenience_frames.cpp
deleted file mode 100644
index 1f739d52..00000000
--- a/bitbots_convenience_frames/src/convenience_frames.cpp
+++ /dev/null
@@ -1,256 +0,0 @@
-#include<bitbots_convenience_frames/convenience_frames.h>
-
-ConvenienceFramesBroadcaster::ConvenienceFramesBroadcaster() : Node("convenience_frames") {
-
-  this->declare_parameter<std::string>("base_link_frame", "base_link");
-  this->get_parameter("base_link_frame", base_link_frame_);
-  this->declare_parameter<std::string>("r_sole_frame", "r_sole");
-  this->get_parameter("r_sole_frame", r_sole_frame_);
-  this->declare_parameter<std::string>("l_sole_frame", "l_sole");
-  this->get_parameter("l_sole_frame", l_sole_frame_);
-  this->declare_parameter<std::string>("r_toe_frame", "r_toe");
-  this->get_parameter("r_toe_frame", r_toe_frame_);
-  this->declare_parameter<std::string>("l_toe_frame", "l_toe");
-  this->get_parameter("l_toe_frame", l_toe_frame_);
-  this->declare_parameter<std::string>("approach_frame", "approach_frame");
-  this->get_parameter("approach_frame", approach_frame_);
-  this->declare_parameter<std::string>("ball_frame", "ball");
-  this->get_parameter("ball_frame", ball_frame_);
-  this->declare_parameter<std::string>("right_post_frame", "right_post");
-  this->get_parameter("right_post_frame", right_post_frame_);
-  this->declare_parameter<std::string>("left_post_frame", "left_post");
-  this->get_parameter("left_post_frame", left_post_frame_);
-  this->declare_parameter<std::string>("general_post_frame", "post_");
-  this->get_parameter("general_post_frame", general_post_frame_);
-
-  got_support_foot_ = false;
-  rclcpp::Subscription<biped_interfaces::msg::Phase>::SharedPtr walking_support_foot_subscriber =
-      this->create_subscription<biped_interfaces::msg::Phase>("walk_support_state",
-                                                              1,
-                                                              std::bind(&ConvenienceFramesBroadcaster::supportFootCallback,
-                                                                        this, _1));
-  rclcpp::Subscription<biped_interfaces::msg::Phase>::SharedPtr dynamic_kick_support_foot_subscriber =
-      this->create_subscription<biped_interfaces::msg::Phase>("dynamic_kick_support_state",
-                                                              1,
-                                                              std::bind(&ConvenienceFramesBroadcaster::supportFootCallback,
-                                                                        this, _1));
-  rclcpp::Subscription<humanoid_league_msgs::msg::PoseWithCertaintyArray>::SharedPtr ball_relative_subscriber =
-      this->create_subscription<humanoid_league_msgs::msg::PoseWithCertaintyArray>("balls_relative",
-                                                                                   1,
-                                                                                   std::bind(&ConvenienceFramesBroadcaster::ballsCallback,
-                                                                                             this, _1));
-  rclcpp::Subscription<humanoid_league_msgs::msg::PoseWithCertaintyArray>::SharedPtr goal_relative_subscriber =
-      this->create_subscription<humanoid_league_msgs::msg::PoseWithCertaintyArray>("goal_relative",
-                                                                                   1,
-                                                                                   std::bind(&ConvenienceFramesBroadcaster::goalCallback,
-                                                                                             this, _1));
-  rclcpp::Subscription<humanoid_league_msgs::msg::PoseWithCertaintyArray>::SharedPtr goal_posts_relative_subscriber =
-      this->create_subscription<humanoid_league_msgs::msg::PoseWithCertaintyArray>("goal_posts_relative",
-                                                                                   1,
-                                                                                   std::bind(&ConvenienceFramesBroadcaster::goalPostsCallback,
-                                                                                             this, _1));
-}
-void ConvenienceFramesBroadcaster::loop() {
-  rclcpp::Time last_published_time;
-  auto node_pointer = this->shared_from_this();
-  while (rclcpp::ok()) {
-    rclcpp::Time startTime = this->get_clock()->now();
-
-    rclcpp::spin_some(node_pointer);
-
-    geometry_msgs::msg::TransformStamped tf_right, // right foot in baselink frame
-    tf_left, tf_right_toe, // right toes baselink frame
-    tf_left_toe, support_foot, // support foot in baselink frame
-    non_support_foot, non_support_foot_in_support_foot_frame, base_footprint_in_support_foot_frame,
-        front_foot; // foot that is currently in front of the other, in baselink frame
-
-    try {
-      // first check if transforms are possible to prevent error spam
-      if (tfBuffer_->_frameExists(r_sole_frame_) && tfBuffer_->_frameExists(l_sole_frame_) &&
-          tfBuffer_->_frameExists(r_toe_frame_) && tfBuffer_->_frameExists(l_toe_frame_) &&
-          tfBuffer_->canTransform(base_link_frame_, r_sole_frame_, this->now()) &&
-          tfBuffer_->canTransform(base_link_frame_, l_sole_frame_, this->now()) &&
-          tfBuffer_->canTransform(base_link_frame_, r_toe_frame_, this->now()) &&
-          tfBuffer_->canTransform(base_link_frame_, l_toe_frame_, this->now())) {
-
-        tf_right = tfBuffer_->lookupTransform(base_link_frame_,
-                                              r_sole_frame_,
-                                              this->now(),
-                                              rclcpp::Duration::from_nanoseconds(1e9 * 0.1));
-        tf_left = tfBuffer_->lookupTransform(base_link_frame_,
-                                             l_sole_frame_,
-                                             this->now(),
-                                             rclcpp::Duration::from_nanoseconds(1e9 * 0.1));
-        tf_right_toe = tfBuffer_->lookupTransform(base_link_frame_,
-                                                  r_toe_frame_,
-                                                  this->now(),
-                                                  rclcpp::Duration::from_nanoseconds(1e9 * 0.1));
-        tf_left_toe = tfBuffer_->lookupTransform(base_link_frame_,
-                                                 l_toe_frame_,
-                                                 this->now(),
-                                                 rclcpp::Duration::from_nanoseconds(1e9 * 0.1));
-
-        // compute support foot
-        if (got_support_foot_) {
-          if (is_left_support) {
-            support_foot = tf_left;
-            non_support_foot = tf_right;
-          } else {
-            support_foot = tf_right;
-            non_support_foot = tf_left;
-          }
-        } else {
-          // check which foot is support foot (which foot is on the ground)
-          if (tf_right.transform.translation.z < tf_left.transform.translation.z) {
-            support_foot = tf_right;
-            non_support_foot = tf_left;
-          } else {
-            support_foot = tf_left;
-            non_support_foot = tf_right;
-          }
-        }
-
-        // check with foot is in front
-        if (tf_right.transform.translation.x < tf_left.transform.translation.x) {
-          front_foot = tf_left_toe;
-        } else {
-          front_foot = tf_right_toe;
-        }
-
-        // get the position of the non support foot in the support frame, used for computing the barycenter
-        non_support_foot_in_support_foot_frame = tfBuffer_->lookupTransform(support_foot.child_frame_id,
-                                                                            non_support_foot.child_frame_id,
-                                                                            support_foot.header.stamp,
-                                                                            rclcpp::Duration::from_nanoseconds(
-                                                                                1e9 * 0.1));
-
-        geometry_msgs::msg::TransformStamped
-            support_to_base_link = tfBuffer_->lookupTransform(support_foot.header.frame_id,
-                                                              support_foot.child_frame_id,
-                                                              support_foot.header.stamp);
-
-        geometry_msgs::msg::PoseStamped approach_frame;
-        // x at front foot toes
-        approach_frame.pose.position.x = front_foot.transform.translation.x;
-        // y between feet
-        tf2::Transform center_between_foot;
-        double y = non_support_foot_in_support_foot_frame.transform.translation.y / 2;
-        center_between_foot.setOrigin({0.0, y, 0.0});
-        center_between_foot.setRotation({0, 0, 0, 1});
-        tf2::Transform support_foot_tf;
-        tf2::fromMsg(support_foot.transform, support_foot_tf);
-        center_between_foot = support_foot_tf * center_between_foot;
-        approach_frame.pose.position.y = center_between_foot.getOrigin().y();
-        // z at ground leven (support foot height)
-        approach_frame.pose.position.z = support_foot.transform.translation.z;
-
-        // roll and pitch of support foot
-        double roll, pitch, yaw;
-        tf2::Quaternion quat;
-        fromMsg(support_foot.transform.rotation, quat);
-        tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);
-        // yaw of front foot
-        yaw = tf2::getYaw(front_foot.transform.rotation);
-
-        // pitch and roll from support foot, yaw from base link
-        tf2::Quaternion rotation;
-        rotation.setRPY(roll, pitch, yaw);
-        approach_frame.pose.orientation = tf2::toMsg(rotation);
-
-        // in simulation, the time does not always advance between loop iteration
-        // in that case, we do not want to republish the transform
-        rclcpp::Time now = this->now();
-        if (now != last_published_time) {
-          last_published_time = now;
-
-          // set the broadcasted transform to the position and orientation of the base footprint
-          tf_.header.stamp = now;
-          tf_.header.frame_id = base_link_frame_;
-          tf_.child_frame_id = approach_frame_;
-          tf_.transform.translation.x = approach_frame.pose.position.x;
-          tf_.transform.translation.y = approach_frame.pose.position.y;
-          tf_.transform.translation.z = approach_frame.pose.position.z;
-          tf_.transform.rotation = approach_frame.pose.orientation;
-          broadcaster_->sendTransform(tf_);
-        } else {
-          RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
-                               "Missing frames for approach frame, not publishing transform");
-        }
-      }
-    } catch (...) {
-      // sleep for a bit to avoid spamming the console
-      rclcpp::sleep_for(std::chrono::milliseconds(1000));
-      continue;
-    }
-    this->get_clock()->sleep_until(
-        startTime + rclcpp::Duration::from_nanoseconds(1e9 / 200));
-  }
-}
-
-void ConvenienceFramesBroadcaster::supportFootCallback(const biped_interfaces::msg::Phase::SharedPtr msg) {
-  got_support_foot_ = true;
-  is_left_support = (msg->phase == biped_interfaces::msg::Phase::LEFT_STANCE);
-}
-
-void ConvenienceFramesBroadcaster::ballsCallback(const humanoid_league_msgs::msg::PoseWithCertaintyArray::SharedPtr msg) {
-  for (humanoid_league_msgs::msg::PoseWithCertainty ball: msg->poses) {
-    publishTransform(msg->header.frame_id, ball_frame_,
-                     ball.pose.pose.position.x,
-                     ball.pose.pose.position.y,
-                     ball.pose.pose.position.z);
-  }
-}
-
-void ConvenienceFramesBroadcaster::goalCallback(const humanoid_league_msgs::msg::PoseWithCertaintyArray::SharedPtr msg) {
-  if (msg->poses.size() > 0) {
-    publishTransform(msg->header.frame_id,
-                     left_post_frame_,
-                     msg->poses[0].pose.pose.position.x,
-                     msg->poses[0].pose.pose.position.y,
-                     msg->poses[0].pose.pose.position.z);
-  }
-  if (msg->poses.size() > 2) {
-    publishTransform(msg->header.frame_id,
-                     right_post_frame_,
-                     msg->poses[1].pose.pose.position.x,
-                     msg->poses[1].pose.pose.position.y,
-                     msg->poses[1].pose.pose.position.z);
-  }
-}
-
-void ConvenienceFramesBroadcaster::goalPostsCallback(const humanoid_league_msgs::msg::PoseWithCertaintyArray::SharedPtr msg) {
-  for (size_t i = 0; i < msg->poses.size(); i++) {
-    publishTransform(msg->header.frame_id,
-                     general_post_frame_ + std::to_string(i),
-                     msg->poses[i].pose.pose.position.x,
-                     msg->poses[i].pose.pose.position.y,
-                     msg->poses[i].pose.pose.position.z);
-  }
-}
-
-void ConvenienceFramesBroadcaster::publishTransform(std::string header_frame_id, std::string child_frame_id,
-                                                    double x, double y, double z) {
-  geometry_msgs::msg::TransformStamped transform;
-  transform.header.stamp = this->now();
-  transform.header.frame_id = std::move(header_frame_id);
-  transform.child_frame_id = std::move(child_frame_id);
-  transform.transform.translation.x = x;
-  transform.transform.translation.y = y;
-  transform.transform.translation.z = z;
-  transform.transform.rotation.x = 0;
-  transform.transform.rotation.y = 0;
-  transform.transform.rotation.z = 0;
-  transform.transform.rotation.w = 1;
-  broadcaster_->sendTransform(transform);
-}
-
-int main(int argc, char **argv) {
-  rclcpp::init(argc, argv);
-  auto node = std::make_shared<ConvenienceFramesBroadcaster>();
-  // wait till connection with publishers has been established
-  // so we do not immediately blast something into the log output
-  rclcpp::sleep_for(std::chrono::milliseconds(500));
-  node->loop();
-  rclcpp::shutdown();
-}
-
diff --git a/bitbots_robot_description/CMakeLists.txt b/bitbots_robot_description/CMakeLists.txt
new file mode 100644
index 00000000..c192f1a3
--- /dev/null
+++ b/bitbots_robot_description/CMakeLists.txt
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.5)
+project(bitbots_robot_description)
+
+find_package(bitbots_docs REQUIRED)
+find_package(ament_cmake REQUIRED)
+
+
+set(INCLUDE_DIRS include)
+include_directories(${INCLUDE_DIRS})
+
+set(CMAKE_CXX_STANDARD 17)
+add_compile_options(-Wall -Werror -Wno-unused -pedantic -Wextra)
+
+enable_bitbots_docs()
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()
diff --git a/bitbots_convenience_frames/docs/_static/logo.png b/bitbots_robot_description/docs/_static/logo.png
similarity index 100%
rename from bitbots_convenience_frames/docs/_static/logo.png
rename to bitbots_robot_description/docs/_static/logo.png
diff --git a/bitbots_convenience_frames/docs/conf.py b/bitbots_robot_description/docs/conf.py
similarity index 100%
rename from bitbots_convenience_frames/docs/conf.py
rename to bitbots_robot_description/docs/conf.py
diff --git a/bitbots_convenience_frames/docs/index.rst b/bitbots_robot_description/docs/index.rst
similarity index 100%
rename from bitbots_convenience_frames/docs/index.rst
rename to bitbots_robot_description/docs/index.rst
diff --git a/bitbots_robot_description/launch/load_robot_description.launch b/bitbots_robot_description/launch/load_robot_description.launch
new file mode 100644
index 00000000..0bffcf86
--- /dev/null
+++ b/bitbots_robot_description/launch/load_robot_description.launch
@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="sim" default="false"/>
+    <arg name="robot_type" default="wolfgang" description="what kind of robot should be started" />
+
+    <include file="$(find-pkg-share bitbots_robot_description)/launch/move_group.py">
+      <arg name="sim" value="$(var sim)"/>
+      <arg name="robot_type" value="$(var robot_type)"/>
+    </include>
+
+    <include file="$(find-pkg-share bitbots_extrinsic_calibration)/launch/calibration.launch">
+      <arg name="sim" value="$(var sim)"/>
+    </include>
+</launch>
diff --git a/bitbots_utils/launch/move_group.py b/bitbots_robot_description/launch/move_group.py
similarity index 83%
rename from bitbots_utils/launch/move_group.py
rename to bitbots_robot_description/launch/move_group.py
index 55f32b98..995a677e 100644
--- a/bitbots_utils/launch/move_group.py
+++ b/bitbots_robot_description/launch/move_group.py
@@ -4,7 +4,6 @@
 from ament_index_python import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.utilities import normalize_to_list_of_substitutions
 from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
 from launch.substitutions import Command, TextSubstitution, LaunchConfiguration
@@ -71,6 +70,8 @@ def launch_setup(context, *args, **kwargs):
         "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
     }
 
+    sensor_config = load_yaml(f"{robot_type.perform(context)}_moveit_config", "config/sensors_3d.yaml")
+
     trajectory_execution = {
         "moveit_manage_controllers": True,
         "trajectory_execution.allowed_execution_duration_scaling": 1.2,
@@ -100,12 +101,12 @@ def launch_setup(context, *args, **kwargs):
         print("### WARNING: trajectory_execution is None")
     if planning_scene_monitor_parameters is None:
         print("### WARNING: planning_scene_monitor_parameters is None")
-    
+
 
     rsp_node = Node(package='robot_state_publisher',
                     executable='robot_state_publisher',
                     respawn=True,
-                    # output='screen',
+                    output='screen',
                     parameters=[{
                         'robot_description': robot_description,
                         'publish_frequency': 100.0,
@@ -114,23 +115,28 @@ def launch_setup(context, *args, **kwargs):
                     arguments=['--ros-args', '--log-level', 'WARN']
                     )
 
-    move_group_node = Node(package='moveit_ros_move_group',
-                           executable='move_group',
-                           # output='screen',
-                           # hacky merging dicts
-                           parameters=[{
-                               'robot_description': robot_description,
-                               'robot_description_semantic': robot_description_semantic_config,
-                               'robot_description_kinematics': kinematics_yaml,
-                               'publish_robot_description_semantic': True,
-                               'use_sim_time': sim
-                           },
-                               ompl_planning_pipeline_config,
-                               trajectory_execution,
-                               moveit_controllers,
-                               planning_scene_monitor_parameters, ],
-                           arguments=['--ros-args', '--log-level', 'WARN']
-                           )  # todo joint limits
+    move_group_node = Node(
+        package='moveit_ros_move_group',
+        executable='move_group',
+        output='screen',
+        # hacky merging dicts
+        parameters=[
+            {   
+                'robot_description': robot_description,
+                'robot_description_semantic': robot_description_semantic_config,
+                'robot_description_kinematics': kinematics_yaml,
+                'publish_robot_description_semantic': True,
+                'use_sim_time': sim,
+                'octomap_resolution': 0.01,
+            },
+            ompl_planning_pipeline_config,
+            trajectory_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+            sensor_config
+        ],
+        arguments=['--ros-args', '--log-level', 'WARN']
+        )  # todo joint limits
     return [move_group_node, rsp_node]
 
 
diff --git a/bitbots_robot_description/package.xml b/bitbots_robot_description/package.xml
new file mode 100644
index 00000000..282b71c8
--- /dev/null
+++ b/bitbots_robot_description/package.xml
@@ -0,0 +1,30 @@
+<?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>bitbots_robot_description</name>
+    <version>2.3.1</version>
+    <description>
+        The bitbots_robot_description package includes utilities to launch the robot description,
+        robot state publisher and the robot calibration.
+    </description>
+
+    <maintainer email="git@flova.de">Florian Vahl</maintainer>
+    <maintainer email="info@bit-bots.de">Hamburg Bit-Bots</maintainer>
+    <author email="info@bit-bots.de">Hamburg Bit-Bots</author>
+    <license>MIT</license>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>
+
+    <depend>bitbots_docs</depend>
+    <depend>bitbots_extrinsic_calibration</depend>
+    <depend>moveit_ros_move_group</depend>
+    <depend>robot_state_publisher</depend>
+    <depend>xacro</depend>
+
+    <export>
+        <bitbots_documentation>
+            <status>unknown</status>
+        </bitbots_documentation>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>
diff --git a/bitbots_teleop/.rdmanifest b/bitbots_teleop/.rdmanifest
deleted file mode 100644
index b097aae6..00000000
--- a/bitbots_teleop/.rdmanifest
+++ /dev/null
@@ -1,15 +0,0 @@
----
-# See http://doku.bit-bots.de/meta/manual/software/ci.html#make-package-resolvable-in-ci
-check-presence-script: '#!/bin/bash
-
-  test -d $BITBOTS_CATKIN_WORKSPACE/src/bitbots_teleop'
-depends:
-- bitbots_docs
-- humanoid_league_msgs
-- rospy
-- std_msgs
-exec-path: bitbots_misc-master/bitbots_teleop
-install-script: '#!/bin/bash
-
-  cp -r . $BITBOTS_CATKIN_WORKSPACE/src/bitbots_teleop'
-uri: https://github.com/bit-bots/bitbots_misc/archive/refs/heads/master.tar.gz
diff --git a/bitbots_teleop/CMakeLists.txt b/bitbots_teleop/CMakeLists.txt
index 5a763820..acd4bf9f 100644
--- a/bitbots_teleop/CMakeLists.txt
+++ b/bitbots_teleop/CMakeLists.txt
@@ -1,10 +1,10 @@
 cmake_minimum_required(VERSION 3.5)
 project(bitbots_teleop)
 
+find_package(ament_cmake REQUIRED)
 find_package(bitbots_docs REQUIRED)
+find_package(bitbots_msgs REQUIRED)
 find_package(rclpy REQUIRED)
-find_package(ament_cmake REQUIRED)
-find_package(humanoid_league_msgs REQUIRED)
 
 enable_bitbots_docs()
 
diff --git a/bitbots_teleop/bitbots_teleop/joy_node.py b/bitbots_teleop/bitbots_teleop/joy_node.py
index 41b913e4..1f7c768d 100755
--- a/bitbots_teleop/bitbots_teleop/joy_node.py
+++ b/bitbots_teleop/bitbots_teleop/joy_node.py
@@ -3,19 +3,16 @@
 import threading
 
 import rclpy
-from humanoid_league_msgs.action import PlayAnimation
 from rclpy.duration import Duration
 from rclpy.node import Node
 from rclpy.action import ActionClient
 import copy
 
-from pathlib import Path
-from sensor_msgs.msg import Joy
+from bitbots_msgs.action import PlayAnimation
+from bitbots_msgs.msg import Audio
+from bitbots_msgs.msg import JointCommand, HeadMode
 from geometry_msgs.msg import Twist
-from humanoid_league_msgs.msg import Audio, HeadMode
-from bitbots_msgs.msg import JointCommand
-
-#from bitbots_animation_server.action import PlayAnimationAction
+from sensor_msgs.msg import Joy
 
 
 class JoyNode(Node):
@@ -145,13 +142,13 @@ def set_head_mode(self, mode):
         msg.head_mode = mode
         self.head_mode_pub.publish(msg)
 
-    def denormalize_joy(self, gain, axis, msg, deadzone=0.0):
+    def denormalize_joy(self, gain, axis, msg: Joy, deadzone=0.0):
         if abs(msg.axes[axis]) > deadzone:
             return gain * msg.axes[axis]
         else:
             return 0
 
-    def joy_cb(self, msg):
+    def joy_cb(self, msg: Joy):
         # forward and sideward walking with left joystick
         self.walk_msg.linear.x = float(self.denormalize_joy(
             self.config['walking']['gain_x'],
@@ -220,7 +217,6 @@ def main():
     rclpy.init(args=None)
     node = JoyNode()
     rclpy.spin(node)
-
     node.destroy_node()
     rclpy.shutdown()
 
diff --git a/bitbots_teleop/package.xml b/bitbots_teleop/package.xml
index 90425513..7c5db8eb 100644
--- a/bitbots_teleop/package.xml
+++ b/bitbots_teleop/package.xml
@@ -12,14 +12,13 @@
   <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <depend>rclpy</depend>
-  <depend>humanoid_league_msgs</depend>
-  <depend>bitbots_msgs</depend>
+
   <depend>bitbots_docs</depend>
+  <depend>bitbots_msgs</depend>
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
   <depend>tf_transformations</depend>
 
-  <exec_depend>std_msgs</exec_depend>
-
   <export>
     <bitbots_documentation>
       <language>python2</language>
diff --git a/bitbots_teleop/scripts/teleop_keyboard.py b/bitbots_teleop/scripts/teleop_keyboard.py
index f9b061e6..51efd27f 100755
--- a/bitbots_teleop/scripts/teleop_keyboard.py
+++ b/bitbots_teleop/scripts/teleop_keyboard.py
@@ -2,7 +2,6 @@
 
 # This script was based on the teleop_twist_keyboard package
 # original code can be found at https://github.com/ros-teleop/teleop_twist_keyboard
-import math
 import os
 import threading
 
@@ -12,100 +11,16 @@
 from geometry_msgs.msg import Twist
 from sensor_msgs.msg import JointState
 from bitbots_msgs.msg import JointCommand
+from bitbots_utils.transforms import quat_from_yaw
 
 import sys, select, termios, tty
 from rclpy.action import ActionClient
-from bitbots_msgs.action import Kick
-from geometry_msgs.msg import Vector3, Quaternion
+from bitbots_msgs.action import Kick, Dynup
+from geometry_msgs.msg import Point, Vector3
 from std_msgs.msg import Bool
 from std_srvs.srv import Empty
-from tf_transformations import quaternion_from_euler
-import argparse
-
-# the following encodes walkready poses for various robots. could be done more nicely via some config
-
-__walkready_joints__ = {
-    "wolfgang":
-        [("HeadPan", 0.0),
-         ("HeadTilt", 0.0),
-         ("LShoulderPitch", math.radians(75.27)),
-         ("LShoulderRoll", 0.0),
-         ("LElbow", math.radians(35.86)),
-         ("RShoulderPitch", math.radians(-75.58)),
-         ("RShoulderRoll", 0.0),
-         ("RElbow", math.radians(-36.10)),
-         ("LHipYaw", -0.0112),
-         ("LHipRoll", 0.0615),
-         ("LHipPitch", 0.4732),
-         ("LKnee", 1.0058),
-         ("LAnklePitch", -0.4512),
-         ("LAnkleRoll", 0.0625),
-         ("RHipYaw", 0.0112),
-         ("RHipRoll", -0.0615),
-         ("RHipPitch", -0.4732),
-         ("RKnee", -1.0059),
-         ("RAnklePitch", 0.4512),
-         ("RAnkleRoll", -0.0625)],
-    "robotis_op2":
-        [("LElbow", math.radians(-60)),
-         ("RElbow", math.radians(60)),
-         ("LShoulderPitch", math.radians(120.0)),
-         ("RShoulderPitch", math.radians(-120.0))],
-    "op3":
-        [("l_el", math.radians(-140)),
-         ("r_el", math.radians(140)),
-         ("l_sho_pitch", math.radians(-135)),
-         ("r_sho_pitch", math.radians(135)),
-         ("l_sho_roll", math.radians(-90)),
-         ("r_sho_roll", math.radians(90))],
-    "nao":
-        [("LShoulderPitch", 1.57),
-         ("RShoulderPitch", 1.57),
-         ('LShoulderRoll', 0.3),
-         ('RShoulderRoll', -0.3)],
-    "rfc":
-        [("LeftElbow", math.radians(-90.0)),
-         ("RightElbow", math.radians(90.0)),
-         ("LeftShoulderPitch [shoulder]", math.radians(45.0),),
-         ("RightShoulderPitch [shoulder]", math.radians(-45.0))],
-    "chape":
-        [("leftElbowYaw", math.radians(-160)),
-         ("rightElbowYaw", math.radians(160)),
-         ("leftShoulderPitch[shoulder]", math.radians(75.27)),
-         ("rightShoulderPitch[shoulder]", math.radians(75.58)),
-         ("leftShoulderYaw", math.radians(-75.58)),
-         ("rightShoulderYaw", math.radians(75.58))],
-    "mrl_hsl":
-        [("Shoulder-L [shoulder]", math.radians(60.0)),
-         ("Shoulder-R [shoulder]", math.radians(-60.0)),
-         ("UpperArm-L", math.radians(10.0)),
-         ("UpperArm-R", math.radians(-10.0)),
-         ("LowerArm-L", math.radians(-135.0)),
-         ("LowerArm-R", math.radians(135.0))],
-    "nugus":
-        [("left_elbow_pitch", math.radians(-120)),
-         ("right_elbow_pitch", math.radians(-120)),
-         ("left_shoulder_pitch [shoulder]", math.radians(120)),
-         ("right_shoulder_pitch [shoulder]", math.radians(120)),
-         ("left_shoulder_roll", math.radians(20)),
-         ("right_shoulder_roll", math.radians(-20))],
-    "sahrv74":
-        [("left_shoulder_pitch [shoulder]", math.radians(60.0)),
-         ("right_shoulder_pitch [shoulder]", math.radians(60.0)),
-         ("left_shoulder_roll", math.radians(10.0)),
-         ("right_shoulder_roll", math.radians(10.0)),
-         ("left_elbow", math.radians(-135.0)),
-         ("right_elbow", math.radians(-135.0))],
-    "bez":
-        [("left_arm_motor_0 [shoulder]", math.radians(0)),
-         ("right_arm_motor_0 [shoulder]", math.radians(0)),
-         ("left_arm_motor_1", math.radians(170)),
-         ("right_arm_motor_1", math.radians(170))]
-}
 
-__velocity__ = 5.0
-__accelerations__ = -1.0
-__max_currents__ = -1.0
+
 
 msg = """
 BitBots Teleop
@@ -113,12 +28,12 @@
 Walk around:            Move head:
     q    w    e         u    i    o
     a    s    d         j    k    l
-                        m    ,    .  
+                        m    ,    .
 
 q/e: turn left/right    k: zero head position
 a/d: left/rigth         i/,: up/down
 w/s: forward/back       j/l: left/right
-                        u/o/m/.: combinations     
+                        u/o/m/.: combinations
 
 Controls increase / decrease with multiple presses.
 SHIFT increases with factor 10
@@ -184,29 +99,6 @@ def __init__(self):
 
         self.settings = termios.tcgetattr(sys.stdin)
 
-        parser = argparse.ArgumentParser()
-        parser.add_argument('--robot-type',
-                            help="Which robot type is used {wolfgang, op2, op3, nao, rfc, chape, mrl_hsl}",
-                            default="wolfgang")
-        args, unknown = parser.parse_known_args()
-        robot_type = args.robot_type
-        if robot_type not in ["wolfgang", "op2", "op3", "nao", "rfc", "chape", "mrl_hsl"]:
-            self.get_logger().fatal(
-                f"Robot type {robot_type} not known. Should be one of [wolfgang, op2, op3, nao, rfc, chape, mrl_hsl]")
-            exit()
-
-        # generate walkready command
-        joint_names = []
-        joint_positions = []
-        for joint_tuple in __walkready_joints__[robot_type]:
-            joint_names.append(joint_tuple[0])
-            joint_positions.append(joint_tuple[1])
-        self.walkready = JointCommand(
-            joint_names=joint_names,
-            velocities=[__velocity__] * len(joint_names),
-            accelerations=[__accelerations__] * len(joint_names),
-            max_currents=[__max_currents__] * len(joint_names),
-            positions=joint_positions)
 
         # Walking Part
         self.pub = self.create_publisher(Twist, 'cmd_vel', 1)
@@ -231,17 +123,16 @@ def __init__(self):
         else:
             self.head_pub = self.create_publisher(JointCommand, "DynamixelController/command", 1)
         self.head_msg = JointCommand()
-        self.head_msg.max_currents = [float(-1)] * 2
-        self.head_msg.velocities = [float(5)] * 2
-        self.head_msg.accelerations = [float(40)] * 2
+        self.head_msg.max_currents = [-1.0] * 2
+        self.head_msg.velocities = [5.0] * 2
+        self.head_msg.accelerations = [40.0] * 2
         self.head_msg.joint_names = ['HeadPan', 'HeadTilt']
-        self.head_msg.positions = [float(0)] * 2
+        self.head_msg.positions = [0.0] * 2
 
         self.head_pan_step = 0.05
         self.head_tilt_step = 0.05
 
         self.walk_kick_pub = self.create_publisher(Bool, "kick", 1)
-        self.joint_pub = self.create_publisher(JointCommand, "DynamixelController/command", 1)
 
         self.reset_robot = self.create_client(Empty, "/reset_pose")
         self.reset_ball = self.create_client(Empty, "/reset_ball")
@@ -249,7 +140,16 @@ def __init__(self):
         print(msg)
 
         self.frame_prefix = "" if os.environ.get("ROS_NAMESPACE") is None else os.environ.get("ROS_NAMESPACE") + "/"
-        self.client = ActionClient(self, Kick, 'dynamic_kick')
+
+        self.dynup_client = ActionClient(self, Dynup, 'dynup')
+        if not self.dynup_client.wait_for_server(timeout_sec=5.0):
+            self.get_logger().error('Dynup action server not available after waiting 5 seconds')
+
+        self.kick_client = ActionClient(self, Kick, 'dynamic_kick')
+        if not self.kick_client.wait_for_server(timeout_sec=5.0):
+            self.get_logger().error('Kick action server not available after waiting 5 seconds')
+
+
 
     def getKey(self):
         tty.setraw(sys.stdin.fileno())
@@ -257,20 +157,22 @@ def getKey(self):
         key = sys.stdin.read(1)
         termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
         return key
+    
+    def getWalkready(self):
+        goal = Dynup.Goal()
+        goal.direction = "walkready"
+        result: Dynup.Result = self.dynup_client.send_goal(goal).result
+        if not result.successful:
+            self.get_logger().error("Could not execute walkready animation")
+        return result.successful
 
     def generate_kick_goal(self, x, y, direction):
         kick_goal = Kick.Goal()
         kick_goal.header.stamp = self.get_clock().now().to_msg()
         kick_goal.header.frame_id = self.frame_prefix + "base_footprint"
-        kick_goal.ball_position.x = float(x)
-        kick_goal.ball_position.y = float(y)
-        kick_goal.ball_position.z = float(0)
-        x, y, z, w = quaternion_from_euler(0, 0, direction)
-        kick_goal.kick_direction.x = float(x)
-        kick_goal.kick_direction.y = float(y)
-        kick_goal.kick_direction.z = float(z)
-        kick_goal.kick_direction.w = float(w)
-        kick_goal.kick_speed = float(1)
+        kick_goal.ball_position = Point(x=float(x), y=float(y), z=0.0)
+        kick_goal.kick_direction = quat_from_yaw(direction)
+        kick_goal.kick_speed = 1.0
         return kick_goal
 
     def joint_state_cb(self, msg):
@@ -302,40 +204,40 @@ def loop(self):
                     self.head_pub.publish(self.head_msg)
                 elif key == 'y':
                     # kick left forward
-                    self.client.send_goal_async(self.generate_kick_goal(0.2, 0.1, 0))
+                    self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0.1, 0))
                 elif key == '<':
                     # kick left side ball left
-                    self.client.send_goal_async(self.generate_kick_goal(0.2, 0.1, -1.57))
+                    self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0.1, -1.57))
                 elif key == '>':
                     # kick left side ball center
-                    self.client.send_goal_async(self.generate_kick_goal(0.2, 0, -1.57))
+                    self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0, -1.57))
                 elif key == 'c':
                     # kick right forward
-                    self.client.send_goal_async(self.generate_kick_goal(0.2, -0.1, 0))
+                    self.kick_client.send_goal_async(self.generate_kick_goal(0.2, -0.1, 0))
                 elif key == 'v':
                     # kick right side ball right
-                    self.client.send_goal_async(self.generate_kick_goal(0.2, -0.1, 1.57))
+                    self.kick_client.send_goal_async(self.generate_kick_goal(0.2, -0.1, 1.57))
                 elif key == 'V':
                     # kick right side ball center
-                    self.client.send_goal_async(self.generate_kick_goal(0.2, 0, 1.57))
+                    self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0, 1.57))
                 elif key == "x":
                     # kick center forward
-                    self.client.send_goal_async(self.generate_kick_goal(0.2, 0, 0))
+                    self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0, 0))
                 elif key == "X":
                     # kick center backwards
-                    self.client.send_goal_async(self.generate_kick_goal(-0.2, 0, 0))
+                    self.kick_client.send_goal_async(self.generate_kick_goal(-0.2, 0, 0))
                 elif key == "b":
                     # kick left backwards
-                    self.client.send_goal_async(self.generate_kick_goal(-0.2, 0.1, 0))
+                    self.kick_client.send_goal_async(self.generate_kick_goal(-0.2, 0.1, 0))
                 elif key == "n":
                     # kick right backwards
-                    self.client.send_goal_async(self.generate_kick_goal(-0.2, -0.1, 0))
+                    self.kick_client.send_goal_async(self.generate_kick_goal(-0.2, -0.1, 0))
                 elif key == "B":
                     # kick left backwards
-                    self.client.send_goal_async(self.generate_kick_goal(0, 0.14, -1.57))
+                    self.kick_client.send_goal_async(self.generate_kick_goal(0, 0.14, -1.57))
                 elif key == "N":
                     # kick right backwards
-                    self.client.send_goal_async(self.generate_kick_goal(0, -0.14, 1.57))
+                    self.kick_client.send_goal_async(self.generate_kick_goal(0, -0.14, 1.57))
                 elif key == 'Y':
                     # kick left walk
                     self.walk_kick_pub.publish(Bool(data=False))
@@ -344,8 +246,7 @@ def loop(self):
                     self.walk_kick_pub.publish(Bool(data=True))
                 elif key == 'F':
                     # play walkready animation
-                    self.walkready.header.stamp = self.get_clock().now().to_msg()
-                    self.joint_pub.publish(self.walkready)
+                    self.getWalkready()
                 elif key == 'r':
                     # reset robot in sim
                     try:
@@ -376,12 +277,8 @@ def loop(self):
                         break
 
                 twist = Twist()
-                twist.linear.x = float(self.x)
-                twist.linear.y = float(self.y)
-                twist.linear.z = float(0)
-                twist.angular.x = float(self.a_x)
-                twist.angular.y = float(0)
-                twist.angular.z = float(self.th)
+                twist.linear = Vector3(x=float(self.x), y=float(self.y), z=0.0)
+                twist.angular = Vector3(x=float(self.a_x), y=0.0, z=float(self.th))
                 self.pub.publish(twist)
                 sys.stdout.write("\x1b[A")
                 sys.stdout.write("\x1b[A")
@@ -397,12 +294,7 @@ def loop(self):
         finally:
             print("\n")
             twist = Twist()
-            twist.linear.x = float(0)
-            twist.linear.y = float(0)
-            twist.linear.z = float(0)
-            twist.angular.x = float(-1)
-            twist.angular.y = float(0)
-            twist.angular.z = float(0)
+            twist.angular.x = -1.0
             self.pub.publish(twist)
 
             termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
diff --git a/bitbots_utils/CMakeLists.txt b/bitbots_utils/CMakeLists.txt
index 95b964b8..0e65f4c3 100644
--- a/bitbots_utils/CMakeLists.txt
+++ b/bitbots_utils/CMakeLists.txt
@@ -1,14 +1,57 @@
 cmake_minimum_required(VERSION 3.5)
 project(bitbots_utils)
 
+# Add support for C++17
+if (NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+endif ()
+
 find_package(bitbots_docs REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 enable_bitbots_docs()
 
+# Define include directories for cpp library headers
+set(INCLUDE_DIRS include)
+include_directories(${INCLUDE_DIRS})
+
+add_compile_options(-Wall -Werror -Wno-unused -pedantic -Wextra)
+
+# Add cpp library
+add_library(${PROJECT_NAME} src/utils.cpp)
+
+# Add dependencies to cpp library
+ament_target_dependencies(${PROJECT_NAME}
+                          rclcpp
+                          tf2_ros)
+
+ament_export_dependencies(rclcpp)   
+ament_export_include_directories(${INCLUDE_DIRS})
+ament_export_libraries(${PROJECT_NAME})
+
+# Python install
 ament_python_install_package(${PROJECT_NAME})
 
+# CPP Script install
+add_executable(tf_delay_plot scripts/tf_delay_plot.cpp)
+
+ament_target_dependencies(tf_delay_plot
+                          ament_cmake
+                          rclcpp
+                          tf2_msgs
+                          std_msgs)
+
+install(TARGETS tf_delay_plot
+        DESTINATION lib/${PROJECT_NAME})
+
+  
+ 
+# Copy config and launch files
 install(DIRECTORY config
   DESTINATION share/${PROJECT_NAME})
 
@@ -19,4 +62,13 @@ install(DIRECTORY scripts/
   USE_SOURCE_PERMISSIONS
   DESTINATION lib/${PROJECT_NAME})
 
+# Install library  
+install(DIRECTORY include/ 
+        DESTINATION include)
+
+install(TARGETS ${PROJECT_NAME}
+        EXPORT ${PROJECT_NAME}
+        LIBRARY DESTINATION lib
+        INCLUDES DESTINATION include)
+        
 ament_package()
diff --git a/bitbots_utils/bitbots_utils/transforms.py b/bitbots_utils/bitbots_utils/transforms.py
index 77484b46..24a42c75 100644
--- a/bitbots_utils/bitbots_utils/transforms.py
+++ b/bitbots_utils/bitbots_utils/transforms.py
@@ -1,25 +1,28 @@
 import math
 import numpy as np
 
-from transforms3d.quaternions import quat2mat, mat2quat
+from geometry_msgs.msg import Quaternion
+from ros2_numpy import msgify
 from transforms3d.euler import quat2euler, euler2quat
+from transforms3d.quaternions import quat2mat, mat2quat
 from transforms3d.quaternions import rotate_vector, qinverse, quat2mat, mat2quat
 
 
-def wxyz2xyzw(quat_wxyz):
-    return np.array([quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]])
 
+def wxyz2xyzw(quat_wxyz: np.ndarray) -> np.ndarray:
+    return quat_wxyz[[1,2,3,0]]
 
-def xyzw2wxyz(quat_xyzw):
-    return np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]])
 
+def xyzw2wxyz(quat_xyzw: np.ndarray) -> np.ndarray:
+    return quat_xyzw[[3,0,1,2]]
 
-def quat2sixd(quat_wxyz):
+
+def quat2sixd(quat_wxyz: np.ndarray) -> np.ndarray:
     # see https://openaccess.thecvf.com/content_CVPR_2019/supplemental/Zhou_On_the_Continuity_CVPR_2019_supplemental.pdf
     # first get matrix
     m = quat2mat(quat_wxyz)
     # 6D represenation is first 2 coloumns of matrix
-    return [m[0][0], m[1][0], m[2][0], m[0][1], m[1][1], m[2][1]]
+    return m[[[0][0], m[1][0], m[2][0], m[0][1], m[1][1], m[2][1]]]
 
 
 def sixd2quat(sixd):
@@ -36,8 +39,15 @@ def sixd2quat(sixd):
     return mat2quat(mat)
 
 
-def quat2fused(q):
-    q_xyzw = wxyz2xyzw(q)
+def quat2fused(q, order='wxyz'):
+    # Check quaternion order
+    if order == 'xyzw':
+        q_xyzw = q
+    elif order == 'wxyz':
+        q_xyzw = wxyz2xyzw(q)
+    else:
+        raise ValueError('Unknown quaternion order: {}'.format(order))
+
     # Fused yaw of Quaternion
     fused_yaw = 2.0 * math.atan2(q_xyzw[2],
                                  q_xyzw[3])  # Output of atan2 is [-tau/2,tau/2], so this expression is in [-tau,tau]
@@ -65,7 +75,7 @@ def quat2fused(q):
     return fused_roll, fused_pitch, fused_yaw, hemi
 
 
-# Conversion: Fused angles (3D/4D) --> Quaternion
+# Conversion: Fused angles (3D/4D) --> Quaternion (wxyz)
 def fused2quat(fusedRoll, fusedPitch, fusedYaw, hemi):
     # Precalculate the sine values
     sth = math.sin(fusedPitch)
@@ -112,3 +122,7 @@ def compute_imu_orientation_from_world(robot_quat_in_world):
     rp = rotate_vector((yrp_world_frame[1], yrp_world_frame[2], 0), qinverse(yaw_quat))
     # save in correct order
     return [rp[0], rp[1], 0], yaw_quat
+
+
+def quat_from_yaw(yaw: float) -> Quaternion:
+    return msgify(Quaternion, wxyz2xyzw(euler2quat(yaw, 0, 0, axes='szxy')))
diff --git a/bitbots_utils/bitbots_utils/utils.py b/bitbots_utils/bitbots_utils/utils.py
index ac3b0467..71562bc3 100644
--- a/bitbots_utils/bitbots_utils/utils.py
+++ b/bitbots_utils/bitbots_utils/utils.py
@@ -150,6 +150,7 @@ def set_parameters_of_other_node(own_node: Node,
     response = future.result()
     return [res.success for res in response.results]
 
+
 def parse_parameter_dict(*, namespace, parameter_dict):
     parameters = []
     for param_name, param_value in parameter_dict.items():
diff --git a/bitbots_utils/config/game_settings_options.yaml b/bitbots_utils/config/game_settings_options.yaml
index b109a230..af9ee71e 100644
--- a/bitbots_utils/config/game_settings_options.yaml
+++ b/bitbots_utils/config/game_settings_options.yaml
@@ -16,7 +16,7 @@ team_color:
   type: int
   explanation: "0 is blue, 1 is red"
 role:
-  options: ['goalie', 'offense', 'defense', 'penalty']
+  options: ['goalie', 'offense', 'defense']
   type: str
   explanation: ""
 position_number:
diff --git a/bitbots_utils/config/multirobot.rviz b/bitbots_utils/config/multirobot.rviz
deleted file mode 100644
index 3468e9f8..00000000
--- a/bitbots_utils/config/multirobot.rviz
+++ /dev/null
@@ -1,1228 +0,0 @@
-Panels:
-  - Class: rviz/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded: ~
-      Splitter Ratio: 0.5
-    Tree Height: 698
-  - Class: rviz/Selection
-    Name: Selection
-  - Class: rviz/Tool Properties
-    Expanded:
-      - /2D Pose Estimate1
-      - /2D Nav Goal1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz/Time
-    Experimental: false
-    Name: Time
-    SyncMode: 0
-    SyncSource: ""
-Preferences:
-  PromptSaveOnExit: true
-Toolbars:
-  toolButtonStyle: 2
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz/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
-    - Alpha: 0.699999988079071
-      Class: rviz/Map
-      Color Scheme: map
-      Draw Behind: false
-      Enabled: true
-      Name: Map
-      Topic: /amy/field/map
-      Unreliable: false
-      Use Timestamp: false
-      Value: true
-    - Alpha: 1
-      Class: rviz/RobotModel
-      Collision Enabled: false
-      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_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        camera:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        camera_optical_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        head:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        imu_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        imu_frame_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_ankle:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_cleat_l_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_cleat_l_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_cleat_r_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_cleat_r_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_foot:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_hip_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_hip_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_lower_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_lower_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_shoulder:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_sole:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_toe:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_upper_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_upper_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_wrist:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        llb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        llf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        lrb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        lrf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        neck:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_ankle:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_cleat_l_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_cleat_l_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_cleat_r_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_cleat_r_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_foot:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_hip_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_hip_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_lower_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_lower_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_shoulder:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_sole:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_toe:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_upper_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_upper_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_wrist:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        rlb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rlf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rrb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rrf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        torso:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Name: RobotModel
-      Robot Description: /amy/robot_description
-      TF Prefix: /amy
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-    - Alpha: 1
-      Class: rviz/RobotModel
-      Collision Enabled: false
-      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_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        camera:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        camera_optical_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        head:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        imu_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        imu_frame_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_ankle:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_cleat_l_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_cleat_l_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_cleat_r_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_cleat_r_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_foot:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_hip_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_hip_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_lower_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_lower_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_shoulder:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_sole:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_toe:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_upper_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_upper_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_wrist:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        llb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        llf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        lrb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        lrf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        neck:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_ankle:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_cleat_l_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_cleat_l_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_cleat_r_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_cleat_r_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_foot:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_hip_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_hip_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_lower_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_lower_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_shoulder:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_sole:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_toe:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_upper_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_upper_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_wrist:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        rlb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rlf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rrb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rrf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        torso:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Name: RobotModel
-      Robot Description: /rory/robot_description
-      TF Prefix: /rory
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-    - Alpha: 1
-      Class: rviz/RobotModel
-      Collision Enabled: false
-      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_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        camera:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        camera_optical_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        head:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        imu_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        imu_frame_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_ankle:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_cleat_l_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_cleat_l_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_cleat_r_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_cleat_r_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_foot:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_hip_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_hip_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_lower_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_lower_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_shoulder:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_sole:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_toe:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_upper_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_upper_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_wrist:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        llb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        llf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        lrb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        lrf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        neck:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_ankle:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_cleat_l_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_cleat_l_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_cleat_r_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_cleat_r_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_foot:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_hip_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_hip_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_lower_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_lower_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_shoulder:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_sole:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_toe:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_upper_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_upper_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_wrist:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        rlb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rlf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rrb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rrf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        torso:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Name: RobotModel
-      Robot Description: /jack/robot_description
-      TF Prefix: /jack
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-    - Alpha: 1
-      Class: rviz/RobotModel
-      Collision Enabled: false
-      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_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        camera:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        camera_optical_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        head:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        imu_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        imu_frame_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_ankle:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_cleat_l_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_cleat_l_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_cleat_r_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_cleat_r_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_foot:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_hip_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_hip_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_lower_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_lower_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_shoulder:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_sole:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_toe:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        l_upper_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_upper_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        l_wrist:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        llb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        llf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        lrb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        lrf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        neck:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_ankle:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_cleat_l_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_cleat_l_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_cleat_r_back:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_cleat_r_front:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_foot:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_hip_1:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_hip_2:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_lower_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_lower_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_shoulder:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_sole:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_toe:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        r_upper_arm:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_upper_leg:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        r_wrist:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        rlb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rlf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rrb:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        rrf:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        torso:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Name: RobotModel
-      Robot Description: /donna/robot_description
-      TF Prefix: /donna
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-    - Alpha: 1
-      Axes Length: 1
-      Axes Radius: 0.10000000149011612
-      Class: rviz/Pose
-      Color: 255; 25; 0
-      Enabled: true
-      Head Length: 0.30000001192092896
-      Head Radius: 0.10000000149011612
-      Name: Pose
-      Shaft Length: 1
-      Shaft Radius: 0.05000000074505806
-      Shape: Arrow
-      Topic: /amy/move_base/current_goal
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Axes Length: 1
-      Axes Radius: 0.10000000149011612
-      Class: rviz/Pose
-      Color: 255; 25; 0
-      Enabled: true
-      Head Length: 0.30000001192092896
-      Head Radius: 0.10000000149011612
-      Name: Pose
-      Shaft Length: 1
-      Shaft Radius: 0.05000000074505806
-      Shape: Arrow
-      Topic: /rory/move_base/current_goal
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Axes Length: 1
-      Axes Radius: 0.10000000149011612
-      Class: rviz/Pose
-      Color: 255; 25; 0
-      Enabled: true
-      Head Length: 0.30000001192092896
-      Head Radius: 0.10000000149011612
-      Name: Pose
-      Shaft Length: 1
-      Shaft Radius: 0.05000000074505806
-      Shape: Arrow
-      Topic: /jack/move_base/current_goal
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Axes Length: 1
-      Axes Radius: 0.10000000149011612
-      Class: rviz/Pose
-      Color: 255; 25; 0
-      Enabled: true
-      Head Length: 0.30000001192092896
-      Head Radius: 0.10000000149011612
-      Name: Pose
-      Shaft Length: 1
-      Shaft Radius: 0.05000000074505806
-      Shape: Arrow
-      Topic: /donna/move_base/current_goal
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 25; 255; 0
-      Enabled: true
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Lines
-      Line Width: 0.029999999329447746
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic: /amy/move_base/NavfnROS/plan
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 25; 255; 0
-      Enabled: true
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Lines
-      Line Width: 0.029999999329447746
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic: /rory/move_base/NavfnROS/plan
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 25; 255; 0
-      Enabled: true
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Lines
-      Line Width: 0.029999999329447746
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic: /jack/move_base/NavfnROS/plan
-      Unreliable: false
-      Value: true
-    - Alpha: 1
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 25; 255; 0
-      Enabled: true
-      Head Diameter: 0.30000001192092896
-      Head Length: 0.20000000298023224
-      Length: 0.30000001192092896
-      Line Style: Lines
-      Line Width: 0.029999999329447746
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Pose Color: 255; 85; 255
-      Pose Style: None
-      Radius: 0.029999999329447746
-      Shaft Diameter: 0.10000000149011612
-      Shaft Length: 0.10000000149011612
-      Topic: /donna/move_base/NavfnROS/plan
-      Unreliable: false
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Default Light: true
-    Fixed Frame: map
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz/Interact
-      Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-    - Class: rviz/FocusCamera
-    - Class: rviz/Measure
-    - Class: rviz/SetInitialPose
-      Theta std deviation: 0.2617993950843811
-      Topic: /initialpose
-      X std deviation: 0.5
-      Y std deviation: 0.5
-    - Class: rviz/SetGoal
-      Topic: /move_base_simple/goal
-    - Class: rviz/PublishPoint
-      Single click: true
-      Topic: /clicked_point
-  Value: true
-  Views:
-    Current:
-      Class: rviz/Orbit
-      Distance: 12.589564323425293
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 0.06874994933605194
-        Y: -0.4793209135532379
-        Z: -0.45180970430374146
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5403993129730225
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 1.5404052734375
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 995
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000015600000345fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000345000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000345fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000345000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000064d0000003efc0100000002fb0000000800540069006d006501000000000000064d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003dc0000034500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1613
-  X: 67
-  Y: 27
diff --git a/bitbots_utils/config/welcome_art.txt b/bitbots_utils/config/welcome_art.txt
new file mode 100644
index 00000000..de18e86b
--- /dev/null
+++ b/bitbots_utils/config/welcome_art.txt
@@ -0,0 +1,29 @@
+
+                                                      
+                   &&  &&&&&&&&,  &&                  
+               &&&&&&&&&&&&&&&&&&&&&&&&(              
+             /&&&&&&&&&&&&&&&&&&&&%&&&&&&             
+            &&&&&&&&&&&&######%&&&&&&&&&&&            
+        &###&&&&&&%&&&&&&&&%&&&&&&%&&&&&&&%##%#       
+     &&%###%&&/******(&&&&&&&&&&(*******(&&##%%#&     
+    &&&%%&&&/***#%%(***#&&&&&&&***(&&&(***%&&%%&&&    
+    &&&&&&&(**%&&&&&&/**&&%&%&/**&&&&&&&**/&&&&&&&&   
+    &&&&&&&%**(&&&&&&***&&%%%&#**#&&&&&(**(&&&&&&&#   
+   #%&&&&&&&&/********(&&&&&&&&&/********&&&&&&&&&%&  
+   #&&&&&#,&&&&&&%%&&&&&&&&&&&&&&&&&&&&&&&&#%%&&&&&&  
+   &%%&%#&  (&&&&&&&&&&&&&&&&&&&&&&&&&&&&&  .%%&&%%,  
+   %%%&.      /&&&%&&&&&&&&&&&&&&&%&&&&&       &%%#&  
+   %%%%&         *&&&&&&&&&&&&&&&&&&&         .&%%%&  
+  &%%%%.        *&%%%&& &&&&&&,&&%%%&&         &%%%%  
+  &%%%&         &&#,*%&        &%(,#&&          (%%%  
+  &%%&          &&&&&&&        &&&&&&&           %%%  
+  &%*&&         &&&&&&&        &&&&&&&          &&&%  
+                &&&&&&&        &&&%&&&                
+               /&&&&&&&&      %&&&&&&&(               
+               /&&%&&&&&      %&&&%&&&(               
+               /&&&&&&&&      %&&&&&&&(               
+               /&&&&&&&&      %&&&%&&&(               
+               /&&&&&&&&      &&&&&&&&(               
+              //%(#*(%&        &%(*((%/%              
+              #///////%        #///////(              
+                                                      
diff --git a/bitbots_utils/include/bitbots_utils/utils.hpp b/bitbots_utils/include/bitbots_utils/utils.hpp
new file mode 100644
index 00000000..5503c87d
--- /dev/null
+++ b/bitbots_utils/include/bitbots_utils/utils.hpp
@@ -0,0 +1,41 @@
+#ifndef BITBOTS_UTILS__UTILS_H_
+#define BITBOTS_UTILS__UTILS_H_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <chrono>
+#include <string>
+#include <tf2_ros/buffer.h>
+#include <vector> 
+
+using namespace std::chrono_literals;
+
+namespace bitbots_utils
+{
+
+/**
+ * @brief Waits for the transforms to be available
+ * @param logger The logger to use for logging
+ * @param clock The clock to use for time
+ * @param tf_buffer The tf buffer to use
+ * @param frames The tf frames to wait for
+ * @param root_frame The root frame to transform from
+ * @param check_interval Interval in which to check for the frames
+ * @param warn_duration Duration after which to warn if the frames are not available
+ * @param warn_interval Interval in which to keep warning if the frames are not available
+ * @param verbose Can be used to disable the warning messages
+ */
+void wait_for_tf(
+    const rclcpp::Logger &logger, 
+    std::shared_ptr<rclcpp::Clock> clock, 
+    std::shared_ptr<tf2_ros::Buffer> tf_buffer, 
+    const std::vector<std::string> &frames, 
+    const std::string &root_frame,
+    const rclcpp::Duration &check_interval = rclcpp::Duration(0.1s),
+    const rclcpp::Duration &warn_duration = rclcpp::Duration(5.0s),
+    const rclcpp::Duration &warn_interval = rclcpp::Duration(1.0s),
+    bool verbose = true);
+
+}  // namespace bitbots_utils
+
+#endif  // BITBOTS_UTILS__UTILS_H_
diff --git a/bitbots_utils/launch/base.launch b/bitbots_utils/launch/base.launch
deleted file mode 100644
index 19800956..00000000
--- a/bitbots_utils/launch/base.launch
+++ /dev/null
@@ -1,47 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <!-- a bit hacky as we can not have an empty string as default value for an environment variable -->
-    <let name="tf_prefix" value="$(eval '\'$(env ROS_NAMESPACE not_defined)\' if \'$(env ROS_NAMESPACE not_defined)\' != \'not_defined\' else \'\'')"/>
-    <arg name="sim" default="false"/>
-    <arg name="sim_ns" default="/" description="namespace for gazebo plugins" />
-    <arg name="fake_walk" default="False" description="fake walk controller for simulation" />
-    <arg name="robot_type" default="wolfgang" description="what kind of robot should be started" />
-
-    <!-- global parameters -->
-    <node name="parameter_blackboard" pkg="demo_nodes_cpp" exec="parameter_blackboard" args="--ros-args --log-level WARN">
-        <param name="simulation_active" value="$(var sim)"/>
-        <param name="use_sim_time" value="$(var sim)"/>
-        <param from="$(find-pkg-share bitbots_utils)/config/global_parameters.yaml" />
-        <param from="$(find-pkg-share bitbots_utils)/config/game_settings.yaml" />
-    </node>
-
-    <group>
-        <include file="$(find-pkg-share bitbots_utils)/launch/move_group.py">
-          <arg name="sim" value="$(var sim)"/>
-          <arg name="robot_type" value="$(var robot_type)"/>
-        </include>
-    </group>
-
-
-    <group>
-        <include file="$(find-pkg-share bitbots_extrinsic_calibration)/launch/calibration.launch">
-          <arg name="sim" value="$(var sim)"/>
-        </include>
-    </group>
-
-    <group>
-        <node pkg="humanoid_base_footprint" exec="base_footprint" output="screen">
-          <param name="support_state_topics" value="[walk_support_state, dynamic_kick_support_state]"/>
-          <param name="use_sim_time" value="$(var sim)"/>
-        </node>
-        <include file="$(find-pkg-share bitbots_convenience_frames)/launch/convenience_frames.launch">
-          <arg name="sim" value="$(var sim)"/>
-        </include>
-    </group>
-
-    <group unless="$(var fake_walk)">
-        <include file="$(find-pkg-share bitbots_odometry)/launch/odometry.launch">
-          <arg name="sim" value="$(var sim)"/>
-        </include>
-    </group>
-</launch>
diff --git a/bitbots_utils/launch/parameter_blackboard.launch b/bitbots_utils/launch/parameter_blackboard.launch
new file mode 100644
index 00000000..9e404b82
--- /dev/null
+++ b/bitbots_utils/launch/parameter_blackboard.launch
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="sim" default="false"/>
+    <arg name="print_bit_bot" default="true"/>
+
+    <!-- Print Bit-Bot on the Terminal -->
+    <executable name="welcome" cmd="cat $(find-pkg-share bitbots_utils)/config/welcome_art.txt" output="screen" if="$(var print_bit_bot)"/>
+
+    <!-- global parameters -->
+    <node name="parameter_blackboard" pkg="demo_nodes_cpp" exec="parameter_blackboard" args="--ros-args --log-level WARN">
+        <param name="simulation_active" value="$(var sim)"/>
+        <param name="use_sim_time" value="$(var sim)"/>
+        <param from="$(find-pkg-share bitbots_utils)/config/global_parameters.yaml" />
+        <param from="$(find-pkg-share bitbots_utils)/config/game_settings.yaml" />
+    </node>
+</launch>
diff --git a/bitbots_utils/package.xml b/bitbots_utils/package.xml
index 050b1a9e..d2870c30 100644
--- a/bitbots_utils/package.xml
+++ b/bitbots_utils/package.xml
@@ -15,21 +15,12 @@
     <buildtool_depend>ament_cmake</buildtool_depend>
 
     <depend>bitbots_docs</depend>
-
-    <exec_depend>bitbots_convenience_frames</exec_depend>
-    <exec_depend>bitbots_extrinsic_calibration</exec_depend>
-    <exec_depend>bitbots_odometry</exec_depend>
-    <exec_depend>demo_nodes_cpp </exec_depend>
-    <exec_depend>humanoid_base_footprint</exec_depend>
-    <exec_depend>robot_state_publisher</exec_depend>
-    <exec_depend>wolfgang_description</exec_depend>
-    <exec_depend>wolfgang_moveit_config</exec_depend>
-    <exec_depend>xacro</exec_depend>
-    <exec_depend>audio_common</exec_depend>
     <depend>python3-yaml</depend>
     <depend>python3-transforms3d</depend>
+    <depend>demo_nodes_cpp</depend>
     <depend>python3-numpy</depend>
-    <depend>topic_tools</depend>
+    <depend>geometry_msgs</depend>
+    <depend>ros2_numpy</depend>
 
     <export>
         <bitbots_documentation>
diff --git a/bitbots_utils/scripts/boot-highlevel.sh b/bitbots_utils/scripts/boot-highlevel.sh
deleted file mode 100755
index db25e301..00000000
--- a/bitbots_utils/scripts/boot-highlevel.sh
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/bin/bash
-
-#
-# This script gets executed on boot if the systemd service 'start_behavior.service' is enabled
-#
-
-if hash roslaunch 2>/dev/null; then
-    roslaunch bitbots_bringup highlevel.launch
-else
-    echo "Please source the workspace"
-    exit 1
-fi
diff --git a/bitbots_utils/scripts/boot-motion.sh b/bitbots_utils/scripts/boot-motion.sh
deleted file mode 100755
index 3c6c4446..00000000
--- a/bitbots_utils/scripts/boot-motion.sh
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/bin/bash
-
-#
-# This script gets executed on boot if the systemd service 'start_roscore.service' is enabled
-#
-
-if hash roslaunch 2>/dev/null; then
-    roslaunch bitbots_bringup motion_standalone.launch
-else
-    echo "Please source the workspace"
-    exit 1
-fi
diff --git a/bitbots_utils/scripts/boot-roscore.sh b/bitbots_utils/scripts/boot-roscore.sh
deleted file mode 100755
index 97b92372..00000000
--- a/bitbots_utils/scripts/boot-roscore.sh
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/bin/bash
-
-#
-# This script gets executed on boot if the systemd service 'start_roscore.service' is enabled
-#
-
-if hash roscore 2>/dev/null; then
-    roscore
-else
-    echo "Please source the workspace"
-    exit 1
-fi
diff --git a/bitbots_utils/scripts/boot-teamplayer.sh b/bitbots_utils/scripts/boot-teamplayer.sh
deleted file mode 100755
index 596139c5..00000000
--- a/bitbots_utils/scripts/boot-teamplayer.sh
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/bin/bash
-
-#
-# This script gets executed on boot if the systemd service 'start_behavior.service' is enabled
-#
-
-if hash roslaunch 2>/dev/null; then
-    roslaunch bitbots_bringup teamplayer.launch
-else
-    echo "Please source the workspace"
-    exit 1
-fi
diff --git a/bitbots_utils/scripts/boot-vision.sh b/bitbots_utils/scripts/boot-vision.sh
deleted file mode 100755
index 29564541..00000000
--- a/bitbots_utils/scripts/boot-vision.sh
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/bin/bash
-
-#
-# This script gets executed on boot if the systemd service 'start_vision.service' is enabled
-#
-
-if hash roslaunch 2>/dev/null; then
-    roslaunch bitbots_vision vision_startup.launch
-else
-    echo "Please source the workspace"
-    exit 1
-fi
diff --git a/bitbots_utils/scripts/dummy_imu.py b/bitbots_utils/scripts/dummy_imu.py
index 9198ea90..0151082a 100755
--- a/bitbots_utils/scripts/dummy_imu.py
+++ b/bitbots_utils/scripts/dummy_imu.py
@@ -1,6 +1,4 @@
 #!/usr/bin/env python3
-import threading
-
 import rclpy
 from rclpy.node import Node
 from rclpy.executors import MultiThreadedExecutor
@@ -13,7 +11,7 @@ def __init__(self):
         super().__init__('DummyImu')
         self.pub = self.create_publisher(Imu, '/imu/data', 1)
         self.msg = Imu()
-        self.msg.header.frame_id = 'imu'
+        self.msg.header.frame_id = 'imu_frame'
         self.msg.orientation.w = 1.0
 
     def loop(self):
diff --git a/bitbots_utils/scripts/extract_profiler.py b/bitbots_utils/scripts/extract_profiler.py
deleted file mode 100755
index a1abf73b..00000000
--- a/bitbots_utils/scripts/extract_profiler.py
+++ /dev/null
@@ -1,20 +0,0 @@
-#!/usr/bin/env python3
-
-import rclpy
-from rclpy.node import Node
-from swri_profiler_msgs.msg import ProfileDataArray
-from bitbots_msgs.msg import ProfileDataStamped
-
-"""
-This script extracts data from the SWRI profiler to later use it in combination with the rosbag_to_pandas script.
-"""
-rclpy.init(args=None)
-pub = self.create_publisher(ProfileDataStamped, "/profiler/single_data", 1)
-
-
-def cb(msg):
-    pub.publish(ProfileDataStamped(msg.header, msg.rostime_stamp, msg.data[0]))
-
-
-rospy.Subscriber("/profiler/data", ProfileDataArray, cb)
-rclpy.spin(self)
diff --git a/bitbots_utils/scripts/goals_to_joint_states.py b/bitbots_utils/scripts/goals_to_joint_states.py
deleted file mode 100755
index 0b96fc92..00000000
--- a/bitbots_utils/scripts/goals_to_joint_states.py
+++ /dev/null
@@ -1,23 +0,0 @@
-#!/usr/bin/env python3
-#  -*- coding: utf8 -*-
-import rospy
-from bitbots_msgs.msg import JointCommand
-from sensor_msgs.msg import JointState
-
-
-def goal_cb(msg):
-    state_msg = JointState()
-    state_msg.name = msg.joint_names
-    state_msg.position = msg.positions
-    state_msg.header = msg.header
-    state_msg.header.stamp = rospy.Time.now()
-
-    state_publisher.publish(state_msg)
-
-
-if __name__ == "__main__":
-    rospy.init_node('goals_to_joint_states')
-    state_publisher = rospy.Publisher("/joint_states", JointState, queue_size=1)
-    sub = rospy.Subscriber("/DynamixelController/command", JointCommand, callback=goal_cb, queue_size=1,
-                           tcp_nodelay=True)
-    rospy.spin()
diff --git a/bitbots_utils/scripts/keep_stable_in_sim.py b/bitbots_utils/scripts/keep_stable_in_sim.py
deleted file mode 100755
index 1d26d8a9..00000000
--- a/bitbots_utils/scripts/keep_stable_in_sim.py
+++ /dev/null
@@ -1,87 +0,0 @@
-#!/usr/bin/env python3
-
-import rclpy
-from rclpy.node import Node
-from bitbots_msgs.msg import FootPressure
-from gazebo_msgs.msg import ModelStates, ModelState
-from gazebo_msgs.srv import SetModelState, SetModelStateRequest
-
-import tf
-
-
-position = None
-roll = None
-pitch = None
-yaw = None
-
-
-def state_update(state_msg):
-    global position
-    global roll
-    global pitch
-    global yaw
-    global ball_pose
-    global ball_twist
-
-    index = 0
-    for name in state_msg.name:
-        if name == "/":
-
-            position = state_msg.pose[index].position
-            orientation = state_msg.pose[index].orientation
-            quaternion = (
-                orientation.x,
-                orientation.y,
-                orientation.z,
-                orientation.w)
-            euler = tf.transformations.euler_from_quaternion(quaternion)
-            roll = euler[0]
-            pitch = euler[1]
-            yaw = euler[2]
-        elif name == "teensize_ball":
-
-            ball_pose = state_msg.pose[index]
-            ball_twist = state_msg.twist[index]
-
-        index += 1
-
-
-if __name__ == "__main__":
-    rclpy.init(args=None)
-    rospy.wait_for_service("/gazebo/set_model_state")
-    set_state = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)
-
-
-    goal_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, state_update, tcp_nodelay=True)
-
-    request = SetModelStateRequest()
-    request.model_state.model_name = "/"
-    ball_request = SetModelStateRequest()
-    ball_request.model_state.model_name = "teensize_ball"
-    # wait because we want to be called
-    rospy.sleep(1.0)
-    rate = self.create_rate(10)
-    while rclpy.ok():
-        try:
-            # check if we have values already, otherwise we will do math with none
-            if(yaw):
-                request.model_state.pose.position = position
-                request.model_state.pose.position.z = 0.43  # the robot is not and will not be ready for take off.
-                quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw)
-                request.model_state.pose.orientation.x = quaternion[0]
-                request.model_state.pose.orientation.y = quaternion[1]
-                request.model_state.pose.orientation.z = quaternion[2]
-                request.model_state.pose.orientation.w = quaternion[3]
-                set_state(request)
-            if ball_pose:
-                ball_request.model_state.pose = ball_pose
-                ball_request.model_state.pose.position.z = 0.095
-                ball_request.model_state.twist = ball_twist
-                set_state(ball_request)
-
-            rate.sleep()
-        except rospy.exceptions.ROSTimeMovedBackwardsException:
-            rospy.logwarn(
-            "We moved backwards in time. I hope you just resetted the simulation. If not there is something wrong")
-        except rospy.exceptions.ROSInterruptException:
-            exit()
diff --git a/bitbots_utils/scripts/motor_goals_viz_helper.py b/bitbots_utils/scripts/motor_goals_viz_helper.py
index 261e90d1..53d085c9 100755
--- a/bitbots_utils/scripts/motor_goals_viz_helper.py
+++ b/bitbots_utils/scripts/motor_goals_viz_helper.py
@@ -4,12 +4,12 @@
 import sys
 
 import rclpy
+
+from bitbots_msgs.msg import Animation, JointCommand
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
-from std_msgs.msg import Float64MultiArray
 from sensor_msgs.msg import JointState
-from bitbots_msgs.msg import JointCommand
-from humanoid_league_msgs.msg import Animation
+from std_msgs.msg import Float64MultiArray
 
 
 class MotorVizHelper(Node):
@@ -109,7 +109,7 @@ def animation_cb(self, msg: Animation):
             self.joint_command_msg.positions[self.joint_names.index(name)] = msg.position.points[0].positions[i]
             self.joint_command_msg.velocities[self.joint_names.index(name)] = -1
 
-    def update_joint_states(self, msg):
+    def update_joint_states(self, msg: JointState):
         for i in range(len(msg.joint_names)):
             name = msg.joint_names[i]
             if msg.velocities[i] == -1:
diff --git a/bitbots_utils/scripts/publish_ball_relative.py b/bitbots_utils/scripts/publish_ball_relative.py
deleted file mode 100755
index bc5d7c52..00000000
--- a/bitbots_utils/scripts/publish_ball_relative.py
+++ /dev/null
@@ -1,51 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf8 -*-
-
-import rclpy
-from rclpy.node import Node
-import sys
-import random
-from humanoid_league_msgs.msg import PoseWithCertainty, PoseWithCertaintyArray
-
-delta = 1
-max_x = 10
-min_x = 0.3
-max_y = 10
-min_y = -10
-max_z = 0
-min_z = 0
-
-if __name__ == "__main__":
-    rclpy.init(args=None)
-    balls_relative_publisher = self.create_publisher(PoseWithCertaintyArray, "balls_relative", 10, tcp_nodelay=True)
-
-    x = random.uniform(min_x, max_x) / 4
-    y = random.uniform(min_y, max_y) / 4
-    z = random.uniform(min_z, max_z) / 4
-
-    while rclpy.ok():
-        ball_msg = PoseWithCertainty()
-        if len(sys.argv) is in [3, 4]:
-            ball_msg.pose.pose.position.x = float(sys.argv[1])
-            ball_msg.pose.pose.position.y = float(sys.argv[2])
-            if len(sys.argv) == 4:
-                ball_msg.pose.pose.position.z= float(sys.argv[3])
-        else:
-            ball_msg.ball_relative.x = x
-            ball_msg.ball_relative.y = y
-            ball_msg.ball_relative.z = z
-
-            # New position for next step
-            x = max(min_x, min(x + delta * random.uniform(-1, 1), max_x))
-            y = max(min_y, min(y + delta * random.uniform(-1, 1), max_y))
-            z = max(min_z, min(z + delta * random.uniform(-1, 1), max_z))
-
-        ball_msg.confidence = 1.0
-
-        balls_msg = PoseWithCertaintyArray()
-        balls_msg.header.stamp = self.get_clock().now()
-        balls_msg.poses = [ball_msg]
-        balls_relative_publisher.publish(balls_msg)
-
-        rospy.sleep(0.5)
-0.5)
diff --git a/bitbots_utils/scripts/rosbag_to_pandas.py b/bitbots_utils/scripts/rosbag_to_pandas.py
deleted file mode 100755
index ed7747ba..00000000
--- a/bitbots_utils/scripts/rosbag_to_pandas.py
+++ /dev/null
@@ -1,184 +0,0 @@
-#!/usr/bin/env python3
-
-import rosbag
-import argparse
-import rosmsg
-import pandas as pd
-import os.path
-import pickle
-
-FILE_PATH = "/tmp/rosbag_to_pandas_input"
-
-"""
-This script reads in a rosbag and ouputs a pandas dataframe representration of the data. This is usefull for later 
-processing, e.g. in scikit-learn or for creating plots.
-"""
-
-
-class COLORS:
-    HEADER = '\033[95m'
-    OKBLUE = '\033[94m'
-    OKGREEN = '\033[92m'
-    WARNING = '\033[93m'
-    FAIL = '\033[91m'
-    ENDC = '\033[0m'
-    BOLD = '\033[1m'
-    UNDERLINE = '\033[4m'
-
-
-parser = argparse.ArgumentParser()
-parser.add_argument("bag")
-parser.add_argument("--output-folder", required=False)
-
-args = parser.parse_args()
-
-bag = rosbag.Bag(args.bag)
-topics = []
-types = []
-for key, value in bag.get_type_and_topic_info().topics.items():
-    topics.append(key)
-    types.append(value[0])
-
-# see if we have a previous set of inputs
-use_saved = False
-if os.path.isfile(FILE_PATH):
-    use_saved_input = input("Do you want to extract the same data as last time? [Y|n]")
-    use_saved = use_saved_input in ["y", "Y", ""]
-    if use_saved:
-        saved_input = pickle.load(open(FILE_PATH, "rb"))
-else:
-    print("No previous input found. You will need to enter everything.")
-
-if use_saved:
-    topic_selections_str = saved_input["topic_selections_str"]
-else:
-    col_width = max(len(word) for row in [topics, types] for word in row) + 4  #
-    for i in range(len(topics)):
-        print(f"{COLORS.BOLD}{COLORS.OKBLUE}[{i}]:{COLORS.ENDC} {topics[i].ljust(col_width)}{types[i]}")
-    topic_selections_str = input("Select data source. Use a format like 0 2 3.\n")
-
-topic_selections_str_list = topic_selections_str.split()
-topic_selections = []
-
-for topic_selection_str in topic_selections_str_list:
-    try:
-        topic_selections.append(int(topic_selection_str))
-    except ValueError as ex:
-        print(ex)
-        exit(-1)
-
-selected_topics_list = [topics[i] for i in topic_selections]
-# used later to know how far you have been
-message_count = sum(bag.get_message_count(topics[i]) for i in topic_selections)
-
-if use_saved:
-    data_selections = saved_input["data_selections"]
-else:
-    data_selections = []
-    for topic_selection in topic_selections:
-        print(
-            f"{COLORS.BOLD}{COLORS.OKGREEN}Available Fields from {COLORS.OKBLUE}{topics[topic_selection]}{COLORS.ENDC}")
-        print(rosmsg.get_msg_text(types[topic_selection]))
-        single_topic_data_selection = input("Specify which fields you want seperated with spaces. "
-                                            "You can also use a semantic like \"orientation.x\" to access internal fields."
-                                            "Header stamp and seq are included automatically.\n")
-        data_selections.append(single_topic_data_selection.split())
-
-
-def recursive_getattr(obj, field_list):
-    if len(field_list) == 1:
-        return getattr(obj, field_list[0])
-    else:
-        return recursive_getattr(getattr(obj, field_list[0]), field_list[1:])
-
-
-frames = []
-for data_selection in data_selections:
-    frames.append(pd.DataFrame(columns=["header"].extend(data_selection)))
-
-msg_generator = bag.read_messages(topics=[topics[i] for i in topic_selections])
-
-if use_saved:
-    seperate_input = saved_input["seperate_input"]
-else:
-    seperate_input = input("Do you want to extract lists and tuples into separate columns? [Y|n]")
-seperate = seperate_input in ["y", "Y", ""]
-if seperate:
-    print("Will separate\n")
-else:
-    print("Will not separate\n")
-
-if use_saved:
-    remove_remove_time_offset_input = saved_input["remove_remove_time_offset_input"]
-else:
-    remove_remove_time_offset_input = input("Do you want the time to start at 0 (remove time offset)? [Y|n]")
-remove_time_offset = remove_remove_time_offset_input in ["y", "Y", ""]
-if remove_time_offset:
-    print("Will remove offset\n")
-else:
-    print("Will not remove offset\n")
-
-print(f"{COLORS.BOLD}{COLORS.WARNING}Extracting data{COLORS.ENDC}")
-header_warning_printed = False
-current_msg_index = 0
-first_stamp = [None] * len(selected_topics_list)
-for msg in msg_generator:
-    # give some status feedback since this can take a while
-    if current_msg_index % 1000 == 0:
-        print(f"{current_msg_index} / {message_count}")
-    current_msg_index += 1
-
-    try:
-        i = selected_topics_list.index(msg.topic)
-    except ValueError:
-        continue
-
-    fields = {}
-    try:
-        time_stamp = msg.message.header.stamp.to_sec()
-    except AttributeError:
-        if not header_warning_printed:
-            header_warning_printed = True
-            print(
-                f"{COLORS.WARNING}No header found in one of the messages. Will use receiving time as stamp.{COLORS.ENDC}")
-        time_stamp = msg.timestamp.to_sec()
-    if not first_stamp[i]:
-        # remember first timestamp to remove offset
-        first_stamp[i] = time_stamp
-    # remove offset if wanted
-    if remove_time_offset:
-        fields["stamp"] = time_stamp - first_stamp[i]
-    else:
-        fields["stamp"] = time_stamp
-
-    for data_selection in data_selections[i]:
-        f = recursive_getattr(msg.message, data_selection.split("."))
-        # extract lists in own rows
-        if seperate and (isinstance(f, list) or isinstance(f, tuple)):
-            j = 0
-            for entry in f:
-                fields[f"{data_selection}_{j}"] = entry
-                j += 1
-        else:
-            fields[data_selection] = f
-    frames[i] = frames[i].append(fields, ignore_index=True)
-
-if args.output_folder:
-    output_folder = args.output_folder
-    if not os.path.exists(output_folder):
-        os.makedirs(output_folder)
-else:
-    output_folder = "."
-
-for i, frame in enumerate(frames):
-    print(f"{COLORS.OKBLUE}{selected_topics_list[i]}{COLORS.ENDC}")
-    print(frame.info())
-    frame.to_pickle(output_folder + "/" + selected_topics_list[i][1:].replace("/", "-") + ".pickle")
-
-# save user input for next time
-input_to_save = {}
-input_to_save["topic_selections_str"] = topic_selections_str
-input_to_save["data_selections"] = data_selections
-input_to_save["seperate_input"] = seperate_input
-input_to_save["remove_remove_time_offset_input"] = remove_remove_time_offset_input
-pickle.dump(input_to_save, open(FILE_PATH, "wb"))
diff --git a/bitbots_utils/scripts/set_motor_position.py b/bitbots_utils/scripts/set_motor_position.py
deleted file mode 100755
index 447808ac..00000000
--- a/bitbots_utils/scripts/set_motor_position.py
+++ /dev/null
@@ -1,36 +0,0 @@
-#!/usr/bin/env python3
-#  -*- coding: utf8 -*-
-from math import pi
-
-import rclpy
-from rclpy.node import Node
-import time
-from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
-
-if __name__ == "__main__":
-    rclpy.init(args=None)
-    ns = rospy.get_param("ns")
-    joint_goal_publisher = self.create_publisher(JointTrajectory, ns +'/controller/command', 10, tcp_nodelay=True)
-
-    i = -1.5
-    while rclpy.ok():
-        msg = JointTrajectoryPoint()
-        msg.positions = [i, -0.4]
-        msg.time_from_start.nsecs = 10000000
-        traj_msg = JointTrajectory()
-        # make an array with String objects (ros message type)
-        traj_msg.joint_names = ['HeadPan', 'HeadTilt']
-        traj_msg.points = []
-        traj_msg.points.append(msg)
-        traj_msg.header.stamp = self.get_clock().now()
-
-        rospy.logwarn(traj_msg)
-
-        joint_goal_publisher.publish(traj_msg)
-        rospy.sleep(0.2)
-        if i < 1.5:
-            i += 0.1
-        else:
-            i = -1.5
-
-    rclpy.spin(self)
\ No newline at end of file
diff --git a/bitbots_utils/scripts/tf_delay_plot.cpp b/bitbots_utils/scripts/tf_delay_plot.cpp
new file mode 100644
index 00000000..cb9fb6b7
--- /dev/null
+++ b/bitbots_utils/scripts/tf_delay_plot.cpp
@@ -0,0 +1,54 @@
+// This script logs the delay of certain tf frames and publishes them to be visualized in plotjuggler.
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/logger.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <vector>
+#include <memory>
+#include <iterator>
+#include <chrono>
+#include <thread>
+
+using namespace std::placeholders;
+
+class TfDelayPlot : public rclcpp::Node {
+
+    public:
+        TfDelayPlot() : Node("tf_delay_plot", rclcpp::NodeOptions()) {
+            RCLCPP_INFO(this->get_logger(), "Starting tf_delay_plot");
+            // Create correct qos profile
+            tf_sub_ = this->create_subscription<tf2_msgs::msg::TFMessage>(
+                "/tf",
+                5,
+                std::bind(&TfDelayPlot::tf_callback, this, _1));
+        }
+
+        void tf_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) {
+            RCLCPP_INFO(this->get_logger(), "Got transform message");
+            for (auto &transform: msg->transforms) {
+                // Calculate delay
+                double delay = ((transform.header.stamp.sec * 1.0e9 + transform.header.stamp.nanosec) - this->now().nanoseconds()) / 1.0e9;
+
+                if (std::abs(delay) > 0.0001) {
+                    // Print
+                    RCLCPP_INFO(this->get_logger(), "Frame: %s, Delay: %f", transform.child_frame_id.c_str(), delay);
+
+                    // Print current time
+                    RCLCPP_INFO(this->get_logger(), "Current time: %f", this->now().seconds());
+
+                    // Print stamp
+                    RCLCPP_INFO(this->get_logger(), "Stamp: %f", transform.header.stamp.sec + transform.header.stamp.nanosec / 1.0e9);
+                }
+            }
+        }
+
+    private:
+        rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_;
+};
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<TfDelayPlot>());
+    rclcpp::shutdown();
+    return 0;
+}
diff --git a/bitbots_utils/src/utils.cpp b/bitbots_utils/src/utils.cpp
new file mode 100644
index 00000000..f0c653a8
--- /dev/null
+++ b/bitbots_utils/src/utils.cpp
@@ -0,0 +1,81 @@
+#include "bitbots_utils/utils.hpp"
+
+namespace bitbots_utils
+{
+
+/**
+ * @brief Waits for the transforms to be available
+ * @param logger The logger to use for logging
+ * @param clock The clock to use for time
+ * @param tf_buffer The tf buffer to use
+ * @param frames The tf frames to wait for
+ * @param root_frame The root frame to transform from
+ * @param check_interval Interval in which to check for the frames
+ * @param warn_duration Duration after which to warn if the frames are not available
+ * @param warn_interval Interval in which to keep warning if the frames are not available
+ * @param verbose Can be used to disable the warning messages
+ */
+void wait_for_tf(
+    const rclcpp::Logger &logger,
+    std::shared_ptr<rclcpp::Clock> clock,
+    std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+    const std::vector<std::string> &frames, 
+    const std::string &root_frame,
+    const rclcpp::Duration &check_interval, 
+    const rclcpp::Duration &warn_duration, 
+    const rclcpp::Duration &warn_interval, 
+    bool verbose) {
+
+  // Store the beginning time
+  auto start_time = clock->now();
+
+  // Endless loop
+  while(rclcpp::ok()) {
+    try {
+      // Check if the frame we want to transform is known yet
+      // Apply tf_buffer->_frameExists to all frames and check if all are available otherwise return false (functional)
+      // We use _frameExists because it does not spam the console with errors if the frame does not exist...
+      if(!std::all_of(frames.begin(), frames.end(), [tf_buffer](std::string frame) {return tf_buffer->_frameExists(frame);})) {
+        // Don't do busy waiting
+        rclcpp::sleep_for(check_interval.to_chrono<std::chrono::nanoseconds>());
+        // Retry
+        continue;
+      }
+
+      // Check if we can transform from the given root frame to all given frames
+      if(!std::all_of(
+        frames.begin(), 
+        frames.end(), 
+        [tf_buffer, root_frame, check_interval](const std::string frame) {
+          return tf_buffer->canTransform(
+            root_frame,
+            frame, 
+            rclcpp::Time(0), 
+            check_interval);
+        })){
+        // Here it is fine not to wait as the canTransform function already includes a timeout
+        // Retry
+        continue;
+      }
+      // We can transform to all frames, so we are done
+      return;
+    }
+    catch(const std::exception& e)
+    {
+      if (verbose) {
+        RCLCPP_ERROR(logger, "Error while waiting for transforms: %s \n", e.what());
+      }
+      rclcpp::sleep_for(check_interval.to_chrono<std::chrono::nanoseconds>());
+    }
+    
+    // Print error message if we waited too long
+    auto wait_time = clock->now() - start_time;
+    if (verbose && wait_time > warn_duration) {
+      RCLCPP_WARN_THROTTLE(logger, *clock, warn_interval.seconds(),
+                          "Waiting for transforms took longer than %f seconds", 
+                          wait_time.seconds());
+    }
+  }
+}
+
+}