diff --git a/config/host/generic/build.sh b/config/host/generic/build.sh
index 31d497db..88fa746c 100755
--- a/config/host/generic/build.sh
+++ b/config/host/generic/build.sh
@@ -6,4 +6,10 @@ set +o nounset
 . install/local_setup.bash
 set -o nounset
 
+colcon build --packages-up-to service_msgs unique_identifier_msgs --metas src --cmake-args -DBUILD_TESTING=OFF -DBUILD_SHARED_LIBS=ON $@
+
+set +o nounset
+. install/local_setup.bash
+set -o nounset
+
 colcon build --metas src --cmake-args -DBUILD_TESTING=OFF -DBUILD_SHARED_LIBS=ON $@
diff --git a/package.xml b/package.xml
index 537da409..112a679a 100644
--- a/package.xml
+++ b/package.xml
@@ -4,13 +4,16 @@
   <name>micro_ros_setup</name>
   <version>5.0.0</version>
   <description>Tools for setting up micro-ROS workspaces</description>
-  <maintainer email="pablogarrido@eprosima.com">Pablo Garrido</maintainer>
+  <maintainer email="eugeniocollado@eprosima.com">Eugenio Collado</maintainer>
+  <maintainer email="carlosespinoza@eprosima.com">Carlos Espinoza</maintainer>
   <license>Apache-2.0</license>
 
   <depend>curl</depend>
   <depend>python3-yaml</depend>
   <depend>clang-tidy</depend>
 
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_lint_auto</buildtool_depend>
   <build_depend>flex</build_depend>
   <build_depend>bison</build_depend>
   <build_depend>libncurses-dev</build_depend>
diff --git a/scripts/build_firmware.sh b/scripts/build_firmware.sh
index 96a2c865..e82cc0fe 100755
--- a/scripts/build_firmware.sh
+++ b/scripts/build_firmware.sh
@@ -76,3 +76,17 @@ else
     . $PREFIX/config/$RTOS/$PLATFORM/build.sh
 fi
 
+######## Fix include paths  ########
+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
+            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
+
diff --git a/scripts/create_firmware_ws.sh b/scripts/create_firmware_ws.sh
index fc3b6665..a2beb6bc 100755
--- a/scripts/create_firmware_ws.sh
+++ b/scripts/create_firmware_ws.sh
@@ -104,6 +104,8 @@ if [ $RTOS != "host" ]; then
     pushd $FW_TARGETDIR/$DEV_WS_DIR >/dev/null
         # Fix failing build by ignoring rmw_test_fixture_implementation.
         touch ros2/ament_cmake_ros/rmw_test_fixture_implementation/COLCON_IGNORE
+        # Fix failing build by ignoring rmw_test_fixture.
+        touch ros2/ament_cmake_ros/rmw_test_fixture/COLCON_IGNORE
         colcon build
         set +o nounset
         # source dev workspace
@@ -116,4 +118,3 @@ rosdep install --os=ubuntu:noble -y --from-paths $PREFIX/config/$RTOS/$TARGET_FO
 
 # Creating specific firmware folder
 . $PREFIX/config/$RTOS/$TARGET_FOLDER/create.sh
-