diff --git a/.github/workflows/bump_version.yaml b/.github/workflows/bump_version.yaml
index 8e017111..5ec9d89c 100644
--- a/.github/workflows/bump_version.yaml
+++ b/.github/workflows/bump_version.yaml
@@ -1,64 +1,64 @@
 ---
 name: Bump version
 on:
-    workflow_dispatch:
-        inputs:
-            name:
-                description: Version to bump (major, minor, patch)
-                default: patch
-                required: true
-    pull_request:
-        branches: humble
-        types: [closed]
+  workflow_dispatch:
+    inputs:
+      name:
+        description: Version to bump (major, minor, patch)
+        default: patch
+        required: true
+  pull_request:
+    branches: humble
+    types: [closed]
 
 jobs:
-    industrial_ci:
-        name: Industrial CI
-        uses: ./.github/workflows/industrial_ci.yaml
+  industrial_ci:
+    name: Industrial CI
+    uses: ./.github/workflows/industrial_ci.yaml
 
-    get-bump:
+  get-bump:
+    name: Get version bump
+    runs-on: ubuntu-latest
+    needs: industrial_ci
+    outputs:
+      bump: ${{ env.BUMP }}
+    steps:
+      - if: github.event_name == 'pull_request' && github.event.pull_request.merged == true
         name: Get version bump
-        runs-on: ubuntu-latest
-        needs: industrial_ci
-        outputs:
-            bump: ${{ env.BUMP }}
-        steps:
-            - if: github.event_name == 'pull_request' && github.event.pull_request.merged == true
-              name: Get version bump
-              id: get-version-bump
-              uses: husarion-ci/action-get-version-bump@v0.3.0
-            - if: github.event_name == 'pull_request' && github.event.pull_request.merged == true
-              run: echo "BUMP=${{ steps.get-version-bump.outputs.bump }}" >> $GITHUB_ENV
-            - if: github.event_name == 'workflow_dispatch'
-              run: echo "BUMP=${{ github.event.inputs.name }}" >> $GITHUB_ENV
+        id: get-version-bump
+        uses: husarion-ci/action-get-version-bump@v0.3.0
+      - if: github.event_name == 'pull_request' && github.event.pull_request.merged == true
+        run: echo "BUMP=${{ steps.get-version-bump.outputs.bump }}" >> $GITHUB_ENV
+      - if: github.event_name == 'workflow_dispatch'
+        run: echo "BUMP=${{ github.event.inputs.name }}" >> $GITHUB_ENV
 
-    catkin-release:
-        name: Bump version
-        runs-on: ubuntu-latest
-        needs: get-bump
-        outputs:
-            new_version: ${{ steps.catkin-release.outputs.new_version }}
-        steps:
-            - name: Checkout
-              uses: actions/checkout@v2
-            - name: Catkin release
-              id: catkin-release
-              uses: husarion-ci/action-catkin-release@v0.1.4
-              with:
-                  bump: ${{ needs.get-bump.outputs.bump }}
-                  github_token: ${{ secrets.GH_PAT }}
-                  git_user: action-bot
-                  git_email: action-bot@action-bot.com
+  catkin-release:
+    name: Bump version
+    runs-on: ubuntu-latest
+    needs: get-bump
+    outputs:
+      new_version: ${{ steps.catkin-release.outputs.new_version }}
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v2
+      - name: Catkin release
+        id: catkin-release
+        uses: husarion-ci/action-catkin-release@v0.1.4
+        with:
+          bump: ${{ needs.get-bump.outputs.bump }}
+          github_token: ${{ secrets.GH_PAT }}
+          git_user: action-bot
+          git_email: action-bot@action-bot.com
 
-    build-and-push-docker-image:
-        name: Create new docker image
-        runs-on: ubuntu-latest
-        needs: catkin-release
-        steps:
-            - name: trigger the endpoint
-              run: >
-                  curl -X POST
-                  -H "Accept: application/vnd.github+json"
-                  -H "Authorization: Bearer ${{ secrets.GH_PAT }}"
-                  https://api.github.com/repos/husarion/rosbot-docker/dispatches
-                  -d '{"event_type":"ros-package-update"}'
+  build-docker-image:
+    name: Trigger rosbot-docker to build image
+    runs-on: ubuntu-latest
+    needs: catkin-release
+    steps:
+      - name: trigger the endpoint
+        run: >
+          curl -X POST
+          -H "Accept: application/vnd.github+json"
+          -H "Authorization: Bearer ${{ secrets.GH_PAT }}"
+          https://api.github.com/repos/husarion/rosbot-docker/dispatches
+          -d '{"event_type":"ros-package-update"}'
diff --git a/.github/workflows/industrial_ci.yaml b/.github/workflows/industrial_ci.yaml
index d1f86469..8ca7242a 100644
--- a/.github/workflows/industrial_ci.yaml
+++ b/.github/workflows/industrial_ci.yaml
@@ -1,82 +1,70 @@
 ---
 name: Industrial CI
 on:
-    workflow_call:
-    workflow_dispatch:
-    push:
+  workflow_call:
+  workflow_dispatch:
+  push:
 
 jobs:
-    black:
-        name: Black
-        runs-on: ubuntu-22.04
-        steps:
-            - name: Checkout
-              uses: actions/checkout@v3
-            - name: Black
-              uses: psf/black@23.11.0
-              with:
-                  options: --line-length=99
+  black:
+    name: Black
+    runs-on: ubuntu-22.04
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v3
+      - name: Black
+        uses: psf/black@23.11.0
+        with:
+          options: --line-length=99
 
-    spellcheck:
-        name: Spellcheck
-        runs-on: ubuntu-22.04
-        steps:
-            - name: Checkout
-              uses: actions/checkout@v3
-            - name: Spellcheck
-              uses: rojopolis/spellcheck-github-actions@0.33.1
+  spellcheck:
+    name: Spellcheck
+    runs-on: ubuntu-22.04
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v3
+      - name: Spellcheck
+        uses: rojopolis/spellcheck-github-actions@0.33.1
 
-    ros_industrial_ci:
-        name: ROS Industrial CI
-        runs-on: ubuntu-22.04
-        timeout-minutes: 30
-        needs:
-            - black
-            - spellcheck
-        strategy:
-            fail-fast: false
-            matrix:
-                ROS_DISTRO: [humble]
-        steps:
-            - name: Checkout
-              uses: actions/checkout@v3
+  ros_industrial_ci:
+    name: ROS Industrial CI
+    runs-on: ubuntu-22.04
+    timeout-minutes: 30
+    needs:
+      - black
+      - spellcheck
+    strategy:
+      fail-fast: false
+      matrix:
+        ROS_DISTRO: [humble]
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v3
 
-            - name: Act + Docker fix
-              run: |
-                  sudo chown runner:docker /var/run/docker.sock
+      - name: Act + Docker fix
+        run: |
+          sudo chown runner:docker /var/run/docker.sock
 
-            - name: Copy to src
-              run: |
-                  mkdir -p src
-                  find . -maxdepth 1 -not -name src -not -name . -exec mv {} src/ \;
+      - name: Setup ROS2 Workspace and Clone Repositories
+        run: |
+          mkdir -p src
+          find . -maxdepth 1 -not -name src -not -name . -exec mv {} src/ \;
+          python3 -m pip install -U vcstool
+          vcs import src < src/rosbot/rosbot_hardware.repos
+          vcs import src < src/rosbot/rosbot_simulation.repos
+          cp -r src/ros2_controllers/diff_drive_controller src/
+          cp -r src/ros2_controllers/imu_sensor_broadcaster src/
+          rm -rf src/ros2_controllers
 
-            - name: Clone installation requirements
-              shell: bash
-              run: |
-                  python3 -m pip install -U vcstool
-                  vcs import src < src/rosbot/rosbot_hardware.repos &&
-                  vcs import src < src/rosbot/rosbot_simulation.repos
+        # Package micro_ros_msgs does not have industrial ci and tests does not pass.
+        # For more information see https://github.com/micro-ROS/micro_ros_msgs/issues/7
+      - name: Remove tests from micro_ros_msgs
+        shell: bash
+        run: sed '/if(BUILD_TESTING)/,/endif()/d' src/micro_ros_msgs/CMakeLists.txt -i
 
-            - name: Copy only diff_drive_controller and imu_sensor_broadcaster, waits for features from ros2-control
-              shell: bash
-              run: |
-                  cp -r src/ros2_controllers/diff_drive_controller src/
-                  cp -r src/ros2_controllers/imu_sensor_broadcaster src/
-                  rm -rf src/ros2_controllers
-
-            - name: Remove ign_ros2_control demo and tests
-              shell: bash
-              run: rm -rf src/gazebosim/gz_ros2_control/ign_ros2_control_demos src/gazebosim/gz_ros2_control/gz_ros2_control_tests
-
-            # Package micro_ros_msgs does not have industrial ci and tests does not pass.
-            # For more information see https://github.com/micro-ROS/micro_ros_msgs/issues/7
-            - name: Remove tests from micro_ros_msgs
-              shell: bash
-              run: sed '/if(BUILD_TESTING)/,/endif()/d' src/micro_ros_msgs/CMakeLists.txt -i
-
-            - name: Running ROS Industrial CI
-              uses: ros-industrial/industrial_ci@master
-              env:
-                  ROS_DISTRO: ${{matrix.ROS_DISTRO}}
-                  DOCKER_IMAGE: ros:${{matrix.ROS_DISTRO}}-ros-base
-                  IMMEDIATE_TEST_OUTPUT: true
+      - name: Running ROS Industrial CI
+        uses: ros-industrial/industrial_ci@master
+        env:
+          ROS_DISTRO: ${{matrix.ROS_DISTRO}}
+          DOCKER_IMAGE: ros:${{matrix.ROS_DISTRO}}-ros-base
+          IMMEDIATE_TEST_OUTPUT: true
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 673c0755..8b997ed4 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -1,51 +1,53 @@
 ---
 repos:
-    - repo: https://github.com/pre-commit/pre-commit-hooks
-      rev: v4.5.0
-      hooks:
-          - id: check-merge-conflict
-          - id: trailing-whitespace
-          - id: end-of-file-fixer
-          - id: check-yaml
-          - id: check-xml
-          - id: check-added-large-files
-          - id: check-ast
-          - id: check-json
-          - id: name-tests-test
-            files: ^.*\/test\/.*$
-            args: [--pytest-test-first]
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.5.0
+    hooks:
+      - id: check-merge-conflict
+      - id: trailing-whitespace
+      - id: end-of-file-fixer
+      - id: check-yaml
+      - id: check-xml
+      - id: check-added-large-files
+      - id: check-ast
+      - id: check-json
+      - id: name-tests-test
+        files: ^.*\/test\/.*$
+        args: [--pytest-test-first]
 
-    - repo: https://github.com/codespell-project/codespell
-      rev: v2.2.6
-      hooks:
-          - id: codespell
-            name: codespell
-            description: Checks for common misspellings in text files.
-            entry: codespell  *
-            language: python
-            types: [text]
+  - repo: https://github.com/codespell-project/codespell
+    rev: v2.2.6
+    hooks:
+      - id: codespell
+        name: codespell
+        description: Checks for common misspellings in text files.
+        entry: codespell  *
+        language: python
+        types: [text]
 
-    - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt
-      rev: 0.2.3
-      hooks:
-          - id: yamlfmt
-            files: ^.github|./\.yaml
+  - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt
+    rev: 0.2.3
+    hooks:
+      - id: yamlfmt
+        files: ^(?!.*compose)(?!.*ekf\.yaml$).*$
+        args: [--mapping, '2', --sequence, '4', --offset, '2', --width, '100']
 
-    - repo: https://github.com/psf/black
-      rev: 23.11.0
-      hooks:
-        - id: black
-          args: ["--line-length=99", "--experimental-string-processing"]
+  # Please keep this version until --experimental-string-processing come back or will be available by default
+  - repo: https://github.com/psf/black
+    rev: 23.11.0
+    hooks:
+      - id: black
+        args: [--line-length=99, --experimental-string-processing]
 
-    - repo: https://github.com/PyCQA/flake8
-      rev: 6.1.0
-      hooks:
-        - id: flake8
-          args: ["--ignore=E501,W503"] # ignore too long line and line break before binary operator,
+  - repo: https://github.com/PyCQA/flake8
+    rev: 7.0.0
+    hooks:
+      - id: flake8
+        args: ['--ignore=E501,W503']   # ignore too long line and line break before binary operator,
                                        # black checks it
 
-    - repo: local
-      hooks:
+  - repo: local
+    hooks:
       - id: ament_lint_cmake
         name: ament_lint_cmake
         description: Check format of CMakeLists.txt files.
@@ -53,8 +55,8 @@ repos:
         language: system
         files: CMakeLists\.txt$
 
-    - repo: local
-      hooks:
+  - repo: local
+    hooks:
       - id: ament_copyright
         name: ament_copyright
         description: Check if copyright notice is available in all files.
@@ -63,9 +65,9 @@ repos:
         language: system
 
       # Docs - RestructuredText hooks
-    - repo: https://github.com/PyCQA/doc8
-      rev: v1.1.1
-      hooks:
-        - id: doc8
-          args: ['--max-line-length=100', '--ignore=D001']
-          exclude: ^.*\/CHANGELOG\.rst/.*$
+  - repo: https://github.com/PyCQA/doc8
+    rev: v1.1.1
+    hooks:
+      - id: doc8
+        args: [--max-line-length=100, --ignore=D001]
+        exclude: ^.*\/CHANGELOG\.rst/.*$
diff --git a/.pyspelling.yaml b/.pyspelling.yaml
index df8259ca..ae490993 100644
--- a/.pyspelling.yaml
+++ b/.pyspelling.yaml
@@ -1,53 +1,54 @@
+---
 matrix:
-- name: Python Source
-  aspell:
-    lang: en
-    d: en_US
-    camel-case: true
-  sources:
-  - 'rosbot*/**/*.py'
+  - name: Python Source
+    aspell:
+      lang: en
+      d: en_US
+      camel-case: true
+    sources:
+      - rosbot*/**/*.py
 
-  dictionary:
-    encoding: utf-8
-    output: wordlist.dic
-    wordlists:
-    - .wordlist.txt
+    dictionary:
+      encoding: utf-8
+      output: wordlist.dic
+      wordlists:
+        - .wordlist.txt
 
-  pipeline:
-  - pyspelling.filters.python:
-      comments: true
+    pipeline:
+      - pyspelling.filters.python:
+          comments: true
 
-- name: Markdown sources
-  aspell:
-    lang: en
-    d: en_US
-    camel-case: true
-  sources:
-  - 'rosbot*/**/*.md'
-  - 'rosbot*/**/*.txt'
-  dictionary:
-    encoding: utf-8
-    output: wordlist.dic
-    wordlists:
-    - .wordlist.txt
+  - name: Markdown sources
+    aspell:
+      lang: en
+      d: en_US
+      camel-case: true
+    sources:
+      - rosbot*/**/*.md
+      - rosbot*/**/*.txt
+    dictionary:
+      encoding: utf-8
+      output: wordlist.dic
+      wordlists:
+        - .wordlist.txt
 
-  pipeline:
-  - pyspelling.filters.text
+    pipeline:
+      - pyspelling.filters.text
 
-- name: XML sources
-  aspell:
-    lang: en
-    d: en_US
-    camel-case: true
-  sources:
-  - 'rosbot*/**/*.xacro'
-  - 'rosbot*/**/*.urdf'
-  - 'rosbot*/**/*.xml'
-  dictionary:
-    encoding: utf-8
-    output: wordlist.dic
-    wordlists:
-    - .wordlist.txt
+  - name: XML sources
+    aspell:
+      lang: en
+      d: en_US
+      camel-case: true
+    sources:
+      - rosbot*/**/*.xacro
+      - rosbot*/**/*.urdf
+      - rosbot*/**/*.xml
+    dictionary:
+      encoding: utf-8
+      output: wordlist.dic
+      wordlists:
+        - .wordlist.txt
 
-  pipeline:
-  - pyspelling.filters.xml
+    pipeline:
+      - pyspelling.filters.xml
diff --git a/.wordlist.txt b/.wordlist.txt
index cfa4a43c..35d0089a 100644
--- a/.wordlist.txt
+++ b/.wordlist.txt
@@ -122,7 +122,7 @@ pids
 pgrep
 namespace
 TODO
-delipl
+delihus
 microros
 namespaces
 fastdds
@@ -150,3 +150,4 @@ subprocess
 cbus
 Dockerfile
 unbuffered
+sp
diff --git a/README.md b/README.md
index 5ab328d1..4005723d 100644
--- a/README.md
+++ b/README.md
@@ -100,9 +100,6 @@ vcs import src < src/rosbot/rosbot_simulation.repos
 # Build only diff_drive_controller and imu_sensor_broadcaster from ros2_controllers
 cp -r src/ros2_controllers/diff_drive_controller src && cp -r src/ros2_controllers/imu_sensor_broadcaster src && rm -rf src/ros2_controllers
 
-# Remove ign_ros2_control demo and test
-rm -rf src/gazebosim/gz_ros2_control/ign_ros2_control_demos src/gazebosim/gz_ros2_control/gz_ros2_control_tests
-
 sudo rosdep init
 rosdep update --rosdistro $ROS_DISTRO
 rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y
diff --git a/rosbot/rosbot_simulation.repos b/rosbot/rosbot_simulation.repos
index 40ba729f..e4d80df2 100644
--- a/rosbot/rosbot_simulation.repos
+++ b/rosbot/rosbot_simulation.repos
@@ -1,9 +1,4 @@
 repositories:
-  gazebosim/gz_ros2_control:
-    type: git
-    url: https://github.com/ros-controls/gz_ros2_control
-    version: humble
-
   husarion/husarion_office_gz:
     type: git
     url: https://github.com/husarion/husarion_office_gz
diff --git a/rosbot_bringup/launch/bringup.launch.py b/rosbot_bringup/launch/bringup.launch.py
index 4785a804..71c70fc9 100644
--- a/rosbot_bringup/launch/bringup.launch.py
+++ b/rosbot_bringup/launch/bringup.launch.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_bringup/launch/combined.launch.py b/rosbot_bringup/launch/combined.launch.py
index af39ddb7..bb893ae5 100644
--- a/rosbot_bringup/launch/combined.launch.py
+++ b/rosbot_bringup/launch/combined.launch.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_bringup/setup.py b/rosbot_bringup/setup.py
index bf0db8eb..f831e3ad 100644
--- a/rosbot_bringup/setup.py
+++ b/rosbot_bringup/setup.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_bringup/test/test_diff_drive_ekf.py b/rosbot_bringup/test/test_diff_drive_ekf.py
index 5da7a3b1..8d92a76b 100644
--- a/rosbot_bringup/test/test_diff_drive_ekf.py
+++ b/rosbot_bringup/test/test_diff_drive_ekf.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_bringup/test/test_mecanum_ekf.py b/rosbot_bringup/test/test_mecanum_ekf.py
index fcdfc633..ff0929ad 100644
--- a/rosbot_bringup/test/test_mecanum_ekf.py
+++ b/rosbot_bringup/test/test_mecanum_ekf.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_bringup/test/test_multirobot_ekf.py b/rosbot_bringup/test/test_multirobot_ekf.py
index 57ae7a6c..13e58bb0 100644
--- a/rosbot_bringup/test/test_multirobot_ekf.py
+++ b/rosbot_bringup/test/test_multirobot_ekf.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py b/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py
index f78629a7..aa9e4cdd 100644
--- a/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py
+++ b/rosbot_bringup/test/test_namespaced_diff_drive_ekf.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_bringup/test/test_namespaced_mecanum_ekf.py b/rosbot_bringup/test/test_namespaced_mecanum_ekf.py
index a49b8faf..579b0454 100644
--- a/rosbot_bringup/test/test_namespaced_mecanum_ekf.py
+++ b/rosbot_bringup/test/test_namespaced_mecanum_ekf.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_bringup/test/test_utils.py b/rosbot_bringup/test/test_utils.py
index cc483be1..517f4757 100644
--- a/rosbot_bringup/test/test_utils.py
+++ b/rosbot_bringup/test/test_utils.py
@@ -1,5 +1,5 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_controller/config/diff_drive_controller.yaml b/rosbot_controller/config/diff_drive_controller.yaml
index df7a0af4..cd723b26 100644
--- a/rosbot_controller/config/diff_drive_controller.yaml
+++ b/rosbot_controller/config/diff_drive_controller.yaml
@@ -1,3 +1,4 @@
+---
 /**/controller_manager:
   ros__parameters:
     update_rate: 20 # Hz
@@ -24,8 +25,8 @@
   ros__parameters:
     tf_frame_prefix_enable: false
 
-    left_wheel_names: ["fl_wheel_joint", "rl_wheel_joint"]
-    right_wheel_names: ["fr_wheel_joint", "rr_wheel_joint"]
+    left_wheel_names: [fl_wheel_joint, rl_wheel_joint]
+    right_wheel_names: [fr_wheel_joint, rr_wheel_joint]
 
     wheel_separation: 0.186
     wheel_radius: 0.0425
diff --git a/rosbot_controller/config/mecanum_drive_controller.yaml b/rosbot_controller/config/mecanum_drive_controller.yaml
index 4ee7d87e..4dc3691b 100644
--- a/rosbot_controller/config/mecanum_drive_controller.yaml
+++ b/rosbot_controller/config/mecanum_drive_controller.yaml
@@ -1,3 +1,4 @@
+---
 /**/controller_manager:
   ros__parameters:
     update_rate: 20 # Hz
diff --git a/rosbot_controller/launch/controller.launch.py b/rosbot_controller/launch/controller.launch.py
index e94a3b9a..ca1596ea 100644
--- a/rosbot_controller/launch/controller.launch.py
+++ b/rosbot_controller/launch/controller.launch.py
@@ -1,5 +1,5 @@
 # Copyright 2020 ros2_control Development Team
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_controller/setup.py b/rosbot_controller/setup.py
index 3812e353..ddeb0003 100644
--- a/rosbot_controller/setup.py
+++ b/rosbot_controller/setup.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_controller/test/test_diff_drive_controllers.py b/rosbot_controller/test/test_diff_drive_controllers.py
index 9a9aa6fb..27f00229 100644
--- a/rosbot_controller/test/test_diff_drive_controllers.py
+++ b/rosbot_controller/test/test_diff_drive_controllers.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_controller/test/test_mecanum_controllers.py b/rosbot_controller/test/test_mecanum_controllers.py
index 9b8aa53c..4ca0dbd0 100644
--- a/rosbot_controller/test/test_mecanum_controllers.py
+++ b/rosbot_controller/test/test_mecanum_controllers.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_controller/test/test_multirobot_controllers.py b/rosbot_controller/test/test_multirobot_controllers.py
index 0b2e9084..8fe87646 100644
--- a/rosbot_controller/test/test_multirobot_controllers.py
+++ b/rosbot_controller/test/test_multirobot_controllers.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -18,9 +18,10 @@
 import pytest
 import rclpy
 
+
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import IncludeLaunchDescription, TimerAction
 from launch.substitutions import PathJoinSubstitution
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from test_utils import ControllersTestNode, controller_readings_test
@@ -46,12 +47,13 @@ def generate_test_description():
             launch_arguments={
                 "use_sim": "False",
                 "mecanum": "True",
-                "use_gpu": "False",
                 "namespace": robot_names[i],
             }.items(),
         )
 
-        actions.append(controller_launch)
+        # When there is no delay the controllers doesn't spawn correctly
+        delayed_controller_launch = TimerAction(period=i * 2.0, actions=[controller_launch])
+        actions.append(delayed_controller_launch)
 
     return LaunchDescription(actions)
 
@@ -66,7 +68,6 @@ def test_multirobot_controllers_startup_success():
             node.start_publishing_fake_hardware()
 
             node.start_node_thread()
-
             controller_readings_test(node, robot_name)
         finally:
             rclpy.shutdown()
diff --git a/rosbot_controller/test/test_namespaced_diff_drive_controllers.py b/rosbot_controller/test/test_namespaced_diff_drive_controllers.py
index 7371ab7d..4b18a058 100644
--- a/rosbot_controller/test/test_namespaced_diff_drive_controllers.py
+++ b/rosbot_controller/test/test_namespaced_diff_drive_controllers.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_controller/test/test_namespaced_mecanum_controllers.py b/rosbot_controller/test/test_namespaced_mecanum_controllers.py
index d1ccd623..68a25bc6 100644
--- a/rosbot_controller/test/test_namespaced_mecanum_controllers.py
+++ b/rosbot_controller/test/test_namespaced_mecanum_controllers.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_controller/test/test_utils.py b/rosbot_controller/test/test_utils.py
index f5fdd34f..4b10dc86 100644
--- a/rosbot_controller/test/test_utils.py
+++ b/rosbot_controller/test/test_utils.py
@@ -1,5 +1,5 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -17,9 +17,7 @@
 
 from threading import Event
 from threading import Thread
-
 from rclpy.node import Node
-
 from sensor_msgs.msg import JointState, Imu
 from nav_msgs.msg import Odometry
 
@@ -46,10 +44,10 @@ def create_test_subscribers_and_publishers(self):
 
         self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10)
 
-        # TODO: @delipl namespaces have not been implemented in microros yet
-        self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10)
+        # TODO: @delihus namespaces have not been implemented in microros yet
+        self.imu_pub = self.create_publisher(Imu, "/_imu/data_raw", 10)
 
-        self.joint_states_publisher = self.create_publisher(JointState, "/_motors_response", 10)
+        self.joint_pub = self.create_publisher(JointState, "/_motors_response", 10)
 
         self.timer = None
 
@@ -72,7 +70,7 @@ def start_publishing_fake_hardware(self):
             self.publish_fake_hardware_messages,
         )
 
-    # TODO: @delipl namespaces have not been implemented in microros yet
+    # TODO: @delihus namespaces have not been implemented in microros yet
     def publish_fake_hardware_messages(self):
         imu_msg = Imu()
         imu_msg.header.stamp = self.get_clock().now().to_msg()
@@ -89,22 +87,22 @@ def publish_fake_hardware_messages(self):
         joint_state_msg.position = [0.0, 0.0, 0.0, 0.0]
         joint_state_msg.velocity = [0.0, 0.0, 0.0, 0.0]
 
-        self.imu_publisher.publish(imu_msg)
-        self.joint_states_publisher.publish(joint_state_msg)
+        self.imu_pub.publish(imu_msg)
+        self.joint_pub.publish(joint_state_msg)
 
 
 def controller_readings_test(node, robot_name="ROSbot"):
-    msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0)
+    msgs_received_flag = node.joint_state_msg_event.wait(10.0)
     assert msgs_received_flag, (
-        f"{robot_name}: expected JointStates message but it was not received. Check"
-        " joint_state_broadcaster!"
+        f"{robot_name}: Expected JointStates message but it was not received. Check "
+        "joint_state_broadcaster!"
     )
-    msgs_received_flag = node.odom_msg_event.wait(timeout=10.0)
+    msgs_received_flag = node.odom_msg_event.wait(10.0)
     assert msgs_received_flag, (
-        f"{robot_name}: expected Odom message but it was not received. Check"
-        " rosbot_base_controller!"
+        f"{robot_name}: Expected Odom message but it was not received. Check "
+        "rosbot_base_controller!"
     )
-    msgs_received_flag = node.imu_msg_event.wait(timeout=10.0)
+    msgs_received_flag = node.imu_msg_event.wait(10.0)
     assert (
         msgs_received_flag
-    ), f"{robot_name}: expected Imu message but it was not received. Check imu_broadcaster!"
+    ), f"{robot_name}: Expected Imu message but it was not received. Check imu_broadcaster!"
diff --git a/rosbot_controller/test/test_xacro.py b/rosbot_controller/test/test_xacro.py
index 0e52cfe6..b78e2032 100644
--- a/rosbot_controller/test/test_xacro.py
+++ b/rosbot_controller/test/test_xacro.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_gazebo/config/gz_remappings.yaml b/rosbot_gazebo/config/gz_remappings.yaml
index 6f7a6730..550cc139 100644
--- a/rosbot_gazebo/config/gz_remappings.yaml
+++ b/rosbot_gazebo/config/gz_remappings.yaml
@@ -1,40 +1,51 @@
-- topic_name: "/clock"
-  ros_type_name: "rosgraph_msgs/msg/Clock"
-  gz_type_name: "ignition.msgs.Clock"
-  direction: GZ_TO_ROS
-
-- topic_name: "<robot_namespace>/scan"
-  ros_type_name: "sensor_msgs/msg/LaserScan"
-  gz_type_name: "ignition.msgs.LaserScan"
-
-- topic_name: "<robot_namespace>/camera/camera_info"
-  ros_type_name: "sensor_msgs/msg/CameraInfo"
-  gz_type_name: "ignition.msgs.CameraInfo"
-
-- topic_name: "<robot_namespace>/camera/depth_image"
-  ros_type_name: "sensor_msgs/msg/Image"
-  gz_type_name: "ignition.msgs.Image"
-
-- topic_name: "<robot_namespace>/camera/image"
-  ros_type_name: "sensor_msgs/msg/Image"
-  gz_type_name: "ignition.msgs.Image"
-
-- topic_name: "<robot_namespace>/camera/points"
-  ros_type_name: "sensor_msgs/msg/PointCloud2"
-  gz_type_name: "ignition.msgs.PointCloudPacked"
-
-- topic_name: "<robot_namespace>/range/fl"
-  ros_type_name: "sensor_msgs/msg/LaserScan"
-  gz_type_name: "ignition.msgs.LaserScan"
-
-- topic_name: "<robot_namespace>/range/fr"
-  ros_type_name: "sensor_msgs/msg/LaserScan"
-  gz_type_name: "ignition.msgs.LaserScan"
-
-- topic_name: "<robot_namespace>/range/rl"
-  ros_type_name: "sensor_msgs/msg/LaserScan"
-  gz_type_name: "ignition.msgs.LaserScan"
-
-- topic_name: "<robot_namespace>/range/rr"
-  ros_type_name: "sensor_msgs/msg/LaserScan"
-  gz_type_name: "ignition.msgs.LaserScan"
+---
+  - topic_name: /clock
+    ros_type_name: rosgraph_msgs/msg/Clock
+    gz_type_name: ignition.msgs.Clock
+    direction: GZ_TO_ROS
+
+  - topic_name: <robot_namespace>/scan
+    ros_type_name: sensor_msgs/msg/LaserScan
+    gz_type_name: ignition.msgs.LaserScan
+
+  - topic_name: <robot_namespace>/camera/color/camera_info
+    ros_type_name: sensor_msgs/msg/CameraInfo
+    gz_type_name: ignition.msgs.CameraInfo
+    lazy: true
+
+  - topic_name: <robot_namespace>/camera/color/image_raw
+    ros_type_name: sensor_msgs/msg/Image
+    gz_type_name: ignition.msgs.Image
+    lazy: true
+
+  - topic_name: <robot_namespace>/camera/depth/camera_info
+    ros_type_name: sensor_msgs/msg/CameraInfo
+    gz_type_name: ignition.msgs.CameraInfo
+    lazy: true
+
+  - topic_name: <robot_namespace>/camera/depth/image_raw
+    ros_type_name: sensor_msgs/msg/Image
+    gz_type_name: ignition.msgs.Image
+    lazy: true
+
+  - ros_topic_name: <robot_namespace>/camera/depth/points
+    gz_topic_name: <robot_namespace>/camera/depth/image_raw/points
+    ros_type_name: sensor_msgs/msg/PointCloud2
+    gz_type_name: ignition.msgs.PointCloudPacked
+    lazy: true
+
+  - topic_name: <robot_namespace>/range/fl
+    ros_type_name: sensor_msgs/msg/LaserScan
+    gz_type_name: ignition.msgs.LaserScan
+
+  - topic_name: <robot_namespace>/range/fr
+    ros_type_name: sensor_msgs/msg/LaserScan
+    gz_type_name: ignition.msgs.LaserScan
+
+  - topic_name: <robot_namespace>/range/rl
+    ros_type_name: sensor_msgs/msg/LaserScan
+    gz_type_name: ignition.msgs.LaserScan
+
+  - topic_name: <robot_namespace>/range/rr
+    ros_type_name: sensor_msgs/msg/LaserScan
+    gz_type_name: ignition.msgs.LaserScan
diff --git a/rosbot_gazebo/launch/simulation.launch.py b/rosbot_gazebo/launch/simulation.launch.py
index 962d9517..f4ecccf5 100644
--- a/rosbot_gazebo/launch/simulation.launch.py
+++ b/rosbot_gazebo/launch/simulation.launch.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py
index e9c92eec..a39b617a 100644
--- a/rosbot_gazebo/launch/spawn.launch.py
+++ b/rosbot_gazebo/launch/spawn.launch.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_gazebo/setup.py b/rosbot_gazebo/setup.py
index 26443e75..bddf71c3 100644
--- a/rosbot_gazebo/setup.py
+++ b/rosbot_gazebo/setup.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py
index 130b1fec..d2c86a8b 100644
--- a/rosbot_gazebo/test/test_diff_drive_simulation.py
+++ b/rosbot_gazebo/test/test_diff_drive_simulation.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -17,7 +17,7 @@
 import launch_pytest
 import pytest
 import rclpy
-import os
+
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
@@ -26,17 +26,13 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_testing.actions import ReadyToTest
 from launch_testing.util import KeepAliveProc
-
-from test_utils import SimulationTestNode, tf_test, diff_test
+from test_utils import SimulationTestNode, diff_test
 from test_ign_kill_utils import kill_ign_linux_processes
+from threading import Thread
 
 
 @launch_pytest.fixture
 def generate_test_description():
-    # This is necessary to get unbuffered output from the process under test
-    proc_env = os.environ.copy()
-    proc_env["PYTHONUNBUFFERED"] = "1"
-
     rosbot_gazebo = get_package_share_directory("rosbot_gazebo")
     simulation_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -74,11 +70,9 @@ def generate_test_description():
 def test_diff_drive_simulation():
     rclpy.init()
     try:
-        node = SimulationTestNode("test_bringup")
-        node.create_test_subscribers_and_publishers()
-        node.start_node_thread()
+        node = SimulationTestNode("test_diff_drive_simulation")
+        Thread(target=lambda node: rclpy.spin(node), args=(node,)).start()
 
-        tf_test(node)
         diff_test(node)
 
     finally:
diff --git a/rosbot_gazebo/test/test_ign_kill_utils.py b/rosbot_gazebo/test/test_ign_kill_utils.py
index c961d22d..a0bcbcdc 100644
--- a/rosbot_gazebo/test/test_ign_kill_utils.py
+++ b/rosbot_gazebo/test/test_ign_kill_utils.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 # Copyright 2023 Open Source Robotics Foundation, Inc.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/rosbot_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py
index eed30286..24453ca3 100644
--- a/rosbot_gazebo/test/test_mecanum_simulation.py
+++ b/rosbot_gazebo/test/test_mecanum_simulation.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -17,7 +17,7 @@
 import launch_pytest
 import pytest
 import rclpy
-import os
+
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
@@ -26,17 +26,13 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_testing.actions import ReadyToTest
 from launch_testing.util import KeepAliveProc
-
-from test_utils import SimulationTestNode, tf_test, mecanum_test
+from test_utils import SimulationTestNode, mecanum_test
 from test_ign_kill_utils import kill_ign_linux_processes
+from threading import Thread
 
 
 @launch_pytest.fixture
 def generate_test_description():
-    # This is necessary to get unbuffered output from the process under test
-    proc_env = os.environ.copy()
-    proc_env["PYTHONUNBUFFERED"] = "1"
-
     rosbot_gazebo = get_package_share_directory("rosbot_gazebo")
     simulation_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -75,11 +71,9 @@ def generate_test_description():
 def test_mecanum_simulation():
     rclpy.init()
     try:
-        node = SimulationTestNode("test_bringup")
-        node.create_test_subscribers_and_publishers()
-        node.start_node_thread()
+        node = SimulationTestNode("test_mecanum_simulation")
+        Thread(target=lambda node: rclpy.spin(node), args=(node,)).start()
 
-        tf_test(node)
         mecanum_test(node)
 
     finally:
diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py
index 6894e8ab..652d51dc 100644
--- a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py
+++ b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -17,27 +17,20 @@
 import launch_pytest
 import pytest
 import rclpy
-import os
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import ExecuteProcess
 from launch_testing.actions import ReadyToTest
 from launch_testing.util import KeepAliveProc
-from rclpy.executors import MultiThreadedExecutor
-from threading import Thread
-
+from rclpy.executors import SingleThreadedExecutor
 from test_utils import SimulationTestNode, diff_test
-
 from test_ign_kill_utils import kill_ign_linux_processes
+from threading import Thread
 
 
 @launch_pytest.fixture
 def generate_test_description():
-    # This is necessary to get unbuffered output from the process under test
-    proc_env = os.environ.copy()
-    proc_env["PYTHONUNBUFFERED"] = "1"
-
     # IncludeLaunchDescription does not work with robots argument
     simulation_launch = ExecuteProcess(
         cmd=[
@@ -49,7 +42,7 @@ def generate_test_description():
                 f'world:={get_package_share_directory("husarion_office_gz")}'
                 "/worlds/empty_with_plugins.sdf"
             ),
-            "robots:=robot1={y: -4.0}; robot2={y: 0.0}; robot3={y: 4.0};",
+            "robots:=robot1={y: -4.0}; robot2={y: 0.0};",
             "headless:=True",
         ],
         output="screen",
@@ -66,31 +59,31 @@ def generate_test_description():
 
 
 @pytest.mark.launch(fixture=generate_test_description)
-def test_multirobot_simulation():
-    robot_names = ["robot1", "robot2", "robot3"]
+def test_multirobot_diff_drive_simulation():
+    robots = ["robot1", "robot2"]
     rclpy.init()
     try:
         nodes = {}
-        executor = MultiThreadedExecutor(num_threads=len(robot_names))
+        executor = SingleThreadedExecutor()
 
-        for robot_name in robot_names:
-            node = SimulationTestNode("test_simulation", namespace=robot_name)
-            node.create_test_subscribers_and_publishers()
-            nodes[robot_name] = node
+        for node_namespace in robots:
+            node = SimulationTestNode(
+                "test_multirobot_diff_drive_simulation", namespace=node_namespace
+            )
+            nodes[node_namespace] = node
             executor.add_node(node)
 
-        ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,))
-        ros_spin_thread.start()
-
-        for robot_name in robot_names:
-            node = nodes[robot_name]
-            diff_test(node, robot_name)
+        Thread(target=lambda executor: executor.spin(), args=(executor,)).start()
 
+        for node_namespace in robots:
+            node = nodes[node_namespace]
+            diff_test(node, node_namespace)
+            executor.remove_node(node)
             node.destroy_node()
 
     finally:
-        rclpy.shutdown()
-
         # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching
         # several tests in a row.
+        executor.shutdown()
         kill_ign_linux_processes()
+        rclpy.shutdown()
diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py
index 65d76431..1a18ce22 100644
--- a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py
+++ b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -17,26 +17,20 @@
 import launch_pytest
 import pytest
 import rclpy
-import os
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import ExecuteProcess
 from launch_testing.actions import ReadyToTest
 from launch_testing.util import KeepAliveProc
-from rclpy.executors import MultiThreadedExecutor
-from threading import Thread
-
+from rclpy.executors import SingleThreadedExecutor
 from test_utils import SimulationTestNode, mecanum_test
 from test_ign_kill_utils import kill_ign_linux_processes
+from threading import Thread
 
 
 @launch_pytest.fixture
 def generate_test_description():
-    # This is necessary to get unbuffered output from the process under test
-    proc_env = os.environ.copy()
-    proc_env["PYTHONUNBUFFERED"] = "1"
-
     # IncludeLaunchDescription does not work with robots argument
     simulation_launch = ExecuteProcess(
         cmd=[
@@ -48,9 +42,9 @@ def generate_test_description():
                 f'world:={get_package_share_directory("husarion_office_gz")}'
                 "/worlds/empty_with_plugins.sdf"
             ),
-            "robots:=robot1={y: -4.0}; robot2={y: 0.0}; robot3={y: 4.0};",
-            "headless:=True",
+            "robots:=robot1={y: -4.0}; robot2={y: 0.0};",
             "mecanum:=True",
+            "headless:=True",
         ],
         output="screen",
     )
@@ -67,31 +61,30 @@ def generate_test_description():
 
 @pytest.mark.launch(fixture=generate_test_description)
 def test_multirobot_mecanum_simulation():
-    robot_names = ["robot1", "robot2", "robot3"]
+    robots = ["robot1", "robot2"]
     rclpy.init()
     try:
         nodes = {}
-        executor = MultiThreadedExecutor(num_threads=len(robot_names))
+        executor = SingleThreadedExecutor()
 
-        for robot_name in robot_names:
-            node = SimulationTestNode("test_simulation", namespace=robot_name)
-            node.create_test_subscribers_and_publishers()
-            nodes[robot_name] = node
+        for node_namespace in robots:
+            node = SimulationTestNode(
+                "test_multirobot_mecanum_simulation", namespace=node_namespace
+            )
+            nodes[node_namespace] = node
             executor.add_node(node)
 
-        ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,))
-        ros_spin_thread.start()
-
-        # Speed test
-        for robot_name in robot_names:
-            node = nodes[robot_name]
-            mecanum_test(node, robot_name)
+        Thread(target=lambda executor: executor.spin(), args=(executor,)).start()
 
+        for node_namespace in robots:
+            node = nodes[node_namespace]
+            mecanum_test(node, node_namespace)
+            executor.remove_node(node)
             node.destroy_node()
 
     finally:
-        rclpy.shutdown()
-
         # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching
         # several tests in a row.
+        executor.shutdown()
         kill_ign_linux_processes()
+        rclpy.shutdown()
diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py
index e4a15ff0..1ce13aa4 100644
--- a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py
+++ b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -17,7 +17,7 @@
 import launch_pytest
 import pytest
 import rclpy
-import os
+
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
@@ -26,17 +26,13 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_testing.actions import ReadyToTest
 from launch_testing.util import KeepAliveProc
-
-from test_utils import SimulationTestNode, tf_test, diff_test
+from test_utils import SimulationTestNode, diff_test
 from test_ign_kill_utils import kill_ign_linux_processes
+from threading import Thread
 
 
 @launch_pytest.fixture
 def generate_test_description():
-    # This is necessary to get unbuffered output from the process under test
-    proc_env = os.environ.copy()
-    proc_env["PYTHONUNBUFFERED"] = "1"
-
     rosbot_gazebo = get_package_share_directory("rosbot_gazebo")
     simulation_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -75,11 +71,9 @@ def generate_test_description():
 def test_namespaced_diff_drive_simulation():
     rclpy.init()
     try:
-        node = SimulationTestNode("test_simulation", namespace="rosbot2r")
-        node.create_test_subscribers_and_publishers()
-        node.start_node_thread()
+        node = SimulationTestNode("test_namespaced_diff_drive_simulation", namespace="rosbot2r")
+        Thread(target=lambda node: rclpy.spin(node), args=(node,)).start()
 
-        tf_test(node)
         diff_test(node)
 
     finally:
diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py
index 138fb47d..08d778d0 100644
--- a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py
+++ b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py
@@ -1,6 +1,6 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
 # Copyright 2023 Intel Corporation. All Rights Reserved.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -17,7 +17,6 @@
 import launch_pytest
 import pytest
 import rclpy
-import os
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
@@ -26,17 +25,13 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_testing.actions import ReadyToTest
 from launch_testing.util import KeepAliveProc
-
-from test_utils import SimulationTestNode, tf_test, mecanum_test
+from test_utils import SimulationTestNode, mecanum_test
 from test_ign_kill_utils import kill_ign_linux_processes
+from threading import Thread
 
 
 @launch_pytest.fixture
 def generate_test_description():
-    # This is necessary to get unbuffered output from the process under test
-    proc_env = os.environ.copy()
-    proc_env["PYTHONUNBUFFERED"] = "1"
-
     rosbot_gazebo = get_package_share_directory("rosbot_gazebo")
     simulation_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -76,11 +71,9 @@ def generate_test_description():
 def test_namespaced_mecanum_simulation():
     rclpy.init()
     try:
-        node = SimulationTestNode("test_simulation", namespace="rosbot2r")
-        node.create_test_subscribers_and_publishers()
-        node.start_node_thread()
+        node = SimulationTestNode("test_namespaced_mecanum_simulation", namespace="rosbot2r")
+        Thread(target=lambda node: rclpy.spin(node), args=(node,)).start()
 
-        tf_test(node)
         mecanum_test(node)
 
     finally:
diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py
index 8457dfc5..e939caca 100644
--- a/rosbot_gazebo/test/test_utils.py
+++ b/rosbot_gazebo/test/test_utils.py
@@ -1,5 +1,5 @@
 # Copyright 2021 Open Source Robotics Foundation, Inc.
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -14,94 +14,103 @@
 # limitations under the License.
 
 import rclpy
+from rclpy.node import Node
 
 from threading import Event
-from threading import Thread
-
-from rclpy.node import Node
 
 from geometry_msgs.msg import Twist
 from nav_msgs.msg import Odometry
-from sensor_msgs.msg import LaserScan, Image, PointCloud2
+from sensor_msgs.msg import LaserScan, Image, Imu, JointState, PointCloud2
 
 
 class SimulationTestNode(Node):
     __test__ = False
+
     # The inaccuracies in measurement uncertainties and wheel slippage
     # cause the rosbot_base_controller to determine inaccurate odometry.
     ACCURACY = 0.10  # 10% accuracy
+    VELOCITY_STABILIZATION_DELAY = 2
 
     RANGE_SENSORS_TOPICS = ["range/fl", "range/fr", "range/rl", "range/rr"]
     RANGE_SENSORS_FRAMES = ["fl_range", "fr_range", "rl_range", "rr_range"]
 
     def __init__(self, name="test_node", namespace=None):
         super().__init__(name, namespace=namespace)
+        self.cmd_vel_pub = self.create_publisher(Twist, "cmd_vel", 10)
 
-        # Use simulation time to correct run on slow machine
-        use_sim_time = rclpy.parameter.Parameter("use_sim_time", rclpy.Parameter.Type.BOOL, True)
-        self.set_parameters([use_sim_time])
+        # Robot callback
+        self.joint_sub = self.create_subscription(
+            JointState, "joint_states", self.joint_states_callback, 10
+        )
+        self.controller_sub = self.create_subscription(
+            Odometry, "rosbot_base_controller/odom", self.controller_callback, 10
+        )
+        self.ekf_sub = self.create_subscription(
+            Odometry, "odometry/filtered", self.ekf_callback, 10
+        )
+        self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10)
 
-        self.VELOCITY_STABILIZATION_DELAY = 3
-        self.current_time = 1e-9 * self.get_clock().now().nanoseconds
-        self.goal_received_time = 1e-9 * self.get_clock().now().nanoseconds
-        self.vel_stabilization_time_event = Event()
+        # Sensor callback
+        self.camera_rgb_sub = self.create_subscription(
+            Image, "camera/color/image_raw", self.camera_image_callback, 10
+        )
+        self.camera_pc_sub = self.create_subscription(
+            PointCloud2, "camera/depth/points", self.camera_points_callback, 10
+        )
+        self.range_subs = []
+        for range_topic_name in self.RANGE_SENSORS_TOPICS:
+            sub = self.create_subscription(LaserScan, range_topic_name, self.ranges_callback, 10)
+            self.range_subs.append(sub)
+        self.scan_sub = self.create_subscription(LaserScan, "scan", self.scan_callback, 10)
+
+        # Timer - send cmd_vel and check if the time needed for speed stabilization has elapsed
+        self.timer = self.create_timer(0.1, self.timer_callback)
 
+        # Destination velocity for cmd_vel
         self.v_x = 0.0
         self.v_y = 0.0
         self.v_yaw = 0.0
-        self.twist = None  # Debug info
 
-        self.controller_odom_flag = False
-        self.ekf_odom_flag = False
-        self.odom_tf_event = Event()
-        self.scan_event = Event()
-        self.ranges_events = [Event() for _ in range(len(self.RANGE_SENSORS_TOPICS))]
+        # Debug values
+        self.controller_twist = None
+        self.ekf_twist = None
+
+        # Robot test flags and events
+        self.is_controller_msg = False
+        self.is_controller_odom_correct = False
+        self.is_ekf_msg = False
+        self.is_ekf_odom_correct = False
+        self.is_imu_msg = False
+        self.is_joint_msg = False
+        self.robot_initialized_event = Event()
+        self.vel_stabilization_time_event = Event()
+
+        # Sensor test events
         self.camera_color_event = Event()
         self.camera_points_event = Event()
+        self.ranges_events = [Event() for _ in range(len(self.RANGE_SENSORS_TOPICS))]
+        self.scan_event = Event()
 
-    def clear_odom_flag(self):
-        self.controller_odom_flag = False
-        self.ekf_odom_flag = False
+        # Using /clock topic as time source (checking the simulation time)
+        use_sim_time = rclpy.parameter.Parameter("use_sim_time", rclpy.Parameter.Type.BOOL, True)
+        self.set_parameters([use_sim_time])
+
+        # Time values
+        self.current_time = 1e-9 * self.get_clock().now().nanoseconds
+        self.goal_received_time = None
+
+    def reset_flags(self):
+        self.is_controller_odom_correct = False
+        self.is_ekf_odom_correct = False
+        self.vel_stabilization_time_event.clear()
 
     def set_destination_speed(self, v_x, v_y, v_yaw):
-        self.clear_odom_flag()
+        self.get_logger().info(f"Setting cmd_vel: x: {v_x}, y: {v_y}, yaw: {v_yaw}")
         self.v_x = v_x
         self.v_y = v_y
         self.v_yaw = v_yaw
         self.goal_received_time = 1e-9 * self.get_clock().now().nanoseconds
-        self.vel_stabilization_time_event.clear()
-
-    def create_test_subscribers_and_publishers(self):
-        self.cmd_vel_publisher = self.create_publisher(Twist, "cmd_vel", 10)
-
-        self.controller_odom_sub = self.create_subscription(
-            Odometry, "rosbot_base_controller/odom", self.controller_callback, 10
-        )
-
-        self.ekf_odom_sub = self.create_subscription(
-            Odometry, "odometry/filtered", self.ekf_callback, 10
-        )
-
-        self.scan_sub = self.create_subscription(LaserScan, "scan", self.scan_callback, 10)
-
-        self.range_subs = []
-        for range_topic_name in self.RANGE_SENSORS_TOPICS:
-            sub = self.create_subscription(LaserScan, range_topic_name, self.ranges_callback, 10)
-            self.range_subs.append(sub)
-
-        self.camera_color_sub = self.create_subscription(
-            Image, "camera/image", self.camera_image_callback, 10
-        )
-
-        self.camera_points_sub = self.create_subscription(
-            PointCloud2, "camera/points", self.camera_points_callback, 10
-        )
-
-        self.timer = self.create_timer(1.0 / 10.0, self.timer_callback)
-
-    def start_node_thread(self):
-        self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,))
-        self.ros_spin_thread.start()
+        self.reset_flags()
 
     def is_twist_ok(self, twist: Twist):
         def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, eps=0.01):
@@ -114,23 +123,47 @@ def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY,
 
         return x_ok and y_ok and yaw_ok
 
+    def is_initialized(self):
+        if self.is_controller_msg and self.is_ekf_msg and self.is_imu_msg and self.is_joint_msg:
+            self.get_logger().info("Robot initialized!", once=True)
+            self.robot_initialized_event.set()
+        return self.robot_initialized_event.is_set()
+
     def controller_callback(self, data: Odometry):
-        self.get_logger().debug(f"Received twist from controller: {data.twist.twist}")
-        self.controller_odom_flag = self.is_twist_ok(data.twist.twist)
-        self.twist = data.twist.twist
+        if not self.is_controller_msg:
+            self.get_logger().info("Controller message arrived")
+        self.is_controller_msg = True
+        self.controller_twist = data.twist.twist
+        self.is_controller_odom_correct = self.is_twist_ok(data.twist.twist)
 
     def ekf_callback(self, data: Odometry):
-        self.get_logger().debug(f"Received twist filtered: {data.twist.twist}")
-
-        self.odom_tf_event.set()
-        self.ekf_odom_flag = self.is_twist_ok(data.twist.twist)
+        if not self.is_ekf_msg:
+            self.get_logger().info("EKF message arrived")
+        self.is_ekf_msg = True
+        self.ekf_twist = data.twist.twist
+        self.is_ekf_odom_correct = self.is_twist_ok(data.twist.twist)
+
+    def imu_callback(self, data):
+        if not self.is_imu_msg:
+            self.get_logger().info("IMU message arrived")
+        self.is_imu_msg = True
+
+    def joint_states_callback(self, data):
+        if not self.is_joint_msg:
+            self.get_logger().info("Joint State message arrived")
+        self.is_joint_msg = True
 
     def timer_callback(self):
-        self.publish_cmd_vel_messages()
-
         self.current_time = 1e-9 * self.get_clock().now().nanoseconds
-        if self.current_time > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY:
-            self.vel_stabilization_time_event.set()
+
+        if self.is_initialized() and self.goal_received_time:
+            self.publish_cmd_vel_msg()
+
+            if self.current_time > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY:
+                self.get_logger().info(
+                    "Speed stabilization time has expired", throttle_duration_sec=1
+                )
+                self.vel_stabilization_time_event.set()
 
     def scan_callback(self, data: LaserScan):
         self.get_logger().debug(f"Received scan length: {len(data.ranges)}")
@@ -150,70 +183,81 @@ def camera_points_callback(self, data: PointCloud2):
         if data.data:
             self.camera_points_event.set()
 
-    def publish_cmd_vel_messages(self):
+    def publish_cmd_vel_msg(self):
         twist_msg = Twist()
 
         twist_msg.linear.x = self.v_x
         twist_msg.linear.y = self.v_y
         twist_msg.angular.z = self.v_yaw
 
-        self.get_logger().debug(f"Publishing twist: {twist_msg}")
-        self.cmd_vel_publisher.publish(twist_msg)
+        self.cmd_vel_pub.publish(twist_msg)
 
+    def __exit__(self, exep_type, exep_value, trace):
+        if exep_type is not None:
+            raise Exception("Exception occurred, value: ", exep_value)
+        self.shutdown()
 
-def x_speed_test(node, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"):
-    node.set_destination_speed(v_x, v_y, v_yaw)
 
-    assert node.vel_stabilization_time_event.wait(timeout=120.0), (
+def wait_for_initialization(node: SimulationTestNode, robot_name="ROSbot"):
+    assert node.robot_initialized_event.wait(30), f"{robot_name} does not initialized correctly!"
+
+
+def x_speed_test(node: SimulationTestNode, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"):
+    node.set_destination_speed(v_x, v_y, v_yaw)
+    assert node.vel_stabilization_time_event.wait(20.0), (
         f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed"
         " since setting the target speed is:"
         f" {(node.current_time - node.goal_received_time):.1f}."
     )
-    assert node.controller_odom_flag, (
+    assert node.is_controller_odom_correct, (
         f"{robot_name}: does not move properly in x direction. Check"
         f" rosbot_base_controller! Twist: {node.twist}"
+        f"\nCommand: x: {v_x}, y:{v_y}, yaw:{v_yaw}"
     )
-    assert node.ekf_odom_flag, (
+    assert node.is_ekf_odom_correct, (
         f"{robot_name}: does not move properly in x direction. Check ekf_filter_node!"
         f" Twist: {node.twist}"
     )
 
 
-def y_speed_test(node, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"):
+def y_speed_test(node: SimulationTestNode, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"):
     node.set_destination_speed(v_x, v_y, v_yaw)
 
-    assert node.vel_stabilization_time_event.wait(timeout=120.0), (
+    assert node.vel_stabilization_time_event.wait(20.0), (
         f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed"
         " since setting the target speed is:"
         f" {(node.current_time - node.goal_received_time):.1f}."
     )
-    assert node.controller_odom_flag, (
+    assert node.is_controller_odom_correct, (
         f"{robot_name} does not move properly in y direction. Check"
         f" rosbot_base_controller! Twist: {node.twist}"
+        f"\nCommand: x: {v_x}, y:{v_y}, yaw:{v_yaw}"
     )
-    assert node.ekf_odom_flag, (
+    assert node.is_ekf_odom_correct, (
         f"{robot_name} does not move properly in y direction. Check ekf_filter_node!"
         f" Twist: {node.twist}"
     )
 
 
-def yaw_speed_test(node, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"):
+def yaw_speed_test(node: SimulationTestNode, v_x=0.0, v_y=0.0, v_yaw=0.0, robot_name="ROSbot"):
     node.set_destination_speed(v_x, v_y, v_yaw)
 
-    assert node.vel_stabilization_time_event.wait(timeout=120.0), (
+    assert node.vel_stabilization_time_event.wait(20.0), (
         f"{robot_name}: The simulation is running slowly or has crashed! The time elapsed"
         " since setting the target speed is:"
         f" {(node.current_time - node.goal_received_time):.1f}."
     )
+    assert node.is_controller_odom_correct, (
+        f"{robot_name} does not rotate properly. Check rosbot_base_controller! Twist:"
+        f" {node.controller_twist}"
+        f"\nCommand: x: {v_x}, y:{v_y}, yaw:{v_yaw}"
+    )
     assert (
-        node.controller_odom_flag
-    ), f"{robot_name} does not rotate properly. Check rosbot_base_controller! Twist: {node.twist}"
-    assert (
-        node.ekf_odom_flag
-    ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.twist}"
+        node.is_ekf_odom_correct
+    ), f"{robot_name} does not rotate properly. Check ekf_filter_node! Twist: {node.ekf_twist}"
 
 
-def sensors_readings_test(node, robot_name="ROSbot"):
+def sensors_readings_test(node: SimulationTestNode, robot_name="ROSbot"):
     flag = node.scan_event.wait(timeout=20.0)
     assert flag, f"{robot_name}'s lidar does not work properly!"
 
@@ -230,26 +274,20 @@ def sensors_readings_test(node, robot_name="ROSbot"):
     assert flag, f"{robot_name}'s camera point cloud does not work properly!"
 
 
-def tf_test(node, robot_name="ROSbot"):
-    flag = node.odom_tf_event.wait(timeout=20.0)
-    assert flag, (
-        f"{robot_name}: expected odom to base_link tf but it was not received. Check"
-        " robot_localization!"
-    )
-
-
-def diff_test(node, robot_name="ROSbot"):
-    sensors_readings_test(node, robot_name)
+def diff_test(node: SimulationTestNode, robot_name="ROSbot"):
+    wait_for_initialization(node, robot_name)
     # 0.9 m/s and 3.0 rad/s are controller's limits defined in
     # rosbot_controller/config/mecanum_drive_controller.yaml
     x_speed_test(node, v_x=0.9, robot_name=robot_name)
     yaw_speed_test(node, v_yaw=3.0, robot_name=robot_name)
+    sensors_readings_test(node, robot_name)
 
 
-def mecanum_test(node, robot_name="ROSbot"):
-    sensors_readings_test(node, robot_name)
+def mecanum_test(node: SimulationTestNode, robot_name="ROSbot"):
+    wait_for_initialization(node, robot_name)
     # 0.9 m/s and 3.0 rad/s are controller's limits defined in
     # rosbot_controller/config/mecanum_drive_controller.yaml
     x_speed_test(node, v_x=0.9, robot_name=robot_name)
     y_speed_test(node, v_y=0.9, robot_name=robot_name)
     yaw_speed_test(node, v_yaw=3.0, robot_name=robot_name)
+    sensors_readings_test(node, robot_name)
diff --git a/rosbot_utils/rosbot_utils/flash-firmware-usb.py b/rosbot_utils/rosbot_utils/flash-firmware-usb.py
index 1cd39c62..337cb5db 100755
--- a/rosbot_utils/rosbot_utils/flash-firmware-usb.py
+++ b/rosbot_utils/rosbot_utils/flash-firmware-usb.py
@@ -1,6 +1,6 @@
 #!/usr/bin/python3
 
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_utils/rosbot_utils/flash-firmware.py b/rosbot_utils/rosbot_utils/flash-firmware.py
index 73928af6..a9ec359f 100755
--- a/rosbot_utils/rosbot_utils/flash-firmware.py
+++ b/rosbot_utils/rosbot_utils/flash-firmware.py
@@ -1,6 +1,6 @@
 #!/usr/bin/python3
 
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_utils/rosbot_utils/flash_firmware.py b/rosbot_utils/rosbot_utils/flash_firmware.py
index fc89bcf1..078f6922 100755
--- a/rosbot_utils/rosbot_utils/flash_firmware.py
+++ b/rosbot_utils/rosbot_utils/flash_firmware.py
@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/rosbot_utils/setup.py b/rosbot_utils/setup.py
index 38f49c6b..822d4b93 100644
--- a/rosbot_utils/setup.py
+++ b/rosbot_utils/setup.py
@@ -1,4 +1,4 @@
-# Copyright 2023 Husarion
+# Copyright 2024 Husarion sp. z o.o.
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
diff --git a/tools/healthcheck.cpp b/tools/healthcheck.cpp
index a6d3294f..34f22455 100644
--- a/tools/healthcheck.cpp
+++ b/tools/healthcheck.cpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Husarion sp. z o.o.
+// Copyright 2024 Husarion sp. z o.o.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.