diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 0de952dc..73e41238 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -217,7 +217,7 @@ jobs:
           apt update
           apt upgrade -y
           apt-get update
-          apt install -y python3-colcon-metadata python3-pip cmake
+          apt install -y python3-colcon-metadata python3-pip cmake rsync
           rosdep update --rosdistro ${{ env.ROS_DISTRO }}
           rosdep install --rosdistro ${{ env.ROS_DISTRO }} -y --from-paths src --ignore-src -y
 
diff --git a/.github/workflows/nightly.yml b/.github/workflows/nightly.yml
index aa4780cb..0311fbd4 100644
--- a/.github/workflows/nightly.yml
+++ b/.github/workflows/nightly.yml
@@ -690,7 +690,7 @@ jobs:
           apt update
           apt upgrade -y
           # Install last version of CMake
-          apt install -y apt-transport-https ca-certificates gnupg software-properties-common wget
+          apt install -y apt-transport-https ca-certificates gnupg software-properties-common wget rsync
           if [ $(lsb_release -sr) = "22.04" ]; then
             echo "Using Ubuntu mainline CMake"
           else
diff --git a/config/generate_lib/generic/build.sh b/config/generate_lib/generic/build.sh
index 7db6e00c..e2e2697a 100755
--- a/config/generate_lib/generic/build.sh
+++ b/config/generate_lib/generic/build.sh
@@ -22,7 +22,7 @@ BUILD_DIR=$FW_TARGETDIR/build
 pushd $FW_TARGETDIR/mcu_ws >/dev/null
 
 	rm -rf build install log
-	
+
    	colcon build \
 		--merge-install \
 		--packages-ignore-regex=.*_cpp \
@@ -50,4 +50,15 @@ pushd $FW_TARGETDIR/mcu_ws >/dev/null
     cp -R $FW_TARGETDIR/mcu_ws/install/include $BUILD_DIR/; \
 	cd ..; rm -rf libmicroros;
 
+	######## Fix include paths  ########
+	INCLUDE_ROS2_PACKAGES=$(cd $FW_TARGETDIR/mcu_ws && 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 "$BUILD_DIR/include/${var}/${var}" ]
+		then
+			rsync -r $BUILD_DIR/include/${var}/${var}/* $BUILD_DIR/include/${var}/ || true
+			rm -rf $BUILD_DIR/include/${var}/${var}/
+		fi
+	done
+
 popd >/dev/null