diff --git a/Dockerfile.hardware b/Dockerfile.hardware
index 2e26259..e3a1358 100644
--- a/Dockerfile.hardware
+++ b/Dockerfile.hardware
@@ -48,6 +48,35 @@ RUN pip3 install -U platformio && \
     pio run && \
     chmod -x .pio/build/olimex_e407/firmware.bin
 
+## =========================== Healthcheck builder ===============================
+
+FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base AS healthcheck_builder
+
+ARG ROS_DISTRO
+ARG PREFIX
+
+SHELL ["/bin/bash", "-c"]
+
+WORKDIR /ros2_ws
+
+RUN mkdir src && \
+    cd src && \
+    MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \
+    source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \
+    # Create health check package
+    ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp nav_msgs && \
+    sed -i '/find_package(nav_msgs REQUIRED)/a \
+        add_executable(healthcheck_node src/healthcheck.cpp)\n \
+        ament_target_dependencies(healthcheck_node rclcpp nav_msgs)\n \
+        install(TARGETS healthcheck_node DESTINATION lib/${PROJECT_NAME})' \
+        /ros2_ws/src/healthcheck_pkg/CMakeLists.txt
+
+COPY healthcheck.cpp src/healthcheck_pkg/src/    
+
+RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \
+    source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \
+    colcon build && \
+    rm -rf build log src
 
 ## =========================== ROS builder ===============================
 FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base AS ros_builder
@@ -67,49 +96,23 @@ RUN apt-get update && apt-get install -y \
         python3-pip \
         python3-sh \
         python3-periphery && \
-    pip3 install pyserial && \
-    git clone --depth 1 https://github.com/husarion/rosbot_ros.git src -b humble && \
+    pip3 install pyserial
+
+RUN git clone --depth 1 https://github.com/husarion/rosbot_ros.git src -b humble && \
     vcs import src < src/rosbot/rosbot_hardware.repos && \
+    cp -r src/ros2_controllers/diff_drive_controller src/ && \
+    cp -r src/ros2_controllers/imu_sensor_broadcaster src/ && \
+    rm -rf src/ros2_controllers && \
     # it is necessary to remove simulation - otherwise rosdep tries to install dependencies
     rm -r src/rosbot_gazebo && \
     # without this line (using vulcanexus base image) rosdep init throws error: "ERROR: default sources list file already exists:"
     rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \
     rosdep init && \
     rosdep update --rosdistro $ROS_DISTRO && \
-    rosdep install --from-paths src --ignore-src -y
-
-
-RUN cd src/ && \
+    rosdep install --from-paths src --ignore-src -y && \
     MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \
     source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \
-    # Create health check package
-    ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp nav_msgs && \
-    sed -i '/find_package(nav_msgs REQUIRED)/a \
-    add_executable(healthcheck_node src/healthcheck.cpp)\n \
-    ament_target_dependencies(healthcheck_node rclcpp nav_msgs)\n \
-    install(TARGETS healthcheck_node DESTINATION lib/${PROJECT_NAME})' \
-    /ros2_ws/src/healthcheck_pkg/CMakeLists.txt && \
-    mv /healthcheck.cpp /ros2_ws/src/healthcheck_pkg/src/ && \
-    cd .. && \
-    # Build
-        colcon build --packages-skip \
-        ackermann_steering_controller \
-        admittance_controller \
-        bicycle_steering_controller \
-        tricycle_steering_controller \
-        effort_controllers \
-        force_torque_sensor_broadcaster \
-        forward_command_controller \
-        gripper_controllers \
-        joint_trajectory_controller \
-        position_controllers \
-        range_sensor_broadcaster \
-        ros2_controllers \
-        ros2_controllers_test_nodes \
-        rqt_joint_trajectory_controller \
-        tricycle_controller \
-        tricycle_steering_controller \
-        velocity_controllers && \
+    colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release  && \
     rm -rf build log
 
 ## =========================== Final Stage ===============================
@@ -123,6 +126,7 @@ SHELL ["/bin/bash", "-c"]
 WORKDIR /ros2_ws
 
 COPY --from=ros_builder /ros2_ws /ros2_ws
+COPY --from=healthcheck_builder /ros2_ws /ros2_ws_healthcheck
 
 RUN apt-get update && apt-get install -y \
         python3-rosdep \
diff --git a/demo/namespace/compose.yaml b/demo/namespace/compose.yaml
index 78424ca..bba2f94 100644
--- a/demo/namespace/compose.yaml
+++ b/demo/namespace/compose.yaml
@@ -12,7 +12,10 @@
 services:
 
   rosbot:
-    image: husarion/rosbot:humble-ros2-combined-microros
+    # image: husarion/rosbot:humble-ros2-combined-microros
+    build:
+      context: ../..
+      dockerfile: Dockerfile.hardware
     network_mode: host
     ipc: host
     devices:
diff --git a/ros_entrypoint.sh b/ros_entrypoint.sh
index 21341ce..d785c50 100755
--- a/ros_entrypoint.sh
+++ b/ros_entrypoint.sh
@@ -16,7 +16,8 @@ fi
 
 # setup ros environment
 source "/opt/ros/$ROS_DISTRO/setup.bash"
-source  "/ros2_ws/install/setup.bash"
+source "/ros2_ws/install/setup.bash"
+source "/ros2_ws_healthcheck/install/setup.bash"
 
 ros2 run healthcheck_pkg healthcheck_node &
 
diff --git a/vulcanexus_entrypoint.sh b/vulcanexus_entrypoint.sh
index 950ae45..a1d99ff 100755
--- a/vulcanexus_entrypoint.sh
+++ b/vulcanexus_entrypoint.sh
@@ -17,6 +17,7 @@ fi
 # setup ros environment
 source "/opt/vulcanexus/$ROS_DISTRO/setup.bash"
 source  "/ros2_ws/install/setup.bash"
+source "/ros2_ws_healthcheck/install/setup.bash"
 
 ros2 run healthcheck_pkg healthcheck_node &