diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs
index 1d9a34b1f..08014261d 100644
--- a/.git-blame-ignore-revs
+++ b/.git-blame-ignore-revs
@@ -10,3 +10,5 @@ d71897d20d9816b2c86bb23d2d8ba78a24622a8d
 7b2835b380d6b63c9953ecb5b7f3258a98d08022
 # Make lsb_release.py black
 287a466e1b3e2018deac2d9214439c705325bc5c
+# (CI) consistent echo colors
+1009e550b338f36656483d934e5dde39ade13c53
diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
index d2476d695..22ea2a8f7 100644
--- a/.github/workflows/main.yml
+++ b/.github/workflows/main.yml
@@ -118,23 +118,24 @@ jobs:
           # shellcheck disable=SC2296
           TARGET_IMAGE="${{ env.REGISTRY }}/tue-robotics/tue-env-ros-${{ matrix.ros-distro }}${IMAGE_FLAVOR}:${IMAGE_TAG}-${{ matrix.platform }}"
           SSH_KNOWN_HOSTS="github.com"
-          SSH_ARG=()
+          SSH_ARGS=()
           if [[ -n "${SSH_KEY}" ]]
           then
-              SSH_KEY_PATH="${HOME}/.ssh/ci_ssh_key"
+              SSH_KEY_PATH="${HOME}"/.ssh/ci_ssh_key
+              echo -e "\e[35;1mmkdir -p ${HOME}/.ssh\e[0m"
               mkdir -p "${HOME}"/.ssh
               echo "${SSH_KEY}" > "${SSH_KEY_PATH}"
               chmod 600 "${SSH_KEY_PATH}"
               eval "$(ssh-agent -s)"
-              SSH_ARG+=("--ssh" "--ssh-key=${SSH_KEY_PATH}")
+              SSH_ARGS+=("--ssh" "--ssh-key=${SSH_KEY_PATH}")
           fi
           if [[ -n "${SSH_KNOWN_HOSTS}" ]]
           then
-              echo -e "\e[35m\e[1mmkdir -p ${HOME}/.ssh\e[0m"
+              echo -e "\e[35;1mmkdir -p ${HOME}/.ssh\e[0m"
               mkdir -p "${HOME}"/.ssh
               for host in ${SSH_KNOWN_HOSTS}
               do
-                  echo -e "\e[35m\e[1mssh-keyscan -t rsa -H \"${host}\" 2>&1 | tee -a ${HOME}/.ssh/known_hosts\e[0m"
+                  echo -e "\e[35;1mssh-keyscan -t rsa -H \"${host}\" 2>&1 | tee -a ${HOME}/.ssh/known_hosts\e[0m"
                   ssh-keyscan -t rsa -H "${host}" 2>&1 | tee -a "${HOME}"/.ssh/known_hosts
               done
           fi
@@ -168,10 +169,10 @@ jobs:
               --virtualenv-include-system-site-packages="${VENV_INCLUDE_SYSTEM_SITE}" \
               --docker_user="${DOCKER_USER}" \
               --docker_user_id="${DOCKER_USER_ID}" \
-              "${SSH_ARG[@]}"
-          echo -e "\e[35m\e[1mimage=${TARGET_IMAGE} >> \${GITHUB_OUTPUT}\e[0m"
+              "${SSH_ARGS[@]}"
+          echo -e "\e[35;1mimage=${TARGET_IMAGE} >> \${GITHUB_OUTPUT}\e[0m"
           echo "image=${TARGET_IMAGE}" >> "${GITHUB_OUTPUT}"
-          echo -e "\e[35m\e[1mtag=${IMAGE_TAG} >> \${GITHUB_OUTPUT}\e[0m"
+          echo -e "\e[35;1mtag=${IMAGE_TAG} >> \${GITHUB_OUTPUT}\e[0m"
           echo "tag=${IMAGE_TAG}" >> "${GITHUB_OUTPUT}"
       - name: Generate artifacts
         id: artifacts
@@ -180,7 +181,7 @@ jobs:
           MATRIX_OUTPUT_KEY=image
           echo "${{ steps.script.outputs.image }}" > "${MATRIX_OUTPUT_KEY}"
           sha256sum "${MATRIX_OUTPUT_KEY}" | cut -d " " -f 1 | xargs -I{} echo "artifact={}" >> $GITHUB_OUTPUT
-          echo -e "\e[35m\e[1mkey=${MATRIX_OUTPUT_KEY} >> \$GITHUB_OUTPUT\e[0m"
+          echo -e "\e[35;1mkey=${MATRIX_OUTPUT_KEY} >> \${GITHUB_OUTPUT}\e[0m"
           echo "key=${MATRIX_OUTPUT_KEY}" >> $GITHUB_OUTPUT
       - name: Write image name
         if: github.event_name != 'pull_request'
@@ -208,7 +209,7 @@ jobs:
         id: read
         run: |
           result="$(find . -name ${{ needs.docker_generation_tue_env.outputs.key }} -exec cat {} \; | jq --slurp --raw-input 'split("\n")[:-1] | .[] | split(":") | {(.[0]) : [(.[1])]}' | jq -cs '[.[] | to_entries] | flatten | reduce .[] as $dot ({}; .[$dot.key] += $dot.value) | { ${{ needs.docker_generation_tue_env.outputs.key }} : . }' )"
-          echo -e "\e[35m\e[1mresult=${result} >> \$GITHUB_OUTPUT\e[0m"
+          echo -e "\e[35;1mresult=${result} >> \${GITHUB_OUTPUT}\e[0m"
           echo "result=${result}" >> $GITHUB_OUTPUT
       - name: Generate new manifest files
         run: |
diff --git a/VERSION b/VERSION
index b45fe3106..198ec23cc 100644
--- a/VERSION
+++ b/VERSION
@@ -1 +1 @@
-1.25.5
+1.25.6
diff --git a/ci/azure-pipelines.example.yml b/ci/azure-pipelines.example.yml
index 66253a59d..9d806d510 100644
--- a/ci/azure-pipelines.example.yml
+++ b/ci/azure-pipelines.example.yml
@@ -67,13 +67,14 @@ steps:
     targetType: inline
     script: |
       # START SSH ONLY
+      echo -e "\e[35;1mmkdir -p ${HOME}/.ssh\e[0m"
       mkdir -p "${HOME}"/.ssh
       cp $(deployKey.secureFilePath) "${HOME}"/.ssh/ci_ssh_key
       if [[ -n "${SSH_KNOWN_HOSTS}" ]]
       then
         for host in ${SSH_KNOWN_HOSTS}
         do
-          echo -e "\e[35m\e[1mssh-keyscan -t rsa -H \"${host}\" 2>&1 | tee -a ${HOME}/.ssh/known_hosts\e[0m"
+          echo -e "\e[35;1mssh-keyscan -t rsa -H \"${host}\" 2>&1 | tee -a ${HOME}/.ssh/known_hosts\e[0m"
           ssh-keyscan -t rsa -H "${host}" 2>&1 | tee -a "${HOME}"/.ssh/known_hosts
         done
       fi
diff --git a/ci/azure-pipelines.multiple_pkgs.example.yml b/ci/azure-pipelines.multiple_pkgs.example.yml
index 4cd0a3e9c..c9cbaff70 100644
--- a/ci/azure-pipelines.multiple_pkgs.example.yml
+++ b/ci/azure-pipelines.multiple_pkgs.example.yml
@@ -110,13 +110,14 @@ stages:
               targetType: inline
               script: |
                 # START SSH ONLY
+                echo -e "\e[35;1mmkdir -p ${HOME}/.ssh\e[0m"
                 mkdir -p "${HOME}"/.ssh
                 cp $(deployKey.secureFilePath) "${HOME}"/.ssh/ci_ssh_key
                 if [[ -n "${SSH_KNOWN_HOSTS}" ]]
                 then
                   for host in ${SSH_KNOWN_HOSTS}
                   do
-                    echo -e "\e[35m\e[1mssh-keyscan -t rsa -H \"${host}\" 2>&1 | tee -a ${HOME}/.ssh/known_hosts\e[0m"
+                    echo -e "\e[35;1mssh-keyscan -t rsa -H \"${host}\" 2>&1 | tee -a ${HOME}/.ssh/known_hosts\e[0m"
                     ssh-keyscan -t rsa -H "${host}" 2>&1 | tee -a "${HOME}"/.ssh/known_hosts
                   done
                 fi
diff --git a/ci/build-docker-image.sh b/ci/build-docker-image.sh
index 80eb131f8..5ddfaa8c4 100755
--- a/ci/build-docker-image.sh
+++ b/ci/build-docker-image.sh
@@ -7,8 +7,15 @@
 # Stop on errors
 set -o errexit
 
+function cleanup {
+  echo -e "\e[35;1mRemoving ./known_hosts\e[0m"
+  rm -f ./known_hosts
+}
+
+trap cleanup EXIT
+
 # Execute script only in a CI environment
-if [[ "$CI" != "true" ]]
+if [[ "${CI}" != "true" ]]
 then
     echo -e "\e[35;1mError!\e[0m Trying to execute a CI script in a non-CI environment. Exiting script."
     exit 1
@@ -227,7 +234,8 @@ then
     echo -e "\e[35;1mCreating a new docker context for multi-arch builds\e[0m"
     docker context create multiarch-environment
     echo -e "\e[35;1mCreating a new buildx builder for multi-arch builds\e[0m"
-    docker buildx create --name multiarchbuilder --driver docker-container --use multiarch-environment
+    docker buildx create --name multiarch-builder --driver docker-container --use multiarch-environment
+    echo -e "\e[35;1mdocker buildx ls\e[0m"
     docker buildx ls
 fi
 
diff --git a/ci/build-package.sh b/ci/build-package.sh
index 154c79ca9..9d77cb09a 100755
--- a/ci/build-package.sh
+++ b/ci/build-package.sh
@@ -8,9 +8,9 @@
 set -o errexit
 
 # Execute script only in a CI environment
-if [ "$CI" != "true" ]
+if [[ "${CI}" != "true" ]]
 then
-    echo -e "\e[35m\e[1mError!\e[0m Trying to execute a CI script in a non-CI environment. Exiting script."
+    echo -e "\e[35;1mError!\e[0m Trying to execute a CI script in a non-CI environment. Exiting script."
     exit 1
 fi
 
@@ -21,27 +21,31 @@ do
         -p=* | --package=* )
             PACKAGE="${i#*=}" ;;
 
+        --debug )
+            DEBUG="true" ;;
+
         * )
             # unknown option
             if [[ -n "$i" ]]  # Ignore empty arguments
             then
-                echo -e "\e[35m\e[1mUnknown input argument '$i'. Check CI .yml file\e[0m"
+                echo -e "\e[35;1mUnknown input argument '$i'. Check CI .yml file\e[0m"
                 exit 1
             fi ;;
     esac
     shift
 done
 
-echo -e "\e[35m\e[1mPACKAGE     = ${PACKAGE}\e[0m"
+echo -e "\e[35;1mPACKAGE     = ${PACKAGE}\e[0m"
 ROS_VERSION=$(docker exec tue-env bash -c 'source ~/.bashrc; echo "${ROS_VERSION}"' | tr -d '\r')
-echo -e "\e[35m\e[1mROS_VERSION = ${ROS_VERSION}\e[0m"
+echo -e "\e[35;1mROS_VERSION = ${ROS_VERSION}\e[0m"
+echo -e "\e[35;1mDEBUG       = ${DEBUG}\e[0m"
 
 # If packages is non-zero, this is a multi-package repo. In multi-package repo, check if this package needs CI.
 # If a single-package repo, CI is always needed.
 # shellcheck disable=SC2153
 if [ -n "$PACKAGES" ] && ! echo "$PACKAGES" | grep -sqw "$PACKAGE"
 then
-    echo -e "\e[35m\e[1mNo changes in this package, so no need to run CI\e[0m"
+    echo -e "\e[35;1mNo changes in this package, so no need to run CI\e[0m"
     exit 0
 fi
 
@@ -49,25 +53,35 @@ fi
 # Compile the package
 if [[ "${ROS_VERSION}" == 1 ]]
 then
-    echo -e "\e[35m\e[1mCompile the package (catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_ENABLE_TESTING=OFF)\e[0m"
-    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}"/src/"${PACKAGE}" && /usr/bin/python3 "$(command -v catkin)" build --this --no-status -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_ENABLE_TESTING=OFF'
+    ADDITIONAL_ARGS_CATKIN=()
+    if [[ ${DEBUG} == "true" ]]
+    then
+        ADDITIONAL_ARGS_CATKIN+=("--verbose")
+    fi
+    echo -e "\e[35;1mCompile the package (catkin build ${ADDITIONAL_ARGS_CATKIN[*]} -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_ENABLE_TESTING=OFF)\e[0m"
+    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}"/src/"${PACKAGE}" && /usr/bin/python3 "$(command -v catkin)" build --this --no-status '"${ADDITIONAL_ARGS_CATKIN[*]}"' -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_ENABLE_TESTING=OFF'
 else
-    echo -e "\e[35m\e[1mCheck for default mixin repo (colcon mixin list)\e[0m"
-    MIXIN_REPOS=$(docker exec tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon mixin list | grep -v "^- "' | tr -d '\r' | awk -F ": " '{print $1}')
+    ADDITIONAL_ARGS_COLCON=()
+    if [[ ${DEBUG} == "true" ]]
+    then
+        ADDITIONAL_ARGS_COLCON+=("--log-level" "debug")
+    fi
+    echo -e "\e[35;1mCheck for default mixin repo (colcon ${ADDITIONAL_ARGS_COLCON[*]} mixin list)\e[0m"
+    MIXIN_REPOS=$(docker exec tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon '"${ADDITIONAL_ARGS_COLCON[*]}"' mixin list | grep -v "^- "' | tr -d '\r' | awk -F ": " '{print $1}')
     if ! echo -e "${MIXIN_REPOS}" | grep "^default$" -q
     then
-        echo -e "\e[35m\e[1mAdd the default mixin repo (colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml)\e[0m"
-        docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml'
+        echo -e "\e[35;1mAdd the default mixin repo (colcon ${ADDITIONAL_ARGS_COLCON[*]} mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml)\e[0m"
+        docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon '"${ADDITIONAL_ARGS_COLCON[*]}"' mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml'
     else
-        echo -e "\e[35m\e[1mDefault mixin repo already exists\e[0m"
+        echo -e "\e[35;1mDefault mixin repo already exists\e[0m"
     fi
 
-    echo -e "\e[35m\e[1mUpdate colcon mixins (colcon mixin update)\e[0m"
-    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon mixin update'
+    echo -e "\e[35;1mUpdate colcon mixins (colcon ${ADDITIONAL_ARGS_COLCON[*]} mixin update)\e[0m"
+    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon '"${ADDITIONAL_ARGS_COLCON[*]}"' mixin update'
 
-    echo -e "\e[35m\e[1mDeleting the merged install directory\e[0m"
+    echo -e "\e[35;1mDeleting the merged install directory\e[0m"
     docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && rm -rf install'
 
-    echo -e "\e[35m\e[1mCompile the package (colcon build --mixin rel-with-deb-info build-testing-off)\e[0m"
-    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon build --packages-up-to "${PACKAGE}" --mixin rel-with-deb-info build-testing-off --event-handlers desktop_notification- status- terminal_title-'
+    echo -e "\e[35;1mCompile the package (colcon ${ADDITIONAL_ARGS_COLCON[*]} build --mixin rel-with-deb-info build-testing-off)\e[0m"
+    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon '"${ADDITIONAL_ARGS_COLCON[*]}"' build --packages-up-to "${PACKAGE}" --mixin rel-with-deb-info build-testing-off --event-handlers desktop_notification- status- terminal_title-'
 fi
diff --git a/ci/commit-range/action.yml b/ci/commit-range/action.yml
index 3d193170e..6a1cb3cb2 100644
--- a/ci/commit-range/action.yml
+++ b/ci/commit-range/action.yml
@@ -31,6 +31,6 @@ runs:
           NEWEST=${{ github.event.pull_request.head.sha }}
           COMMIT_RANGE="${OLDEST}...${NEWEST}"
         fi
-        echo -e "\e[35m\e[1m COMMIT_RANGE = ${COMMIT_RANGE} \e[0m"
+        echo -e "\e[35;1m COMMIT_RANGE = ${COMMIT_RANGE} \e[0m"
         echo "commit-range=${COMMIT_RANGE}" >> $GITHUB_OUTPUT
       shell: bash
diff --git a/ci/gh-actions.example.yml b/ci/gh-actions.example.yml
index 4f3af87fd..5dcb6b6c1 100644
--- a/ci/gh-actions.example.yml
+++ b/ci/gh-actions.example.yml
@@ -1,6 +1,6 @@
 name: CI
 
-on: [push, pull_request]
+on: [push, pull_request, workflow_dispatch]
 
 jobs:
   tue-ci:
diff --git a/ci/gh-actions.multiple_pkgs.example.yml b/ci/gh-actions.multiple_pkgs.example.yml
index ddffe0ab5..7d6d2ea1a 100644
--- a/ci/gh-actions.multiple_pkgs.example.yml
+++ b/ci/gh-actions.multiple_pkgs.example.yml
@@ -1,6 +1,6 @@
 name: CI
 
-on: [push, pull_request]
+on: [push, pull_request, workflow_dispatch]
 
 jobs:
   matrix:
diff --git a/ci/install-package.sh b/ci/install-package.sh
index 80a9b14c0..7d1e84fcc 100755
--- a/ci/install-package.sh
+++ b/ci/install-package.sh
@@ -9,9 +9,9 @@
 set -o errexit
 
 # Execute script only in a CI environment
-if [ "$CI" != "true" ]
+if [[ "${CI}" != "true" ]]
 then
-    echo -e "\e[35m\e[1mError!\e[0m Trying to execute a CI script in a non-CI environment. Exiting script."
+    echo -e "\e[35;1mError!\e[0m Trying to execute a CI script in a non-CI environment. Exiting script."
     exit 1
 fi
 
@@ -23,7 +23,7 @@ do
             PACKAGE="${i#*=}" ;;
 
         -b=* | --branch=* )
-        # BRANCH should allways be targetbranch
+        # BRANCH should allways be target branch
             BRANCH="${i#*=}" ;;
 
         -c=* | --commit=* )
@@ -47,33 +47,41 @@ do
         --ref-name=* )
             REF_NAME="${i#*=}" ;;
 
+        --debug )
+            DEBUG=true ;;
+
         * )
             # unknown option
             if [[ -n "$i" ]]  # Ignore empty arguments
             then
-                echo -e "\e[35m\e[1mUnknown input argument '$i'. Check CI .yml file\e[0m"
+                echo -e "\e[35;1mUnknown input argument '$i'. Check CI .yml file\e[0m"
                 exit 1
             fi ;;
     esac
     shift
 done
 
-echo -e "\e[35m\e[1mPACKAGE      = ${PACKAGE}\e[0m"
-echo -e "\e[35m\e[1mBRANCH       = ${BRANCH}\e[0m"
-echo -e "\e[35m\e[1mCOMMIT       = ${COMMIT}\e[0m"
-echo -e "\e[35m\e[1mPULL_REQUEST = ${PULL_REQUEST}\e[0m"
+echo -e "\e[35;1mPACKAGE      = ${PACKAGE}\e[0m"
+echo -e "\e[35;1mBRANCH       = ${BRANCH}\e[0m"
+echo -e "\e[35;1mCOMMIT       = ${COMMIT}\e[0m"
+echo -e "\e[35;1mPULL_REQUEST = ${PULL_REQUEST}\e[0m"
 
 # Set default value for IMAGE_NAME
 [ -z "$IMAGE_NAME" ] && IMAGE_NAME='ghcr.io/tue-robotics/tue-env-ros-noetic'
-echo -e "\e[35m\e[1mIMAGE_NAME   = ${IMAGE_NAME}\e[0m"
+echo -e "\e[35;1mIMAGE_NAME   = ${IMAGE_NAME}\e[0m"
 
 # Set default value for directory to place mountable container assests
 [ -z "$SHARED_DIR" ] && SHARED_DIR="$HOME"
-echo -e "\e[35m\e[1mSHARED_DIR   = ${SHARED_DIR}\e[0m"
+echo -e "\e[35;1mSHARED_DIR   = ${SHARED_DIR}\e[0m"
 
 # Set default value for REF_NAME
 [ -z "$REF_NAME" ] && REF_NAME="pull"
-echo -e "\e[35m\e[1mREF_NAME     = ${REF_NAME}\e[0m"
+echo -e "\e[35;1mREF_NAME     = ${REF_NAME}\e[0m"
+echo -e "\e[35;1mDEBUG       = ${DEBUG}\e[0m"
+
+ADDITIONAL_ARGS_LOCAL_INSTALL=()
+ADDITIONAL_ARGS_LOCAL_BUILD=()
+ADDITIONAL_ARGS_LOCAL_TEST=()
 
 if [ "$USE_SSH" == "true" ]
 then
@@ -123,22 +131,31 @@ then
     [[ "${SSH_KEY_CHECK}" == "true" ]] || { echo "No SSH keys found" && exit 1; }
 
     [[ -n "${SSH_KEY_FINGERPRINT}" ]] || SSH_KEY_FINGERPRINT="default"
-    echo -e "\e[35m\e[1mSSH_KEY      = ${SSH_KEY_FINGERPRINT}\e[0m"
+    echo -e "\e[35;1mSSH_KEY      = ${SSH_KEY_FINGERPRINT}\e[0m"
 
     DOCKER_SSH_AUTH_SOCK="/tmp/ssh_auth_sock"
-    DOCKER_MOUNT_KNOWN_HOSTS_ARGS="-e SSH_AUTH_SOCK=$DOCKER_SSH_AUTH_SOCK --mount type=bind,source=$SHARED_DIR/.ssh,target=/tmp/.ssh"
+    DOCKER_MOUNT_KNOWN_HOSTS_ARGS=("-e" "SSH_AUTH_SOCK=${DOCKER_SSH_AUTH_SOCK}" "--mount" "type=bind,source=$SHARED_DIR/.ssh,target=/tmp/.ssh")
 
     # Used in the print statement to reproduce CI build locally
-    ADDITIONAL_ARGS_LOCAL_BUILD="--shared=/tmp/shared/${PACKAGE} --ssh"
+    ADDITIONAL_ARGS_LOCAL_INSTALL+=("--shared=/tmp/shared/${PACKAGE}" "--ssh")
+fi
+
+ADDITIONAL_ARGS_TUE_GET=()
+if [[ ${DEBUG} == "true" ]]
+then
+    ADDITIONAL_ARGS_LOCAL_INSTALL+=("--debug")
+    ADDITIONAL_ARGS_LOCAL_BUILD+=("--debug")
+    ADDITIONAL_ARGS_LOCAL_TEST+=("--debug")
+    ADDITIONAL_ARGS_TUE_GET+=("--debug")
 fi
 
-echo -e "\e[35m\e[1m
+echo -e "\e[35;1m
 This build can be reproduced locally using the following commands:
 
 tue-get install docker
-~/.tue/ci/install-package.sh --package=${PACKAGE} --branch=${BRANCH} --commit=${COMMIT} --pullrequest=${PULL_REQUEST} --image=${IMAGE_NAME} --ref-name=${REF_NAME} ${ADDITIONAL_ARGS_LOCAL_BUILD}
-~/.tue/ci/build-package.sh --package=${PACKAGE}
-~/.tue/ci/test-package.sh --package=${PACKAGE}
+"'${TUE_DIR}'"/ci/install-package.sh --package=${PACKAGE} --branch=${BRANCH} --commit=${COMMIT} --pullrequest=${PULL_REQUEST} --image=${IMAGE_NAME} --ref-name=${REF_NAME} ${ADDITIONAL_ARGS_LOCAL_INSTALL[*]}
+"'${TUE_DIR}'"/ci/build-package.sh --package=${PACKAGE} ${ADDITIONAL_ARGS_LOCAL_BUILD[*]}
+"'${TUE_DIR}'"/ci/test-package.sh --package=${PACKAGE} ${ADDITIONAL_ARGS_LOCAL_TEST[*]}
 
 Optionally fix your compilation errors and re-run only the last command
 \e[0m"
@@ -148,7 +165,7 @@ Optionally fix your compilation errors and re-run only the last command
 # shellcheck disable=SC2153
 if [ -n "$PACKAGES" ] && ! echo "$PACKAGES" | grep -sqw "$PACKAGE"
 then
-    echo -e "\e[35m\e[1mNo changes in this package, so no need to run CI\e[0m"
+    echo -e "\e[35;1mNo changes in this package, so no need to run CI\e[0m"
     exit 0
 fi
 
@@ -163,10 +180,10 @@ docker stop tue-env  &> /dev/null || true
 docker rm tue-env &> /dev/null || true
 
 # Pull the identical branch name from dockerhub if exist, use master as fallback
-echo -e "\e[35m\e[1mTrying to fetch docker image: $IMAGE_NAME:$BRANCH_TAG\e[0m"
+echo -e "\e[35;1mTrying to fetch docker image: $IMAGE_NAME:$BRANCH_TAG\e[0m"
 if ! docker pull "$IMAGE_NAME:$BRANCH_TAG"
 then
-    echo -e "\e[35m\e[1mNo worries, we just test against the master branch: $IMAGE_NAME:$MASTER_TAG\e[0m"
+    echo -e "\e[35;1mNo worries, we just test against the master branch: $IMAGE_NAME:$MASTER_TAG\e[0m"
     docker pull "$IMAGE_NAME":"$MASTER_TAG"
     BRANCH_TAG=$MASTER_TAG
 fi
@@ -185,7 +202,7 @@ mkdir -p "$HOME"/.cache/pip
 
 # Run the docker image along with setting new environment variables
 # shellcheck disable=SC2086
-docker run --detach --interactive --tty -e CI="true" -e PACKAGE="$PACKAGE" -e BRANCH="$BRANCH" -e COMMIT="$COMMIT" -e PULL_REQUEST="$PULL_REQUEST" -e REF_NAME="$REF_NAME" --name tue-env --mount type=bind,source=$HOME/.ccache,target=$DOCKER_HOME/.ccache --mount type=bind,source=$HOME/.cache/pip,target=$DOCKER_HOME/.cache/pip $DOCKER_MOUNT_KNOWN_HOSTS_ARGS "$IMAGE_NAME:$BRANCH_TAG"
+docker run --detach --interactive --tty -e CI="true" -e PACKAGE="${PACKAGE}" -e BRANCH="${BRANCH}" -e COMMIT="${COMMIT}" -e PULL_REQUEST="${PULL_REQUEST}" -e REF_NAME="${REF_NAME}" --name tue-env --mount type=bind,source=${HOME}/.ccache,target=${DOCKER_HOME}/.ccache --mount type=bind,source=${HOME}/.cache/pip,target=${DOCKER_HOME}/.cache/pip "${DOCKER_MOUNT_KNOWN_HOSTS_ARGS[@]}" "${IMAGE_NAME}:${BRANCH_TAG}"
 
 # Own the ~/.ccache folder for permissions
 docker exec -t tue-env bash -c 'sudo chown "${USER}":"${USER}" -R ~/.ccache'
@@ -206,17 +223,17 @@ fi
 
 # Use docker environment variables in all exec commands instead of script variables
 # Catch the ROS_DISTRO of the docker container
-# stip carriage return from docker output by "tr -d '\r'"
-# see https://unix.stackexchange.com/a/487185
+# Stop carriage return from docker output by "tr -d '\r'"
+# See https://unix.stackexchange.com/a/487185
 ROS_DISTRO=$(docker exec tue-env bash -c 'source ~/.bashrc; echo "${ROS_DISTRO}"' | tr -d '\r')
-echo -e "\e[35m\e[1mROS_DISTRO = ${ROS_DISTRO}\e[0m"
+echo -e "\e[35;1mROS_DISTRO = ${ROS_DISTRO}\e[0m"
 
 TUE_SYSTEM_DIR=$(docker exec tue-env bash -c 'source ~/.bashrc; echo "${TUE_SYSTEM_DIR}"' | tr -d '\r')
 
 # First install only the git repo of the package so that appropriate branch can be checked out later
-echo -e "\e[35m\e[1mtue-get install ros-$PACKAGE --no-ros-deps\e[0m"
+echo -e "\e[35;1mtue-get install ros-${PACKAGE} --no-ros-deps ${ADDITIONAL_ARGS_TUE_GET[*]}\e[0m"
 docker exec -t tue-env bash -c 'echo "debconf debconf/frontend select Noninteractive" | sudo debconf-set-selections'
-docker exec -t tue-env bash -c 'source ~/.bashrc; tue-get install ros-"${PACKAGE}" --no-ros-deps'
+docker exec -t tue-env bash -c 'source ~/.bashrc; tue-get install ros-"${PACKAGE}" --no-ros-deps '"${ADDITIONAL_ARGS_TUE_GET[*]}"
 
 if [[ $PULL_REQUEST != "false" ]]
 then
@@ -228,27 +245,27 @@ then
     # After a tue-get run, we checkout forced, just to be sure.
 
     # Fetch the merged branch
-    echo -e "\e[35m\e[1mgit -C ~${TUE_SYSTEM_DIR#"${DOCKER_HOME}"}/src/$PACKAGE fetch origin $REF_NAME/$PULL_REQUEST/merge:PULLREQUEST\e[0m"
+    echo -e "\e[35;1mgit -C ~${TUE_SYSTEM_DIR#"${DOCKER_HOME}"}/src/${PACKAGE} fetch origin ${REF_NAME}/${PULL_REQUEST}/merge:PULLREQUEST\e[0m"
     docker exec -t tue-env bash -c 'source ~/.bashrc; git -C "$TUE_SYSTEM_DIR"/src/"$PACKAGE" fetch origin "$REF_NAME"/"$PULL_REQUEST"/merge:PULLREQUEST'
 
     # Install the package completely
     branch_string=${BRANCH:+" --try-branch=${BRANCH}"}
-    echo -e "\e[35m\e[1mtue-get install ros-$PACKAGE --test-depend${branch_string} --try-branch=PULLREQUEST\e[0m"
-    docker exec -t tue-env bash -c 'source ~/.bashrc; tue-get install ros-"${PACKAGE}" --test-depend --try-branch="${BRANCH}" --try-branch=PULLREQUEST'
+    echo -e "\e[35;1mtue-get install ros-${PACKAGE} --test-depend${branch_string} --try-branch=PULLREQUEST\e[0m"
+    docker exec -t tue-env bash -c 'source ~/.bashrc; tue-get install ros-"${PACKAGE}" --test-depend --try-branch="${BRANCH}" --try-branch=PULLREQUEST '"${ADDITIONAL_ARGS_TUE_GET[*]}"
 
     # Checkout -f to be really sure
-    echo -e "\e[35m\e[1mgit -C ~${TUE_SYSTEM_DIR#"${DOCKER_HOME}"}/src/$PACKAGE checkout -f PULLREQUEST --\e[0m"
+    echo -e "\e[35;1mgit -C ~${TUE_SYSTEM_DIR#"${DOCKER_HOME}"}/src/${PACKAGE} checkout -f PULLREQUEST --\e[0m"
     docker exec -t tue-env bash -c 'source ~/.bashrc; git -C "$TUE_SYSTEM_DIR"/src/"$PACKAGE" checkout -f PULLREQUEST --'
 else
     # Install the package
     branch_string=${BRANCH:+" --try-branch=${BRANCH}"}
-    echo -e "\e[35m\e[1mtue-get install ros-${PACKAGE} --test-depend${branch_string}\e[0m"
-    docker exec -t tue-env bash -c 'source ~/.bashrc; tue-get install ros-"${PACKAGE}" --test-depend --try-branch="${BRANCH}"'
+    echo -e "\e[35;1mtue-get install ros-${PACKAGE} --test-depend${branch_string}\e[0m"
+    docker exec -t tue-env bash -c 'source ~/.bashrc; tue-get install ros-"${PACKAGE}" --test-depend --try-branch="${BRANCH}" '"${ADDITIONAL_ARGS_TUE_GET[*]}"
 
     # Set the package to the right commit
-    echo -e "\e[35m\e[1mReset package to this commit\e[0m"
-    echo -e "\e[35m\e[1mgit -C ~${TUE_SYSTEM_DIR#"${DOCKER_HOME}"}/src/$PACKAGE reset --hard $COMMIT\e[0m"
-    docker exec -t tue-env bash -c 'source ~/.bashrc; git -C "$TUE_SYSTEM_DIR"/src/"$PACKAGE" reset --hard "$COMMIT"'
+    echo -e "\e[35;1mReset package to this commit\e[0m"
+    echo -e "\e[35;1mgit -C ~${TUE_SYSTEM_DIR#"${DOCKER_HOME}"}/src/${PACKAGE} reset --hard ${COMMIT}\e[0m"
+    docker exec -t tue-env bash -c 'source ~/.bashrc; git -C "${TUE_SYSTEM_DIR}"/src/"${PACKAGE}" reset --hard "${COMMIT}"'
 fi
 
 # Allow everyone to read ~/.cache/pip folder for caching inside CI pipelines
diff --git a/ci/main/action.yml b/ci/main/action.yml
index 17f45e19b..8e09bcec3 100644
--- a/ci/main/action.yml
+++ b/ci/main/action.yml
@@ -59,31 +59,39 @@ runs:
         PULLREQUEST=${PULLREQUEST:-false}
         SSH_KEY="${{ inputs.ssh-key }}"
         SSH_KNOWN_HOSTS="${{ inputs.ssh-known-hosts }}"
+        SSH_ARGS=()
         if [[ -n "${SSH_KEY}" ]]
         then
-          SSH_ARG="--ssh"
+          SSH_KEY_PATH="${HOME}"/.ssh/ci_ssh_key
+          echo -e "\e[35;1mmkdir -p ${HOME}/.ssh\e[0m"
           mkdir -p "${HOME}"/.ssh
-          echo "${SSH_KEY}" > "${HOME}"/.ssh/ci_ssh_key
-          SSH_KEY_ARG="--ssh-key=${HOME}/.ssh/ci_ssh_key"
+          echo "${SSH_KEY}" > "${SSH_KEY_PATH}"
+          SSH_ARGS+=("--ssh" "--ssh-key=${SSH_KEY_PATH}")
         fi
         if [[ -n "${SSH_KNOWN_HOSTS}" ]]
         then
-          echo -e "\e[35m\e[1m mkdir -p ${HOME}/.ssh \e[0m"
-          mkdir -p ${HOME}/.ssh
+          echo -e "\e[35;1mmkdir -p ${HOME}/.ssh\e[0m"
+          mkdir -p "${HOME}"/.ssh
           for host in ${SSH_KNOWN_HOSTS}
           do
-            echo -e "\e[35m\e[1m ssh-keyscan -t rsa -H \"${host}\" 2>&1 | tee -a ${HOME}/.ssh/known_hosts \e[0m"
+            echo -e "\e[35;1mssh-keyscan -t rsa -H \"${host}\" 2>&1 | tee -a ${HOME}/.ssh/known_hosts\e[0m"
             ssh-keyscan -t rsa -H "${host}" 2>&1 | tee -a "${HOME}"/.ssh/known_hosts
           done
         fi
+        DEBUG_ARGS=()
+        DEBUG=${RUNNER_DEBUG:-false}
+        if [[ ${DEBUG} == "true" ]]
+        then
+          DEBUG_ARGS+=("--debug")
+        fi
         ${{ github.action_path }}/../install-package.sh \
           --image=${{ inputs.image }} \
           --package=${{ inputs.package }} \
           --branch="${BRANCH}" \
           --commit="${GITHUB_SHA}" \
           --pullrequest="${PULLREQUEST}" \
-          "${SSH_ARG}" \
-          "${SSH_KEY_ARG}"
+          "${SSH_ARGS[@]}" \
+          "${DEBUG_ARGS[@]}"
         echo "::endgroup::"
       shell: bash
 
@@ -91,8 +99,15 @@ runs:
       id: build
       run: |
         echo "::group::build-package.sh"
+        DEBUG_ARGS=()
+        DEBUG=${RUNNER_DEBUG:-false}
+        if [[ ${DEBUG} == "true" ]]
+        then
+          DEBUG_ARGS+=("--debug")
+        fi
         ${{ github.action_path }}/../build-package.sh \
-          --package=${{ inputs.package }}
+          --package=${{ inputs.package }} \
+          "${DEBUG_ARGS[@]}"
         echo "::endgroup::"
       shell: bash
 
@@ -100,7 +115,14 @@ runs:
       id: test
       run: |
         echo "::group::test-package.sh"
+        DEBUG_ARGS=()
+        DEBUG=${RUNNER_DEBUG:-false}
+        if [[ ${DEBUG} == "true" ]]
+        then
+          DEBUG_ARGS+=("--debug")
+        fi
         ${{ github.action_path }}/../test-package.sh \
-          --package=${{ inputs.package }}
+          --package=${{ inputs.package }} \
+          "${DEBUG_ARGS[@]}"
         echo "::endgroup::"
       shell: bash
diff --git a/ci/mirror-upstream.bash b/ci/mirror-upstream.bash
index aa2bda7eb..a65cad34c 100755
--- a/ci/mirror-upstream.bash
+++ b/ci/mirror-upstream.bash
@@ -8,7 +8,7 @@ set -o errexit
 # Execute script only in a CI environment
 if [ "$CI" != "true" ]
 then
-    echo -e "\e[35m\e[1m Error!\e[0mTrying to execute a CI script in a non-CI environment. Exiting script."
+    echo -e "\e[35;1mError!\e[0m Trying to execute a CI script in a non-CI environment. Exiting script."
     exit 1
 fi
 
@@ -39,29 +39,29 @@ do
             # unknown option
             if [[ -n "$i" ]]  # Ignore empty arguments
             then
-                echo -e "\e[35m\e[1mUnknown input argument '$i'. Check CI .yml file\e[0m"
+                echo -e "\e[35;1mUnknown input argument '$i'. Check CI .yml file\e[0m"
                 exit 1
             fi ;;
     esac
     shift
 done
 
-echo -e "\e[35m\e[1mUPSTREAM_REMOTE_URL    = ${UPSTREAM_REMOTE_URL} \e[0m"
+echo -e "\e[35;1mUPSTREAM_REMOTE_URL    = ${UPSTREAM_REMOTE_URL} \e[0m"
 git remote add fork "$UPSTREAM_REMOTE_URL" || { echo "Could not add remote: $UPSTREAM_REMOTE"; exit 1; }
 git fetch fork || { echo "Could not fetch remote refs for $UPSTREAM_REMOTE"; exit 1; }
 
 default_upstream_remote_branch=$(git remote show fork | grep HEAD | awk '{print $3;}')
 [ -z "$UPSTREAM_REMOTE_BRANCH" ] && UPSTREAM_REMOTE_BRANCH="$default_upstream_remote_branch"
 
-echo -e "\e[35m\e[1mUPSTREAM_REMOTE_BRANCH = ${UPSTREAM_REMOTE_BRANCH} \e[0m"
-echo -e "\e[35m\e[1mLOCAL_REMOTE_URL       = ${LOCAL_REMOTE_URL} \e[0m"
-echo -e "\e[35m\e[1mLOCAL_REMOTE_BRANCH    = ${LOCAL_REMOTE_BRANCH} \e[0m"
+echo -e "\e[35;1mUPSTREAM_REMOTE_BRANCH = ${UPSTREAM_REMOTE_BRANCH} \e[0m"
+echo -e "\e[35;1mLOCAL_REMOTE_URL       = ${LOCAL_REMOTE_URL} \e[0m"
+echo -e "\e[35;1mLOCAL_REMOTE_BRANCH    = ${LOCAL_REMOTE_BRANCH} \e[0m"
 
 [ -z "$GIT_USERNAME" ] && { echo "Need User Name for git config."; exit 1; }
-echo -e "\e[35m\e[1mGIT_USERNAME           = ${GIT_USERNAME} \e[0m"
+echo -e "\e[35;1mGIT_USERNAME           = ${GIT_USERNAME} \e[0m"
 
 [ -z "$GIT_USEREMAIL" ] && { echo "Need User Email for git config."; exit 1; }
-echo -e "\e[35m\e[1mGIT_USEREMAIL          = ${GIT_USEREMAIL} \e[0m"
+echo -e "\e[35;1mGIT_USEREMAIL          = ${GIT_USEREMAIL} \e[0m"
 
 git config user.email "$GIT_USEREMAIL"
 git config user.name "$GIT_USERNAME"
@@ -76,4 +76,4 @@ git remote set-url --push origin "$LOCAL_REMOTE_URL"
 git push origin "$LOCAL_REMOTE_BRANCH"
 
 echo
-echo -e "\e[35m\e[1mSynced local branch '$LOCAL_REMOTE_BRANCH' with upstream remote branch '$UPSTREAM_REMOTE_BRANCH'"
+echo -e "\e[35;1mSynced local branch '${LOCAL_REMOTE_BRANCH}' with upstream remote branch '${UPSTREAM_REMOTE_BRANCH}'"
diff --git a/ci/organization-packages.sh b/ci/organization-packages.sh
index 0e93ce865..1d43aabfa 100644
--- a/ci/organization-packages.sh
+++ b/ci/organization-packages.sh
@@ -16,7 +16,7 @@ do
             # unknown option
             if [[ -n "${i}" ]]  # Ignore empty arguments
             then
-                echo -e "\e[35m\e[1mUnknown input argument '${i}'. Check CI .yml file\e[0m"
+                echo -e "\e[35;1mUnknown input argument '${i}'. Check CI .yml file\e[0m"
                 exit 1
             fi
             ;;
@@ -24,9 +24,9 @@ do
     shift
 done
 
-echo -e "\e[35m\e[1mORGANIZATION = ${ORGANIZATION}\e[0m"
-echo -e "\e[35m\e[1mPACKAGE_TYPE = ${PACKAGE_TYPE}\e[0m"
-echo -e "\e[35m\e[1mTOKEN length = ${#TOKEN}\e[0m"
+echo -e "\e[35;1mORGANIZATION = ${ORGANIZATION}\e[0m"
+echo -e "\e[35;1mPACKAGE_TYPE = ${PACKAGE_TYPE}\e[0m"
+echo -e "\e[35;1mTOKEN length = ${#TOKEN}\e[0m"
 
 echo "::group::curl -L -H \"Accept: application/vnd.github+json\" -H \"Authorization: Bearer \${TOKEN}\" -H \"X-GitHub-Api-Version: 2022-11-28\" \"https://api.github.com/orgs/${ORGANIZATION}/packages?package_type=${PACKAGE_TYPE}&per_page=100&page=1\""
 echo -e "\e[1mcurl -L -H \"Accept: application/vnd.github+json\" -H \"Authorization: Bearer \${TOKEN}\" -H \"X-GitHub-Api-Version: 2022-11-28\" \"https://api.github.com/orgs/${ORGANIZATION}/packages?package_type=${PACKAGE_TYPE}&per_page=100&page=1\""
@@ -36,10 +36,10 @@ echo "::endgroup::"
 
 echo -e "\e[1mjq -r '.[].name'\e[0m"
 packages=$(jq -r '.[].name' <<< "${output}")
-echo -e "\e[35m\e[1mpackages:\e[0m"
+echo -e "\e[35;1mpackages:\e[0m"
 for pkg in ${packages}
 do
-    echo -e "\e[35m\e[1m  ${pkg}\e[0m"
+    echo -e "\e[35;1m  ${pkg}\e[0m"
     packages_list="${packages_list:+$packages_list, }'${pkg}'"
 done
 packages_list="[${packages_list}]"
diff --git a/ci/organization-packages/action.yaml b/ci/organization-packages/action.yaml
index 889c35aad..cf0d1a353 100644
--- a/ci/organization-packages/action.yaml
+++ b/ci/organization-packages/action.yaml
@@ -27,6 +27,6 @@ runs:
       id: packages
       run: |
         source ${{ github.action_path }}/../organization-packages.sh --organization=${{ inputs.organization }} --type=${{ inputs.package-type }} --token=${{ inputs.token }}
-        echo -e "\e[35m\e[1mpackages=${packages_list} >> \${GITHUB_OUTPUT}\e[0m"
+        echo -e "\e[35;1mpackages=${packages_list} >> \${GITHUB_OUTPUT}\e[0m"
         echo "packages=${packages_list}" >> ${GITHUB_OUTPUT}
       shell: bash --noprofile --norc -o pipefail {0}
diff --git a/ci/packages.sh b/ci/packages.sh
index 631404145..4c6767075 100755
--- a/ci/packages.sh
+++ b/ci/packages.sh
@@ -22,11 +22,11 @@ do
     shift
 done
 
-echo -e "\e[35m\e[1mCOMMIT_RANGE = ${COMMIT_RANGE}\e[0m"
-echo -e "\e[35m\e[1mPULL_REQUEST = ${PULL_REQUEST}\e[0m"
-echo -e "\e[35m\e[1mBRANCH       = ${BRANCH}\e[0m"
-echo -e "\e[35m\e[1mALL          = ${ALL}\e[0m"
-echo -e "\e[35m\e[1mexclude_dirs = ${exclude_dirs}\e[0m"
+echo -e "\e[35;1mCOMMIT_RANGE = ${COMMIT_RANGE}\e[0m"
+echo -e "\e[35;1mPULL_REQUEST = ${PULL_REQUEST}\e[0m"
+echo -e "\e[35;1mBRANCH       = ${BRANCH}\e[0m"
+echo -e "\e[35;1mALL          = ${ALL}\e[0m"
+echo -e "\e[35;1mexclude_dirs = ${exclude_dirs}\e[0m"
 
 exclude_dirs=$(echo "${exclude_dirs}" | xargs ls -dl 2>/dev/null |  grep "^d" | grep -v "\." | awk '{print $NF}')
 
@@ -34,8 +34,8 @@ if [ -n "$COMMIT_RANGE" ]
 then
     oldest_commit=${COMMIT_RANGE%...*}
     newest_commit=${COMMIT_RANGE#*...}
-    echo -e "\e[35m\e[1mOldest commit: ${oldest_commit}\e[0m"
-    echo -e "\e[35m\e[1mNewest commit: ${newest_commit}\e[0m"
+    echo -e "\e[35;1mOldest commit: ${oldest_commit}\e[0m"
+    echo -e "\e[35;1mNewest commit: ${newest_commit}\e[0m"
 
     mod_files=$(git diff-tree --name-only "${oldest_commit}" "${newest_commit}")
 else
@@ -81,8 +81,8 @@ export PACKAGES_DICT
 # shellcheck disable=SC2090
 export PACKAGES_LIST
 
-echo -e "\e[35m\e[1mPACKAGES:\e[0m"
+echo -e "\e[35;1mPACKAGES:\e[0m"
 for PKG in $PACKAGES
 do
-    echo -e "\e[35m\e[1m  ${PKG}\e[0m"
+    echo -e "\e[35;1m  ${PKG}\e[0m"
 done
diff --git a/ci/set-branch.sh b/ci/set-branch.sh
index 2ccf9a751..489d18f2e 100755
--- a/ci/set-branch.sh
+++ b/ci/set-branch.sh
@@ -11,7 +11,7 @@ do
         # unknown option
         if [[ -n "$i" ]]  # Ignore empty arguments
         then
-            echo -e "\e[35m\e[1mUnknown input argument '$i'. Check CI .yml file\e[0m"
+            echo -e "\e[35;1mUnknown input argument '$i'. Check CI .yml file\e[0m"
             exit 1
         fi ;;
     esac
@@ -20,15 +20,15 @@ done
 
 DEFAULT_BRANCH=$(git remote show origin 2>/dev/null | grep HEAD | awk '{print $3}')
 
-echo -e "\e[35m\e[1mBRANCH         = ${BRANCH}\e[0m"
-echo -e "\e[35m\e[1mDEFAULT_BRANCH = ${DEFAULT_BRANCH}\e[0m"
+echo -e "\e[35;1mBRANCH         = ${BRANCH}\e[0m"
+echo -e "\e[35;1mDEFAULT_BRANCH = ${DEFAULT_BRANCH}\e[0m"
 
 if [[ "$BRANCH" == "$DEFAULT_BRANCH" ]]
 then
-    echo -e "\e[35m\e[1mOn the default branch, clearing BRANCH variable\e[0m"
+    echo -e "\e[35;1mOn the default branch, clearing BRANCH variable\e[0m"
     BRANCH=""
 else
-    echo -e "\e[35m\e[1mOn a feature branch, keeping the BRANCH variable: ${BRANCH}\e[0m"
+    echo -e "\e[35;1mOn a feature branch, keeping the BRANCH variable: ${BRANCH}\e[0m"
 fi
 
 export BRANCH
diff --git a/ci/test-package.sh b/ci/test-package.sh
index 684dc1b79..19419d1be 100755
--- a/ci/test-package.sh
+++ b/ci/test-package.sh
@@ -8,9 +8,9 @@
 set -o errexit
 
 # Execute script only in a CI environment
-if [ "$CI" != "true" ]
+if [[ "${CI}" != "true" ]]
 then
-    echo -e "\e[35m\e[1mError!\e[0mTrying to execute a CI script in a non-CI environment. Exiting script."
+    echo -e "\e[35;1mError!\e[0m Trying to execute a CI script in a non-CI environment. Exiting script."
     exit 1
 fi
 
@@ -21,50 +21,64 @@ do
         -p=* | --package=* )
             PACKAGE="${i#*=}" ;;
 
+        --debug )
+            DEBUG="true" ;;
+
         * )
             # unknown option
             if [[ -n "$i" ]]  # Ignore empty arguments
             then
-                echo -e "\e[35m\e[1mUnknown input argument '$i'. Check CI .yml file\e[0m"
+                echo -e "\e[35;1mUnknown input argument '$i'. Check CI .yml file\e[0m"
                 exit 1
             fi ;;
     esac
     shift
 done
 
-echo -e "\e[35m\e[1mPACKAGE     = ${PACKAGE}\e[0m"
+echo -e "\e[35;1mPACKAGE     = ${PACKAGE}\e[0m"
 ROS_VERSION=$(docker exec tue-env bash -c 'source ~/.bashrc; echo "${ROS_VERSION}"' | tr -d '\r')
-echo -e "\e[35m\e[1mROS_VERSION = ${ROS_VERSION}\e[0m"
+echo -e "\e[35;1mROS_VERSION = ${ROS_VERSION}\e[0m"
+echo -e "\e[35;1mDEBUG       = ${DEBUG}\e[0m"
 
 # If packages is non-zero, this is a multi-package repo. In multi-package repo, check if this package needs CI.
 # If a single-package repo, CI is always needed.
 # shellcheck disable=SC2153
 if [ -n "$PACKAGES" ] && ! echo "$PACKAGES" | grep -sqw "$PACKAGE"
 then
-    echo -e "\e[35m\e[1mNo changes in this package, so no need to run CI\e[0m"
-    exit 0
+    echo -e "\e[35;1mNo changes in this package, so no need to run CI\e[0m"
+h    exit 0
 fi
 
 # Use docker environment variables in all exec commands instead of script variables
 if [[ "${ROS_VERSION}" == 1 ]]
 then
+    CATKIN_ADDITIONAL_ARGS=()
+    if [[ ${DEBUG} == "true" ]]
+    then
+        CATKIN_ADDITIONAL_ARGS+=("--verbose")
+    fi
     # Build test targets
-    echo -e "\e[35m\e[1mBuild test targets of this package (catkin build --this --no-deps -DCATKIN_ENABLE_TESTING=ON)\e[0m"
-    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}"/src/"${PACKAGE}" && /usr/bin/python3 "$(command -v catkin)" build --this --no-status --no-deps -DCATKIN_ENABLE_TESTING=ON'
+    echo -e "\e[35;1mBuild test targets of this package (catkin build --this --no-deps ${CATKIN_ADDITIONAL_ARGS[*]} -DCATKIN_ENABLE_TESTING=ON)\e[0m"
+    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}"/src/"${PACKAGE}" && /usr/bin/python3 "$(command -v catkin)" build --this --no-status --no-deps '"${CATKIN_ADDITIONAL_ARGS[*]}"' -DCATKIN_ENABLE_TESTING=ON'
 
     # Run unit tests
-    echo -e "\e[35m\e[1mRun tests on this package (catkin test --this --no-deps -DCATKIN_ENABLE_TESTING=ON)\e[0m"
-    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}"/src/"${PACKAGE}" && /usr/bin/python3 "$(command -v catkin)" test --this --no-status --no-deps -DCATKIN_ENABLE_TESTING=ON'
+    echo -e "\e[35;1mRun tests on this package (catkin test --this --no-deps ${CATKIN_ADDITIONAL_ARGS[*]} -DCATKIN_ENABLE_TESTING=ON)\e[0m"
+    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}"/src/"${PACKAGE}" && /usr/bin/python3 "$(command -v catkin)" test --this --no-status --no-deps '"${CATKIN_ADDITIONAL_ARGS[*]}"' -DCATKIN_ENABLE_TESTING=ON'
 else
+    COLCON_ADDITIONAL_ARGS=()
+    if [[ ${DEBUG} == "true" ]]
+    then
+        COLCON_ADDITIONAL_ARGS+=("--log-level" "debug")
+    fi
     # Build test targets
-    echo -e "\e[35m\e[1mBuild test targets of this package (colcon build --packages-select ${PACKAGE} --mixin rel-with-deb-info build-testing-on)\e[0m"
-    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon build --packages-select "${PACKAGE}" --mixin rel-with-deb-info build-testing-on --event-handlers desktop_notification- status- terminal_title-'
+    echo -e "\e[35;1mBuild test targets of this package (colcon ${COLCON_ADDITIONAL_ARGS[*]} build --packages-select ${PACKAGE} --mixin rel-with-deb-info build-testing-on)\e[0m"
+    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon '"${COLCON_ADDITIONAL_ARGS[*]}"' build --packages-select "${PACKAGE}" --mixin rel-with-deb-info build-testing-on --event-handlers desktop_notification- status- terminal_title-'
 
     # Run unit tests
-    echo -e "\e[35m\e[1mRun tests on this package (colcon test --packages-select ${PACKAGE} --executor sequential)\e[0m"
-    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon test --packages-select "${PACKAGE}" --executor sequential --event-handlers desktop_notification- status- terminal_title- console_cohesion+'
+    echo -e "\e[35;1mRun tests on this package (colcon ${COLCON_ADDITIONAL_ARGS[*]} test --packages-select ${PACKAGE} --executor sequential)\e[0m"
+    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon '"${COLCON_ADDITIONAL_ARGS[*]}"' test --packages-select "${PACKAGE}" --executor sequential --event-handlers desktop_notification- status- terminal_title- console_cohesion+'
 
     # Check test results
-    echo -e "\e[35m\e[1mCheck test results (colcon test-result --verbose)\e[0m"
-    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon test-result --verbose'
+    echo -e "\e[35;1mCheck test results (colcon ${COLCON_ADDITIONAL_ARGS[*]} test-result --verbose)\e[0m"
+    docker exec -t tue-env bash -c 'source ~/.bashrc; cd "${TUE_SYSTEM_DIR}" && colcon '"${COLCON_ADDITIONAL_ARGS[*]}"' test-result --verbose'
 fi
diff --git a/ci/update-docker.bash b/ci/update-docker.bash
index 1b066e54f..a226373bd 100755
--- a/ci/update-docker.bash
+++ b/ci/update-docker.bash
@@ -6,7 +6,7 @@ set -o errexit
 # Execute script only in a CI environment
 if [ "$CI" != "true" ]
 then
-    echo -e "\e[35m\e[1mError!\e[0m Trying to execute a CI script in a non-CI environment. Exiting script."
+    echo -e "\e[35;1mError!\e[0m Trying to execute a CI script in a non-CI environment. Exiting script."
     exit 1
 fi
 
diff --git a/dockerfiles/tue-env.Dockerfile b/dockerfiles/tue-env.Dockerfile
index c6e259b3c..05281e138 100644
--- a/dockerfiles/tue-env.Dockerfile
+++ b/dockerfiles/tue-env.Dockerfile
@@ -63,8 +63,6 @@ RUN apt-get update -qq && \
 USER "$USER"
 WORKDIR /home/"$USER"
 
-ADD installer/bootstrap.bash ./bootstrap.bash
-
 RUN mkdir -p -m 0700 ~/.ssh
 ADD ./known_hosts ./.ssh/known_hosts
 RUN sudo chown $USER_ID:$USER_ID ~/.ssh/known_hosts && sudo chmod 644 ~/.ssh/known_hosts
@@ -73,8 +71,9 @@ RUN sudo chown $USER_ID:$USER_ID ~/.ssh/known_hosts && sudo chmod 644 ~/.ssh/kno
 RUN { [[ -n "$OAUTH2_TOKEN" ]] && git config --global credential.helper '!f() { printf "%s\n" "username=oauth2" "password=$OAUTH2_TOKEN"; };f'; } || exit 0
 
 # Setup tue-env and install target ros
+RUN --mount=type=ssh,uid=$USER_ID --mount=type=bind,source=installer/bootstrap.bash,target=bootstrap.bash \
     # Remove interactive check from bashrc, otherwise bashrc refuses to execute
-RUN --mount=type=ssh,uid=$USER_ID sed -e s/return//g -i ~/.bashrc && \
+    sed -e s/return//g -i ~/.bashrc && \
     # Set the CI args in the container as docker currently provides no method to
     # remove the environment variables
     # NOTE: The following exports will exist only in this container
@@ -92,17 +91,22 @@ RUN --mount=type=ssh,uid=$USER_ID sed -e s/return//g -i ~/.bashrc && \
     --targets-repo="${TARGETS_REPO}" && \
     # Make tue-env to be available to the environment
     source ~/.bashrc && \
-    # Install target ros
-    tue-get install ros${ROS_VERSION} --test-depend --branch="$BRANCH" && \
+    # Install optional git target
+    { tue-get install git --test-depend || true; } && \
     # Install target ccache
     tue-get install ccache --test-depend && \
+    # Install target ros
+    tue-get install ros${ROS_VERSION} --test-depend --branch="$BRANCH" && \
     # Remove temp tue files
     (rm -rf /tmp/tue_* > /dev/null || true) && \
-    # Show ownership of .tue
+    # Show ownership of ~/.tue
+    echo -e "\e[35mOwner of ~/.tue\e[0m" && \
     namei -l ~/.tue && \
     # Check git remote origin
+    echo -e "\e[35mgit -C ~/.tue remote -v\e[0m" && \
     git -C ~/.tue remote -v && \
     # Show the branches of tue-env
+    echo -e "\e[35mgit -C ~/.tue branch\e[0m" && \
     git -C ~/.tue branch && \
     # Remove docker-clean. APT will be able to autocomplete packages now
     sudo rm /etc/apt/apt.conf.d/docker-clean && \
diff --git a/setup.bash b/setup.bash
index ad81bd0b4..245e92461 100644
--- a/setup.bash
+++ b/setup.bash
@@ -1,96 +1,111 @@
 #! /usr/bin/env bash
-TUE_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
-export TUE_DIR
-
-# Load tue-env tool
-# shellcheck disable=SC1091
-source "$TUE_DIR"/setup/tue-env.bash
 
 # ------------------------------------------
 # Helper function for checking if all env vars are set
 function _tue-check-env-vars
 {
-    [ -n "$TUE_DIR" ] && [ -n "$TUE_ENV" ] && [ -n "$TUE_ENV_DIR" ] \
-       && [ -n "$TUE_BIN" ] && [ -n "$TUE_ENV_TARGETS_DIR" ] && return 0
+    [[ -n "${TUE_DIR}" ]] && [[ -n "${TUE_ENV}" ]] && [[ -n "${TUE_ENV_DIR}" ]] \
+       && [[ -n "${TUE_BIN}" ]] && [[ -n "${TUE_ENV_TARGETS_DIR}" ]] && return 0
     echo "[tue] Not all needed environment variables are set."
     return 1
 }
 export -f _tue-check-env-vars
 
-if [ -z "$TUE_ENV" ]
-then
-    if [ ! -f "$TUE_DIR"/user/config/default_env ]
+function main
+{
+    # -----------------------------------------
+    # Set the TUE_DIR variable
+    TUE_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+    export TUE_DIR
+
+    # -----------------------------------------
+    # Add `.local/bin` and TUE_DIR to PATH
+    export TUE_BIN=${TUE_DIR}/bin
+    # .local/bin is needed in the path for all user installs like pip. It gets added automatically on reboot but not on CI
+    if [[ :${PATH}: != *:${HOME}/.local/bin:* ]]
+    then
+        export PATH=${HOME}/.local/bin${PATH:+:${PATH}}
+    fi
+
+    if [[ :${PATH}: != *:${TUE_BIN}:* ]]
     then
-        # No environment, so all environment specific setup below does not need to be sourced
-        return 0
+        export PATH=${TUE_BIN}${PATH:+:${PATH}}
     fi
 
-    TUE_ENV=$(cat "$TUE_DIR"/user/config/default_env)
-    export TUE_ENV
+    # -----------------------------------------
+    # Load tue-env tool
+    # shellcheck disable=SC1091
+    source "${TUE_DIR}"/setup/tue-env.bash
 
-    if [ ! -f "$TUE_DIR"/user/envs/"$TUE_ENV" ]
+    # Load the (optional) default environment
+    if [[ -z "${TUE_ENV}" ]]
     then
-        echo "[tue] No such environment: '$TUE_ENV'"
+        [[ -f "${TUE_DIR}"/user/config/default_env ]] || return 0  # Quietly return
+
+        TUE_ENV=$(cat "${TUE_DIR}"/user/config/default_env)
+        export TUE_ENV
+
+        if [[ ! -f "${TUE_DIR}"/user/envs/"${TUE_ENV}" ]]
+        then
+            echo "[tue] No such environment: '${TUE_ENV}'"
+            return 1
+        fi
+    fi
+
+    # -----------------------------------------
+    # Set the TUE_ENV_DIR and TUE_ENV_TARGETS_DIR variables
+    TUE_ENV_DIR=$(cat "${TUE_DIR}"/user/envs/"${TUE_ENV}")
+    export TUE_ENV_DIR
+
+    if [[ ! -d "${TUE_ENV_DIR}" ]]
+    then
+        echo "[tue] Environment directory '${TUE_ENV_DIR}' (environment '${TUE_ENV}') does not exist"
         return 1
     fi
-fi
 
-TUE_ENV_DIR=$(cat "$TUE_DIR"/user/envs/"$TUE_ENV")
-export TUE_ENV_DIR
+    export TUE_ENV_TARGETS_DIR=${TUE_ENV_DIR}/.env/targets
 
-if [ ! -d "$TUE_ENV_DIR" ]
-then
-    echo "[tue] Environment directory '$TUE_ENV_DIR' (environment '$TUE_ENV') does not exist"
-    return 1
-fi
+    if [[ ! -d "${TUE_ENV_TARGETS_DIR}" ]]
+    then
+        echo "[tue] Targets directory '${TUE_ENV_TARGETS_DIR}' (environment '${TUE_ENV}') does not exist"
+        return 1
+    fi
 
-export TUE_ENV_TARGETS_DIR=$TUE_ENV_DIR/.env/targets
+    # -----------------------------------------
+    # Load the user setup file
+    if [[ -f "${TUE_ENV_DIR}"/.env/setup/user_setup.bash ]]
+    then
+        # shellcheck disable=SC1091
+        source "${TUE_ENV_DIR}"/.env/setup/user_setup.bash
+    fi
 
-if [ ! -d "$TUE_ENV_TARGETS_DIR" ]
-then
-    echo "[tue] Targets directory '$TUE_ENV_TARGETS_DIR' (environment '$TUE_ENV') does not exist"
-    return 1
-fi
+    # -----------------------------------------
+    # Load the python virtual environment if it exists
+    if [[ -d "${TUE_ENV_DIR}"/.venv/"${TUE_ENV}" ]]
+    then
+        # shellcheck disable=SC1090
+        source "${TUE_ENV_DIR}"/.venv/"${TUE_ENV}"/bin/activate
+    fi
 
-if [ -f "$TUE_ENV_DIR"/.env/setup/user_setup.bash ]
-then
+    # -----------------------------------------
+    # Load all the bash functions
     # shellcheck disable=SC1091
-    source "$TUE_ENV_DIR"/.env/setup/user_setup.bash
-fi
+    source "${TUE_DIR}"/setup/tue-functions.bash
+
+    if [[ -f "${TUE_DIR}"/setup/tue-misc.bash ]]
+    then
+        # shellcheck disable=SC1091
+        source "${TUE_DIR}"/setup/tue-misc.bash
+    fi
 
-# -----------------------------------------
-# Load all the bash functions
-# shellcheck disable=SC1091
-source "$TUE_DIR"/setup/tue-functions.bash
+    # -----------------------------------------
+    # Load the target setup files
+    # The target setup files could depend on anything that was loaded above
+    if [[ -f "${TUE_ENV_DIR}"/.env/setup/target_setup.bash ]]
+    then
+        # shellcheck disable=SC1091
+        source "${TUE_ENV_DIR}"/.env/setup/target_setup.bash
+    fi
+}
 
-if [ -f "$TUE_DIR"/setup/tue-misc.bash ]
-then
-    # shellcheck disable=SC1091
-    source "$TUE_DIR"/setup/tue-misc.bash
-fi
-
-export TUE_BIN=$TUE_DIR/bin
-
-# .local/bin is needed in the path for all user installs like pip. It gets added automatically on reboot but not on CI
-if [[ :$PATH: != *:$HOME/.local/bin:* ]]
-then
-    export PATH=$HOME/.local/bin${PATH:+:${PATH}}
-fi
-
-if [[ :$PATH: != *:$TUE_BIN:* ]]
-then
-    export PATH=$TUE_BIN${PATH:+:${PATH}}
-fi
-
-# Source the python virtual environment if it exists
-if [[ -d "${TUE_ENV_DIR}"/.venv/"${TUE_ENV}" ]]
-then
-    # shellcheck disable=SC1090
-    source "${TUE_ENV_DIR}"/.venv/"${TUE_ENV}"/bin/activate
-fi
-
-if [ -f "$TUE_ENV_DIR"/.env/setup/target_setup.bash ]
-then
-    # shellcheck disable=SC1091
-    source "$TUE_ENV_DIR"/.env/setup/target_setup.bash
-fi
+main "$@"