diff --git a/scripts/build_firmware.sh b/scripts/build_firmware.sh
index 00295688..e82cc0fe 100755
--- a/scripts/build_firmware.sh
+++ b/scripts/build_firmware.sh
@@ -77,13 +77,16 @@ else
 fi
 
 ######## Fix include paths  ########
-cd $FW_TARGETDIR/mcu_ws
-INCLUDE_ROS2_PACKAGES=$(colcon list | awk '{print $1}' | awk -v d=" " '{s=(NR==1?s:s d)$0}END{print s}')
+if [ "$RTOS" != "host" ]; then
+    cd "$FW_TARGETDIR/mcu_ws"
+    INCLUDE_ROS2_PACKAGES=$(colcon list | awk '{print $1}' | awk -v d=" " '{s=(NR==1?s:s d)$0}END{print s}')
 
-for var in ${INCLUDE_ROS2_PACKAGES}; do
-    if [ -d "$FW_TARGETDIR/build/include/${var}/${var}" ]; then
-        rsync -r $FW_TARGETDIR/build/include/${var}/${var}/* $FW_TARGETDIR/build/include/${var}
-        rm -rf $FW_TARGETDIR/build/include/${var}/${var}
-    fi
-done
+    for var in ${INCLUDE_ROS2_PACKAGES}; do
+        if [ -d "$FW_TARGETDIR/build/include/${var}/${var}" ]; then
+            mkdir -p "$FW_TARGETDIR/build/include/${var}"
+            cp -a "$FW_TARGETDIR/build/include/${var}/${var}/". "$FW_TARGETDIR/build/include/${var}/"
+            rm -rf "$FW_TARGETDIR/build/include/${var}/${var}"
+        fi
+    done
+fi