diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile deleted file mode 100644 index 60f66bc0..00000000 --- a/.devcontainer/Dockerfile +++ /dev/null @@ -1,22 +0,0 @@ -FROM qzhhhi/rmcs-develop:latest - -SHELL ["/bin/bash", "-lc"] - -USER root - -RUN apt-get update && apt-get install -y --no-install-recommends \ - nodejs npm && \ - npm install -g @openai/codex && \ - apt-get autoremove -y && apt-get clean && \ - rm -rf /var/lib/apt/lists/* /tmp/* - -USER ubuntu -WORKDIR /home/ubuntu - -RUN curl -fsSL https://opencode.ai/install | bash && \ - if ! grep -q '/.opencode/bin' ~/.bashrc; then printf '\n# opencode\nexport PATH="$HOME/.opencode/bin:$PATH"\n' >> ~/.bashrc; fi && \ - if ! grep -q '/.opencode/bin' ~/.profile; then printf '\n# opencode\nexport PATH="$HOME/.opencode/bin:$PATH"\n' >> ~/.profile; fi && \ - if ! grep -q '/.opencode/bin' ~/.zshrc; then printf '\n# opencode\nexport PATH="$HOME/.opencode/bin:$PATH"\n' >> ~/.zshrc; fi && \ - mkdir -p ~/.opencode/skills - -ENV PATH="/home/ubuntu/.opencode/bin:${PATH}" diff --git a/.devcontainer/bootstrap-agent-state.sh b/.devcontainer/bootstrap-agent-state.sh deleted file mode 100644 index 3bafcee1..00000000 --- a/.devcontainer/bootstrap-agent-state.sh +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env bash - -set -euo pipefail - -is_uninitialized_dir() { - local dir="$1" - local entries - - shopt -s dotglob nullglob - entries=("${dir}"/*) - shopt -u dotglob nullglob - - if [ "${#entries[@]}" -eq 0 ]; then - return 0 - fi - - if [ "${#entries[@]}" -eq 1 ] && [ "$(basename "${entries[0]}")" = ".gitignore" ]; then - return 0 - fi - - return 1 -} - -copy_dir_contents() { - local src="$1" - local dst="$2" - - mkdir -p "${dst}" - cp -a "${src}"/. "${dst}"/ -} - -bootstrap_dir() { - local src="$1" - local dst="$2" - local label="$3" - - mkdir -p "${dst}" - - if ! is_uninitialized_dir "${dst}"; then - echo "[bootstrap-agent-state] Skip ${label}: state already initialized" - return 0 - fi - - if [ ! -d "${src}" ]; then - echo "[bootstrap-agent-state] Skip ${label}: source ${src} not found" - return 0 - fi - - echo "[bootstrap-agent-state] Importing ${label} from ${src}" - copy_dir_contents "${src}" "${dst}" -} - -bootstrap_dir "/mnt/host-agent-source/codex" "${HOME}/.codex" "Codex state" -bootstrap_dir "/mnt/host-agent-source/opencode-config" "${HOME}/.config/opencode" "OpenCode config" -bootstrap_dir "/mnt/host-agent-source/opencode-data" "${HOME}/.local/share/opencode" "OpenCode data" -bootstrap_dir "/mnt/host-agent-source/opencode-local-state" "${HOME}/.local/state/opencode" "OpenCode local state" -bootstrap_dir "/mnt/host-agent-source/opencode-cache" "${HOME}/.cache/opencode" "OpenCode cache" -bootstrap_dir "/mnt/host-agent-source/oh-my-opencode-data" "${HOME}/.local/share/oh-my-opencode" "Oh My OpenCode data" -bootstrap_dir "/mnt/host-agent-source/opencode-skills" "${HOME}/.opencode/skills" "OpenCode skills" diff --git a/.devcontainer/state/.gitignore b/.devcontainer/state/.gitignore deleted file mode 100644 index c6bf8540..00000000 --- a/.devcontainer/state/.gitignore +++ /dev/null @@ -1,6 +0,0 @@ -* -!.gitignore -!codex/ -!opencode-config/ -!opencode-data/ -!opencode-skills/ diff --git a/.devcontainer/state/codex/.gitignore b/.devcontainer/state/codex/.gitignore deleted file mode 100644 index d6b7ef32..00000000 --- a/.devcontainer/state/codex/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -* -!.gitignore diff --git a/.devcontainer/state/opencode-config/.gitignore b/.devcontainer/state/opencode-config/.gitignore deleted file mode 100644 index d6b7ef32..00000000 --- a/.devcontainer/state/opencode-config/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -* -!.gitignore diff --git a/.devcontainer/state/opencode-data/.gitignore b/.devcontainer/state/opencode-data/.gitignore deleted file mode 100644 index d6b7ef32..00000000 --- a/.devcontainer/state/opencode-data/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -* -!.gitignore diff --git a/.devcontainer/state/opencode-skills/.gitignore b/.devcontainer/state/opencode-skills/.gitignore deleted file mode 100644 index d6b7ef32..00000000 --- a/.devcontainer/state/opencode-skills/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -* -!.gitignore diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml index 435d3f46..78e039a0 100644 --- a/.github/workflows/update-image.yml +++ b/.github/workflows/update-image.yml @@ -3,49 +3,344 @@ name: Build and Push RMCS Images on: workflow_dispatch: push: - paths: - - 'Dockerfile' branches: - - main + - main + paths: + - Dockerfile + - .github/workflows/update-image.yml + - .script/build-rmcs-cross + - rmcs_ws/toolchain.cmake + +env: + REGISTRY_IMAGE_BASE: qzhhhi/rmcs-base + REGISTRY_IMAGE_DEVELOP: qzhhhi/rmcs-develop + REGISTRY_IMAGE_RUNTIME: qzhhhi/rmcs-runtime jobs: - build-and-push: - runs-on: ubuntu-latest + build_base: + strategy: + fail-fast: false + matrix: + include: + - arch: amd64 + runner: ubuntu-latest + - arch: arm64 + runner: ubuntu-24.04-arm + runs-on: ${{ matrix.runner }} + steps: + - name: Checkout repository + uses: actions/checkout@v6 + + - name: Log in to Docker Hub + uses: docker/login-action@v4 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v4 + + - name: Build and push by digest (Base) + id: build_base + uses: docker/build-push-action@v7 + with: + context: . + target: rmcs-base + platforms: linux/${{ matrix.arch }} + outputs: type=image,name=${{ env.REGISTRY_IMAGE_BASE }},push-by-digest=true,name-canonical=true,push=true + cache-from: type=gha,scope=docker-build-base-${{ matrix.arch }} + cache-to: type=gha,mode=max,scope=docker-build-base-${{ matrix.arch }} + + - name: Export digest (Base) + run: | + mkdir -p "${{ runner.temp }}/digests-base" + printf '%s\n' "${{ steps.build_base.outputs.digest }}" > "${{ runner.temp }}/digests-base/${{ matrix.arch }}.digest" + + - name: Upload digest (Base) + uses: actions/upload-artifact@v6 + with: + name: digests-base-${{ matrix.arch }} + path: ${{ runner.temp }}/digests-base/*.digest + if-no-files-found: error + retention-days: 1 + + build_images: + needs: build_base + strategy: + fail-fast: false + matrix: + include: + - arch: amd64 + runner: ubuntu-latest + - arch: arm64 + runner: ubuntu-24.04-arm + runs-on: ${{ matrix.runner }} steps: - - name: Checkout repository - uses: actions/checkout@v4 - - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v3 - - - name: Log in to Docker Hub - uses: docker/login-action@v3 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - - name: Set up SSH keys - run: | - mkdir -p .ssh - chmod 700 .ssh - echo "${{ secrets.CONTAINER_ID_RSA }}" > .ssh/id_rsa - echo "${{ secrets.CONTAINER_ID_RSA_PUB }}" > .ssh/id_rsa.pub - chmod 600 .ssh/id_rsa - chmod 644 .ssh/id_rsa.pub - - - name: Build and push rmcs-develop:latest - uses: docker/build-push-action@v6 - with: - context: . - push: true - target: rmcs-develop - tags: qzhhhi/rmcs-develop:latest - - - name: Build and push rmcs-runtime:latest - uses: docker/build-push-action@v6 - with: - context: . - push: true - target: rmcs-runtime - tags: qzhhhi/rmcs-runtime:latest + - name: Checkout repository + uses: actions/checkout@v6 + + - name: Log in to Docker Hub + uses: docker/login-action@v4 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v4 + + - name: Set up SSH keys + run: | + mkdir -p .ssh + chmod 700 .ssh + echo "${{ secrets.CONTAINER_ID_RSA }}" > .ssh/id_rsa + echo "${{ secrets.CONTAINER_ID_RSA_PUB }}" > .ssh/id_rsa.pub + chmod 600 .ssh/id_rsa + chmod 644 .ssh/id_rsa.pub + + - name: Download digests (Base) + uses: actions/download-artifact@v7 + with: + path: ${{ runner.temp }}/digests/base + pattern: digests-base-* + merge-multiple: true + + - name: Resolve base digests + id: resolve + run: | + set -euo pipefail + base_dir="${{ runner.temp }}/digests/base" + base_digest_amd64="$(cat "${base_dir}/amd64.digest")" + base_digest_arm64="$(cat "${base_dir}/arm64.digest")" + echo "base_digest_amd64=${base_digest_amd64}" >> "$GITHUB_OUTPUT" + echo "base_digest_arm64=${base_digest_arm64}" >> "$GITHUB_OUTPUT" + + - name: Build and push by digest (Develop) + id: build_develop + uses: docker/build-push-action@v7 + with: + context: . + target: rmcs-develop + platforms: linux/${{ matrix.arch }} + outputs: type=image,name=${{ env.REGISTRY_IMAGE_DEVELOP }},push-by-digest=true,name-canonical=true,push=true + cache-from: type=gha,scope=docker-build-develop-${{ matrix.arch }} + cache-to: type=gha,mode=max,scope=docker-build-develop-${{ matrix.arch }} + + - name: Export digest (Develop) + run: | + mkdir -p "${{ runner.temp }}/digests-develop-standard" + printf '%s\n' "${{ steps.build_develop.outputs.digest }}" > "${{ runner.temp }}/digests-develop-standard/${{ matrix.arch }}.digest" + + - name: Upload digest (Develop) + uses: actions/upload-artifact@v6 + with: + name: digests-develop-standard-${{ matrix.arch }} + path: ${{ runner.temp }}/digests-develop-standard/*.digest + if-no-files-found: error + retention-days: 1 + + - name: Build and push by digest (Develop Full) + id: build_develop_full + uses: docker/build-push-action@v7 + with: + context: . + target: rmcs-develop-full + platforms: linux/${{ matrix.arch }} + build-args: | + SYSROOT_IMAGE_AMD64=${{ env.REGISTRY_IMAGE_BASE }}@${{ steps.resolve.outputs.base_digest_amd64 }} + SYSROOT_IMAGE_ARM64=${{ env.REGISTRY_IMAGE_BASE }}@${{ steps.resolve.outputs.base_digest_arm64 }} + outputs: type=image,name=${{ env.REGISTRY_IMAGE_DEVELOP }},push-by-digest=true,name-canonical=true,push=true + cache-from: type=gha,scope=docker-build-develop-full-${{ matrix.arch }} + cache-to: type=gha,mode=max,scope=docker-build-develop-full-${{ matrix.arch }} + + - name: Export digest (Develop Full) + run: | + mkdir -p "${{ runner.temp }}/digests-develop-full" + printf '%s\n' "${{ steps.build_develop_full.outputs.digest }}" > "${{ runner.temp }}/digests-develop-full/${{ matrix.arch }}.digest" + + - name: Upload digest (Develop Full) + uses: actions/upload-artifact@v6 + with: + name: digests-develop-full-${{ matrix.arch }} + path: ${{ runner.temp }}/digests-develop-full/*.digest + if-no-files-found: error + retention-days: 1 + + - name: Build and push by digest (Runtime) + id: build_runtime + uses: docker/build-push-action@v7 + with: + context: . + target: rmcs-runtime + platforms: linux/${{ matrix.arch }} + outputs: type=image,name=${{ env.REGISTRY_IMAGE_RUNTIME }},push-by-digest=true,name-canonical=true,push=true + cache-from: type=gha,scope=docker-build-runtime-${{ matrix.arch }} + cache-to: type=gha,mode=max,scope=docker-build-runtime-${{ matrix.arch }} + + - name: Export digest (Runtime) + run: | + mkdir -p "${{ runner.temp }}/digests-runtime" + printf '%s\n' "${{ steps.build_runtime.outputs.digest }}" > "${{ runner.temp }}/digests-runtime/${{ matrix.arch }}.digest" + + - name: Upload digest (Runtime) + uses: actions/upload-artifact@v6 + with: + name: digests-runtime-${{ matrix.arch }} + path: ${{ runner.temp }}/digests-runtime/*.digest + if-no-files-found: error + retention-days: 1 + + verify_images: + needs: build_images + strategy: + fail-fast: false + matrix: + include: + - arch: amd64 + runner: ubuntu-latest + cross_target: arm64 + cross_triplet: aarch64-linux-gnu + expected_machine: AArch64 + - arch: arm64 + runner: ubuntu-24.04-arm + cross_target: amd64 + cross_triplet: x86_64-linux-gnu + expected_machine: X86-64 + runs-on: ${{ matrix.runner }} + steps: + - name: Checkout repository + uses: actions/checkout@v6 + + - name: Log in to Docker Hub + uses: docker/login-action@v4 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + + - name: Download digests (Develop Full) + uses: actions/download-artifact@v7 + with: + path: ${{ runner.temp }}/digests/develop-full + pattern: digests-develop-full-* + merge-multiple: true + + - name: Resolve develop-full digest + id: resolve + run: | + set -euo pipefail + digest_file="${{ runner.temp }}/digests/develop-full/${{ matrix.arch }}.digest" + image_digest="$(cat "${digest_file}")" + echo "image_digest=${image_digest}" >> "$GITHUB_OUTPUT" + + - name: Smoke test (cross build + readelf) + env: + IMAGE_DIGEST: ${{ steps.resolve.outputs.image_digest }} + CROSS_TARGET: ${{ matrix.cross_target }} + CROSS_TRIPLET: ${{ matrix.cross_triplet }} + EXPECTED_MACHINE: ${{ matrix.expected_machine }} + run: | + set -euo pipefail + uid="$(id -u)" + gid="$(id -g)" + docker run --rm \ + --platform "linux/${{ matrix.arch }}" \ + --user "${uid}:${gid}" \ + -e HOME=/tmp/rmcs-home \ + -e RMCS_PATH=/workspaces/RMCS \ + -e CROSS_TARGET="${CROSS_TARGET}" \ + -e CROSS_TRIPLET="${CROSS_TRIPLET}" \ + -e EXPECTED_MACHINE="${EXPECTED_MACHINE}" \ + -v "$PWD:/workspaces/RMCS" \ + -w /workspaces/RMCS \ + "${{ env.REGISTRY_IMAGE_DEVELOP }}@${IMAGE_DIGEST}" \ + bash -lc ' + set -euo pipefail + mkdir -p "${HOME}" + test -d "/opt/sysroots/${CROSS_TARGET}" + command -v "${CROSS_TRIPLET}-gcc" + command -v "${CROSS_TRIPLET}-g++" + ./.script/build-rmcs-cross --target-arch "${CROSS_TARGET}" --packages-up-to rmcs_executor + lib_path="/workspaces/RMCS/rmcs_ws/install-cross-${CROSS_TARGET}/lib/librmcs_executor.so" + if [[ ! -f "${lib_path}" ]]; then + echo "Missing library: ${lib_path}" >&2 + exit 1 + fi + readelf -h "${lib_path}" | grep -q "Machine:.*${EXPECTED_MACHINE}" + ' + + promote: + runs-on: ubuntu-latest + needs: + - build_base + - build_images + - verify_images + steps: + - name: Download digests (Base) + uses: actions/download-artifact@v7 + with: + path: ${{ runner.temp }}/digests/base + pattern: digests-base-* + merge-multiple: true + + - name: Download digests (Develop) + uses: actions/download-artifact@v7 + with: + path: ${{ runner.temp }}/digests/develop-standard + pattern: digests-develop-standard-* + merge-multiple: true + + - name: Download digests (Develop Full) + uses: actions/download-artifact@v7 + with: + path: ${{ runner.temp }}/digests/develop-full + pattern: digests-develop-full-* + merge-multiple: true + + - name: Download digests (Runtime) + uses: actions/download-artifact@v7 + with: + path: ${{ runner.temp }}/digests/runtime + pattern: digests-runtime-* + merge-multiple: true + + - name: Log in to Docker Hub + uses: docker/login-action@v4 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v4 + + - name: Create manifest lists and push + run: | + set -euo pipefail + + create_manifest() { + local image="$1" + local tag="$2" + local digest_dir="$3" + local refs=() + + while IFS= read -r digest_file; do + refs+=("${image}@$(cat "${digest_file}")") + done < <(find "${digest_dir}" -maxdepth 1 -type f -name '*.digest' | sort) + + if [[ ${#refs[@]} -eq 0 ]]; then + echo "No digest files found in ${digest_dir}" >&2 + exit 1 + fi + + docker buildx imagetools create -t "${image}:${tag}" "${refs[@]}" + } + + create_manifest "${{ env.REGISTRY_IMAGE_BASE }}" "latest" "${{ runner.temp }}/digests/base" + create_manifest "${{ env.REGISTRY_IMAGE_DEVELOP }}" "latest" "${{ runner.temp }}/digests/develop-standard" + create_manifest "${{ env.REGISTRY_IMAGE_DEVELOP }}" "latest-full" "${{ runner.temp }}/digests/develop-full" + create_manifest "${{ env.REGISTRY_IMAGE_RUNTIME }}" "latest" "${{ runner.temp }}/digests/runtime" + + - name: Inspect images + run: | + docker buildx imagetools inspect "${{ env.REGISTRY_IMAGE_BASE }}:latest" + docker buildx imagetools inspect "${{ env.REGISTRY_IMAGE_DEVELOP }}:latest" + docker buildx imagetools inspect "${{ env.REGISTRY_IMAGE_DEVELOP }}:latest-full" + docker buildx imagetools inspect "${{ env.REGISTRY_IMAGE_RUNTIME }}:latest" diff --git a/.gitignore b/.gitignore index 667e7150..3754f60b 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,9 @@ devel/ log/ build/ install/ +log-cross-*/ +build-cross-*/ +install-cross-*/ bin/ lib/ msg_gen/ @@ -70,22 +73,4 @@ record/ # For temperary development develop_ws -.venv - -# Dev container persistent state -/.devcontainer/state/* -!/.devcontainer/state/.gitignore -!/.devcontainer/state/codex/ -!/.devcontainer/state/codex/.gitignore -!/.devcontainer/state/opencode-cache/ -!/.devcontainer/state/opencode-cache/.gitignore -!/.devcontainer/state/opencode-config/ -!/.devcontainer/state/opencode-config/.gitignore -!/.devcontainer/state/opencode-data/ -!/.devcontainer/state/opencode-data/.gitignore -!/.devcontainer/state/oh-my-opencode-data/ -!/.devcontainer/state/oh-my-opencode-data/.gitignore -!/.devcontainer/state/opencode-local-state/ -!/.devcontainer/state/opencode-local-state/.gitignore -!/.devcontainer/state/opencode-skills/ -!/.devcontainer/state/opencode-skills/.gitignore +.venv \ No newline at end of file diff --git a/.script/build-rmcs b/.script/build-rmcs index 2c24a7ed..a95deed0 100755 --- a/.script/build-rmcs +++ b/.script/build-rmcs @@ -11,6 +11,46 @@ fi cd "${RMCS_PATH}"/rmcs_ws || exit 1 +check_default_dir_restorable() { + local link_name="$1" + local expected_prefix="$2" + + if [[ ! -L "${link_name}" ]]; then + return + fi + + local target + target="$(readlink "${link_name}")" + if [[ "${target}" != "${expected_prefix}"-cross-* && "${target}" != */"${expected_prefix}"-cross-* ]]; then + echo "> ERROR: ${RMCS_PATH}/rmcs_ws/${link_name} is a symlink to ${target}." + echo "> It is not a build-rmcs-cross default link. Fix it before running build-rmcs." + exit 1 + fi +} + +restore_default_dir() { + local link_name="$1" + + if [[ ! -L "${link_name}" ]]; then + return + fi + + local target + target="$(readlink "${link_name}")" + + rm "${link_name}" || exit 1 + mkdir -p "${link_name}" || exit 1 + echo "> Restored ${RMCS_PATH}/rmcs_ws/${link_name} from ${target} symlink." +} + +check_default_dir_restorable build build +check_default_dir_restorable install install +check_default_dir_restorable log log + +restore_default_dir build +restore_default_dir install +restore_default_dir log + [[ -x /opt/cmake/bin/cmake ]] && export PATH="/opt/cmake/bin:$PATH" diff --git a/.script/build-rmcs-cross b/.script/build-rmcs-cross new file mode 100755 index 00000000..488054da --- /dev/null +++ b/.script/build-rmcs-cross @@ -0,0 +1,205 @@ +#!/bin/bash + +set -euo pipefail + +: "${RMCS_PATH:=/workspaces/RMCS}" + +usage() { + cat <<'EOF' +Usage: + build-rmcs-cross --target-arch [--link-default] [colcon build args...] + +Examples: + build-rmcs-cross --target-arch arm64 + build-rmcs-cross --target-arch arm64 --link-default + build-rmcs-cross --target-arch amd64 --packages-up-to rmcs_executor +EOF +} + +target_arch="" +link_default=0 +colcon_args=() + +while (($# > 0)); do + case "$1" in + --target-arch) + if (($# < 2)); then + echo "> ERROR: --target-arch requires a value." + usage + exit 1 + fi + target_arch="$2" + shift 2 + ;; + --target-arch=*) + target_arch="${1#*=}" + shift + ;; + --link-default) + link_default=1 + shift + ;; + -h | --help) + usage + exit 0 + ;; + *) + colcon_args+=("$1") + shift + ;; + esac +done + +if [[ -z "${target_arch}" ]]; then + echo "> ERROR: --target-arch is required." + usage + exit 1 +fi + +case "${target_arch}" in +arm64) + target_triplet="aarch64-linux-gnu" + ;; +amd64) + target_triplet="x86_64-linux-gnu" + ;; +*) + echo "> ERROR: Unsupported target arch '${target_arch}'. Use arm64 or amd64." + exit 1 + ;; +esac + +workspace="${RMCS_PATH}/rmcs_ws" +toolchain_file="${workspace}/toolchain.cmake" +sysroot="${RMCS_SYSROOT:-/opt/sysroots/${target_arch}}" + +if [[ ! -d "${workspace}" ]]; then + echo "> ERROR: Workspace not found: ${workspace}" + exit 1 +fi + +if [[ ! -f "${toolchain_file}" ]]; then + echo "> ERROR: Toolchain file not found: ${toolchain_file}" + exit 1 +fi + +if [[ ! -d "${sysroot}" ]]; then + echo "> ERROR: Sysroot not found: ${sysroot}" + exit 1 +fi + +if ! command -v "${target_triplet}-gcc" &>/dev/null; then + echo "> ERROR: Missing cross compiler: ${target_triplet}-gcc" + exit 1 +fi + +if ! command -v "${target_triplet}-g++" &>/dev/null; then + echo "> ERROR: Missing cross compiler: ${target_triplet}-g++" + exit 1 +fi + +unset AMENT_PREFIX_PATH +unset CMAKE_PREFIX_PATH +unset COLCON_PREFIX_PATH +unset AMENT_CURRENT_PREFIX +unset PKG_CONFIG_PATH +unset PKG_CONFIG_LIBDIR +unset PKG_CONFIG_SYSROOT_DIR +unset ROS_PACKAGE_PATH +unset PYTHONPATH +unset PYTHONHOME +unset LD_LIBRARY_PATH +unset LIBRARY_PATH +unset CPATH +unset C_INCLUDE_PATH +unset CPLUS_INCLUDE_PATH +unset CFLAGS +unset CXXFLAGS +unset CPPFLAGS +unset LDFLAGS +unset CMAKE_INCLUDE_PATH +unset CMAKE_LIBRARY_PATH +unset CMAKE_PROGRAM_PATH +unset CMAKE_TOOLCHAIN_FILE +unset CMAKE_GENERATOR + +restore_nounset=0 +case "$-" in +*u*) + restore_nounset=1 + set +u + ;; +esac +source /opt/ros/jazzy/setup.bash +if ((restore_nounset)); then + set -u +fi + +export RMCS_TARGET_ARCH="${target_arch}" +export RMCS_TARGET_TRIPLET="${target_triplet}" +export RMCS_SYSROOT="${sysroot}" + +export CC="$(command -v "${target_triplet}-gcc")" +export CXX="$(command -v "${target_triplet}-g++")" + +export CMAKE_PREFIX_PATH="${RMCS_SYSROOT}/opt/ros/jazzy:${RMCS_SYSROOT}/usr/local:${RMCS_SYSROOT}/usr" +export AMENT_PREFIX_PATH="${RMCS_SYSROOT}/opt/ros/jazzy" +export COLCON_PREFIX_PATH="${RMCS_SYSROOT}/opt/ros/jazzy" +export PKG_CONFIG_PATH="" + +export PKG_CONFIG_SYSROOT_DIR="${RMCS_SYSROOT}" +export PKG_CONFIG_LIBDIR="${RMCS_SYSROOT}/usr/local/lib/${RMCS_TARGET_TRIPLET}/pkgconfig:${RMCS_SYSROOT}/usr/local/lib/pkgconfig:${RMCS_SYSROOT}/usr/local/share/pkgconfig:${RMCS_SYSROOT}/usr/lib/${RMCS_TARGET_TRIPLET}/pkgconfig:${RMCS_SYSROOT}/usr/lib/pkgconfig:${RMCS_SYSROOT}/usr/share/pkgconfig:${RMCS_SYSROOT}/lib/${RMCS_TARGET_TRIPLET}/pkgconfig:${RMCS_SYSROOT}/lib/pkgconfig:${RMCS_SYSROOT}/opt/ros/jazzy/lib/${RMCS_TARGET_TRIPLET}/pkgconfig:${RMCS_SYSROOT}/opt/ros/jazzy/lib/pkgconfig:${RMCS_SYSROOT}/opt/ros/jazzy/share/pkgconfig" + +cd "${workspace}" + +build_base="build-cross-${RMCS_TARGET_ARCH}" +install_base="install-cross-${RMCS_TARGET_ARCH}" +log_base="log-cross-${RMCS_TARGET_ARCH}" + +check_default_linkable() { + local target="$1" + local link_name="$2" + + if [[ ! -d "${target}" ]]; then + echo "> ERROR: Cross build output not found: ${workspace}/${target}" + exit 1 + fi + + if [[ -e "${link_name}" && ! -L "${link_name}" ]]; then + echo "> ERROR: Cannot link ${workspace}/${link_name} -> ${target}." + echo "> ${workspace}/${link_name} exists and is not a symlink. Move or remove it first." + exit 1 + fi +} + +link_default_base() { + local target="$1" + local link_name="$2" + + ln -sfnT "${target}" "${link_name}" + echo "> Linked ${workspace}/${link_name} -> ${target}" +} + +CLICOLOR_FORCE=1 NINJA_STATUS="" \ + colcon \ + --log-base "${log_base}" \ + build \ + --merge-install \ + --build-base "${build_base}" \ + --install-base "${install_base}" \ + "${colcon_args[@]}" \ + --cmake-args \ + -DCMAKE_TOOLCHAIN_FILE="${toolchain_file}" \ + -DRMCS_TARGET_ARCH="${RMCS_TARGET_ARCH}" \ + -DRMCS_SYSROOT="${RMCS_SYSROOT}" \ + -DRMCS_TARGET_TRIPLET="${RMCS_TARGET_TRIPLET}" + +if ((link_default)); then + check_default_linkable "${build_base}" build + check_default_linkable "${install_base}" install + check_default_linkable "${log_base}" log + + link_default_base "${build_base}" build + link_default_base "${install_base}" install + link_default_base "${log_base}" log +fi diff --git a/.script/complete/_build-rmcs-cross b/.script/complete/_build-rmcs-cross new file mode 100644 index 00000000..34d609c2 --- /dev/null +++ b/.script/complete/_build-rmcs-cross @@ -0,0 +1,6 @@ +#compdef build-rmcs-cross + +_arguments \ + '--target-arch=[Cross compile target architecture]:target:(arm64 amd64)' \ + '--link-default[Link build/install/log to cross build output directories]' \ + '*:colcon build args:' diff --git a/.script/host/rmcs b/.script/host/rmcs index 0b65f932..f94a7cf7 100755 --- a/.script/host/rmcs +++ b/.script/host/rmcs @@ -3,7 +3,7 @@ set -euo pipefail readonly DEVELOPER_NAME="ubuntu" -readonly NVIM_PATH="/opt/nvim-linux-x86_64/bin/nvim" +readonly NVIM_PATH="/opt/nvim/bin/nvim" readonly NVIM_PORT=6666 readonly NVIM_HOST="localhost" diff --git a/Dockerfile b/Dockerfile index d9fe2f27..ce3b6a57 100644 --- a/Dockerfile +++ b/Dockerfile @@ -3,8 +3,16 @@ +# Sysroot source images for rmcs-develop-full. +# CI overrides these with digest-pinned references. For local builds, build +# rmcs-base for both architectures first and pass --build-arg explicitly; +# see docs/zh-cn/build_docker_image.md. +ARG SYSROOT_IMAGE_AMD64=rmcs-base:latest +ARG SYSROOT_IMAGE_ARM64=rmcs-base:latest + # Base container, provides a runtime environment FROM ros:jazzy AS rmcs-base +ARG TARGETARCH # Change bash as default shell instead of sh SHELL ["/bin/bash", "-c"] @@ -37,14 +45,20 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # Install openvino runtime -RUN wget https://apt.repos.intel.com/intel-gpg-keys/GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ - apt-key add ./GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ - rm ./GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ - echo "deb https://apt.repos.intel.com/openvino ubuntu24 main" > /etc/apt/sources.list.d/intel-openvino.list && \ - apt-get update && \ - apt-get install -y --no-install-recommends openvino-2025.2.0 && \ - apt-get autoremove -y && apt-get clean && \ - rm -rf /var/lib/apt/lists/* /tmp/* +RUN if [ "${TARGETARCH}" = "amd64" ]; then \ + mkdir -p /etc/apt/keyrings && \ + wget -O /etc/apt/keyrings/intel-openvino.asc \ + https://apt.repos.intel.com/intel-gpg-keys/GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ + chmod 644 /etc/apt/keyrings/intel-openvino.asc && \ + echo "deb [signed-by=/etc/apt/keyrings/intel-openvino.asc] https://apt.repos.intel.com/openvino ubuntu24 main" \ + > /etc/apt/sources.list.d/intel-openvino.list && \ + apt-get update && \ + apt-get install -y --no-install-recommends openvino-2025.2.0 && \ + apt-get autoremove -y && apt-get clean && \ + rm -rf /var/lib/apt/lists/* /tmp/*; \ + else \ + echo "Skipping OpenVINO install on ${TARGETARCH}"; \ + fi # Install Livox SDK RUN git clone https://github.com/Livox-SDK/Livox-SDK2.git && \ @@ -63,17 +77,28 @@ RUN --mount=type=bind,target=/rmcs_ws/src,source=rmcs_ws/src,readonly \ apt-get autoremove -y && apt-get clean && \ rm -rf /var/lib/apt/lists/* /tmp/* -# Install unison to allow file synchronization -RUN cd /tmp && \ - wget -O unison.tar.gz https://github.com/bcpierce00/unison/releases/download/v2.53.7/unison-2.53.7-ubuntu-x86_64.tar.gz && \ - mkdir -p unison && tar -zxf unison.tar.gz -C unison && \ - cp unison/bin/* /usr/local/bin && \ - rm -rf unison unison.tar.gz - - +# Build Unison from source because upstream does not ship arm64 release binaries +# and the distro package does not include unison-fsmonitor. +# SHA256 verified against Ubuntu Launchpad source package unison-2.53.8-1. +RUN export UNISON_VERSION=2.53.8 && \ + apt-get update && \ + apt-get install -y --no-install-recommends ocaml-nox && \ + curl -L "https://github.com/bcpierce00/unison/archive/refs/tags/v${UNISON_VERSION}.tar.gz" \ + -o "/tmp/unison-v${UNISON_VERSION}.tar.gz" && \ + echo "d0d30ea63e09fc8edf10bd8cbab238fffc8ed510d27741d06b5caa816abd58b6 /tmp/unison-v${UNISON_VERSION}.tar.gz" | sha256sum -c - && \ + tar -xzf "/tmp/unison-v${UNISON_VERSION}.tar.gz" -C /tmp && \ + cd "/tmp/unison-${UNISON_VERSION}" && \ + make -j"$(nproc)" && \ + make install PREFIX=/usr/local && \ + cd / && \ + rm -rf "/tmp/unison-${UNISON_VERSION}" "/tmp/unison-v${UNISON_VERSION}.tar.gz" && \ + apt-get purge -y --auto-remove ocaml-nox && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* /tmp/* # Developing container, works with devcontainer FROM rmcs-base AS rmcs-develop +ARG TARGETARCH # Install develop tools RUN apt-get update && apt-get install -y --no-install-recommends \ @@ -87,6 +112,19 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ apt-get autoremove -y && apt-get clean && \ rm -rf /var/lib/apt/lists/* /tmp/* +# Install OpenAI Codex CLI +RUN mkdir -p /etc/apt/keyrings && \ + curl -fsSL https://deb.nodesource.com/gpgkey/nodesource-repo.gpg.key \ + | gpg --dearmor -o /etc/apt/keyrings/nodesource.gpg && \ + chmod 644 /etc/apt/keyrings/nodesource.gpg && \ + echo "deb [signed-by=/etc/apt/keyrings/nodesource.gpg] https://deb.nodesource.com/node_22.x nodistro main" \ + > /etc/apt/sources.list.d/nodesource.list && \ + apt-get update && \ + apt-get install -y --no-install-recommends nodejs && \ + npm install -g @openai/codex && \ + apt-get autoremove -y && apt-get clean && \ + rm -rf /var/lib/apt/lists/* /tmp/* + # Install llvm-toolchain ARG LLVM_VERSION=22 RUN mkdir -p /etc/apt/keyrings && \ @@ -121,14 +159,25 @@ RUN --mount=type=bind,target=/tmp/.ssh,source=.ssh,readonly=false \ chown -R 1000:1000 .unison # Install latest neovim -RUN curl -LO https://github.com/neovim/neovim/releases/latest/download/nvim-linux-x86_64.tar.gz && \ +RUN case "${TARGETARCH}" in \ + amd64) nvim_arch=x86_64 ;; \ + arm64) nvim_arch=arm64 ;; \ + *) echo "Unsupported TARGETARCH: ${TARGETARCH}" >&2; exit 1 ;; \ + esac && \ + curl -LO https://github.com/neovim/neovim/releases/latest/download/nvim-linux-${nvim_arch}.tar.gz && \ rm -rf /opt/nvim && \ - tar -C /opt -xzf nvim-linux-x86_64.tar.gz && \ - rm nvim-linux-x86_64.tar.gz -ENV PATH="${PATH}:/opt/nvim-linux-x86_64/bin" + tar -C /opt -xzf nvim-linux-${nvim_arch}.tar.gz && \ + mv /opt/nvim-linux-${nvim_arch} /opt/nvim && \ + rm nvim-linux-${nvim_arch}.tar.gz +ENV PATH="${PATH}:/opt/nvim/bin" # Install latest stable cmake for user ubuntu -RUN wget https://github.com/kitware/cmake/releases/download/v4.2.3/cmake-4.2.3-linux-x86_64.sh -O install.sh && \ +RUN case "${TARGETARCH}" in \ + amd64) cmake_arch=x86_64 ;; \ + arm64) cmake_arch=aarch64 ;; \ + *) echo "Unsupported TARGETARCH: ${TARGETARCH}" >&2; exit 1 ;; \ + esac && \ + wget https://github.com/kitware/cmake/releases/download/v4.2.3/cmake-4.2.3-linux-${cmake_arch}.sh -O install.sh && \ mkdir -p /opt/cmake/ && bash install.sh --skip-license --prefix=/opt/cmake/ --exclude-subdir && \ rm install.sh @@ -151,6 +200,62 @@ RUN sh -c "$(wget https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools COPY --chown=1000:1000 .script/template/env_setup.bash env_setup.bash COPY --chown=1000:1000 .script/template/env_setup.zsh env_setup.zsh +FROM --platform=linux/amd64 ${SYSROOT_IMAGE_AMD64} AS rmcs-sysroot-amd64 +FROM --platform=linux/arm64 ${SYSROOT_IMAGE_ARM64} AS rmcs-sysroot-arm64 + +# Developing container with cross toolchains and opposite-arch sysroot. +FROM rmcs-develop AS rmcs-develop-full +ARG TARGETARCH + +USER root + +RUN apt-get update && \ + case "${TARGETARCH}" in \ + amd64) cross_triplet=aarch64-linux-gnu; cross_pkg_triplet=aarch64-linux-gnu ;; \ + arm64) cross_triplet=x86_64-linux-gnu; cross_pkg_triplet=x86-64-linux-gnu ;; \ + *) echo "Unsupported TARGETARCH: ${TARGETARCH}" >&2; exit 1 ;; \ + esac && \ + apt-get install -y --no-install-recommends \ + "gcc-14-${cross_pkg_triplet}" "g++-14-${cross_pkg_triplet}" \ + "binutils-${cross_pkg_triplet}" && \ + update-alternatives --install "/usr/bin/${cross_triplet}-gcc" "${cross_triplet}-gcc" "/usr/bin/${cross_triplet}-gcc-14" 50 && \ + update-alternatives --install "/usr/bin/${cross_triplet}-g++" "${cross_triplet}-g++" "/usr/bin/${cross_triplet}-g++-14" 50 && \ + apt-get autoremove -y && apt-get clean && \ + rm -rf /var/lib/apt/lists/* /tmp/* + +RUN mkdir -p /opt/sysroots && \ + case "${TARGETARCH}" in \ + amd64) mkdir -p /opt/sysroots/arm64 ;; \ + arm64) mkdir -p /opt/sysroots/amd64 ;; \ + *) echo "Unsupported TARGETARCH: ${TARGETARCH}" >&2; exit 1 ;; \ + esac + +RUN --mount=from=rmcs-sysroot-amd64,target=/mnt/sysroot-amd64,readonly \ + --mount=from=rmcs-sysroot-arm64,target=/mnt/sysroot-arm64,readonly \ + set -euo pipefail && \ + case "${TARGETARCH}" in \ + amd64) tar \ + --exclude='./dev/*' \ + --exclude='./proc/*' \ + --exclude='./sys/*' \ + --exclude='./run/*' \ + --exclude='./tmp/*' \ + -C /mnt/sysroot-arm64 -cf - . | tar -C /opt/sysroots/arm64 -xf - ;; \ + arm64) tar \ + --exclude='./dev/*' \ + --exclude='./proc/*' \ + --exclude='./sys/*' \ + --exclude='./run/*' \ + --exclude='./tmp/*' \ + -C /mnt/sysroot-amd64 -cf - . | tar -C /opt/sysroots/amd64 -xf - ;; \ + *) echo "Unsupported TARGETARCH: ${TARGETARCH}" >&2; exit 1 ;; \ + esac + +WORKDIR /home/ubuntu +ENV USER=ubuntu +ENV WORKDIR=/home/ubuntu +USER ubuntu + # Runtime container, will automatically launch the main program FROM rmcs-base AS rmcs-runtime diff --git a/README.md b/README.md index 693b658a..769f61b5 100644 --- a/README.md +++ b/README.md @@ -19,6 +19,11 @@ RoboMaster Control System based on ROS2. docker pull qzhhhi/rmcs-develop:latest ``` +如需交叉编译环境，可下载： +```bash +docker pull qzhhhi/rmcs-develop:latest-full +``` + 也可自行使用 `Dockerfile` 构建，参见 [镜像构建指南](docs/zh-cn/build_docker_image.md)。 ### Step 2：克隆并打开仓库 @@ -89,6 +94,19 @@ build-rmcs Note: 用于开发的所有脚本均位于 `.script` 中，参见 开发脚本手册(TODO)。 +如需在 `latest-full` 中执行交叉编译，请根据当前容器架构选择对向目标： +```bash +build-rmcs-cross --target-arch arm64 +``` +适用于 `linux/amd64` 的 `latest-full` 变体。 + +```bash +build-rmcs-cross --target-arch amd64 +``` +适用于 `linux/arm64` 的 `latest-full` 变体。 + +详见 [交叉编译使用说明](docs/zh-cn/cross_build.md)。 + ### Step 5 (Optional)：运行 编写代码并编译完成后，可以使用： diff --git a/docs/zh-cn/build_docker_image.md b/docs/zh-cn/build_docker_image.md index 88c5389a..768f977e 100644 --- a/docs/zh-cn/build_docker_image.md +++ b/docs/zh-cn/build_docker_image.md @@ -3,31 +3,60 @@ 于 Repo 根目录下，在终端中分别使用： ```bash -docker build . --target rmcs-runtime -t rmcs-runtime:latest +docker build . --target rmcs-base -t rmcs-base:latest ``` ```bash docker build . --target rmcs-develop -t rmcs-develop:latest ``` -构建开发容器和部署容器。 +```bash +docker build . --target rmcs-runtime -t rmcs-runtime:latest +``` + +构建基础容器、开发容器和部署容器。 + +## 构建 `rmcs-develop:latest-full` + +`latest-full` 需要对向架构的 `rmcs-base` 作为 sysroot 来源。 + +下面示例以在 `amd64` 主机上构建 `amd64 latest-full` 为例： + +```bash +docker buildx build . --platform linux/amd64 --target rmcs-base -t rmcs-base:linux-amd64 --load +``` + +```bash +docker buildx build . --platform linux/arm64 --target rmcs-base -t rmcs-base:linux-arm64 --load +``` + +```bash +docker buildx build . --platform linux/amd64 --target rmcs-develop-full -t rmcs-develop:latest-full \ + --build-arg SYSROOT_IMAGE_AMD64=rmcs-base:linux-amd64 \ + --build-arg SYSROOT_IMAGE_ARM64=rmcs-base:linux-arm64 \ + --load +``` + +如果是在 `arm64` 主机上构建，则交换目标平台即可（`latest-full` 的 sysroot 方向相反）。 + +## 代理构建 -需要注意的是，部分国家和地区会阻断对 `github` 的连接，此时构建需要使用代理： +部分国家和地区会阻断对 `github` 的连接，此时构建需要使用代理： ```bash docker build . --target rmcs-runtime -t rmcs-runtime:latest \ ---network host \ ---build-arg HTTP_PROXY=http://127.0.0.1:7890 \ ---build-arg HTTPS_PROXY=http://127.0.0.1:7890 + --network host \ + --build-arg HTTP_PROXY=http://127.0.0.1:7890 \ + --build-arg HTTPS_PROXY=http://127.0.0.1:7890 ``` ```bash docker build . --target rmcs-develop -t rmcs-develop:latest \ ---network host \ ---build-arg HTTP_PROXY=http://127.0.0.1:7890 \ ---build-arg HTTPS_PROXY=http://127.0.0.1:7890 + --network host \ + --build-arg HTTP_PROXY=http://127.0.0.1:7890 \ + --build-arg HTTPS_PROXY=http://127.0.0.1:7890 ``` 请自行把 `http://127.0.0.1:7890` 改为合适的代理地址。 -如果不使用本机回环地址 (127.0.0.1) 作为代理地址，构建参数中 `--network host` 可以省去。 \ No newline at end of file +如果不使用本机回环地址 (`127.0.0.1`) 作为代理地址，构建参数中的 `--network host` 可以省去。 diff --git a/docs/zh-cn/cross_build.md b/docs/zh-cn/cross_build.md new file mode 100644 index 00000000..25dfb805 --- /dev/null +++ b/docs/zh-cn/cross_build.md @@ -0,0 +1,111 @@ +# RMCS 交叉编译使用说明（第一阶段） + +本文档说明 `rmcs-develop:latest-full`、`build-rmcs-cross` 与 CI 的约定。 + +## 1. 镜像语义 + +- `qzhhhi/rmcs-develop:latest`：原有开发镜像，行为不变。 +- `qzhhhi/rmcs-develop:latest-full`：`latest` 的超集，额外提供 cross 工具链与对向架构 sysroot。 + +对应关系： + +- `latest-full` 的 `linux/amd64` 变体内置 `/opt/sysroots/arm64` +- `latest-full` 的 `linux/arm64` 变体内置 `/opt/sysroots/amd64` + +## 2. 仓库内新增能力 + +- `rmcs_ws/toolchain.cmake`：双向 cross 参数化工具链文件。 +- `.script/build-rmcs-cross`：独立 cross 构建入口，不影响现有 `build-rmcs`。 + +`build-rmcs-cross` 默认输出目录： + +- `rmcs_ws/build-cross-` +- `rmcs_ws/install-cross-` +- `rmcs_ws/log-cross-` + +## 3. 使用方式 + +在 `latest-full` 容器里，根据当前容器架构执行对向架构的交叉编译： + +```bash +build-rmcs-cross --target-arch arm64 +``` + +适用于 `linux/amd64` 的 `latest-full` 变体。 + +```bash +build-rmcs-cross --target-arch amd64 +``` + +适用于 `linux/arm64` 的 `latest-full` 变体。 + +例如，可追加常见 `colcon build` 参数： + +```bash +build-rmcs-cross --target-arch arm64 --packages-up-to rmcs_executor +``` + +若需要让现有 `sync-remote`、`env_setup` 等继续使用默认目录，可在 cross 构建成功后自动链接默认目录： + +```bash +build-rmcs-cross --target-arch arm64 --link-default +``` + +链接方向等价于在 `rmcs_ws` 下执行： + +```bash +ln -sfn build-cross-arm64 build +ln -sfn install-cross-arm64 install +ln -sfn log-cross-arm64 log +``` + +也就是创建或更新默认目录名，让 `build`、`install`、`log` 这三个软链接分别指向对应的 `*-cross-arm64` 目录。 + +切回 native 构建时，直接运行： + +```bash +build-rmcs +``` + +`build-rmcs` 会自动识别上述 cross 默认软链接，删除软链接并恢复成普通目录。 + +## 4. 构建环境隔离约束 + +`build-rmcs-cross` 会显式清理并重建以下环境，避免 host/target 串用： + +- `AMENT_PREFIX_PATH` +- `CMAKE_PREFIX_PATH` +- `COLCON_PREFIX_PATH` +- `PKG_CONFIG_PATH` +- `PKG_CONFIG_LIBDIR` +- `PKG_CONFIG_SYSROOT_DIR` + +并且默认启用： + +- `--merge-install` +- 不使用 `--symlink-install` + +## 5. 本地快速自检 + +```bash +test -d /opt/sysroots/arm64 +command -v aarch64-linux-gnu-gcc +./.script/build-rmcs-cross --target-arch arm64 --packages-up-to rmcs_core +readelf -h rmcs_ws/install-cross-arm64/lib/librmcs_core.so +``` + +若目标为 `amd64`，将上述 `arm64/aarch64-linux-gnu` 替换为 `amd64/x86_64-linux-gnu`。 + +本地自检故意覆盖到 `rmcs_core`，用于在推送前尽早暴露 workspace 级联依赖和交叉工具链问题；CI 只保留 `rmcs_executor` 最小 smoke，避免高频业务改动阻塞镜像推送。 + +## 6. CI 维护约定 + +- CI 先构建并推送 `qzhhhi/rmcs-base` 双架构 digest。 +- 构建 `latest-full` 时显式传入： + - `SYSROOT_IMAGE_AMD64=` + - `SYSROOT_IMAGE_ARM64=` +- `latest-full` 在每个架构 runner 上执行最小 smoke： + - sysroot 目录存在 + - cross 编译器可执行 + - `build-rmcs-cross --target-arch --packages-up-to rmcs_executor` 可完成构建 + - `readelf -h` 校验 `librmcs_executor.so` 架构 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml index 0e6c0684..facc17fa 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-omni.yaml @@ -30,7 +30,7 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster - - rmcs::AutoAimComponent -> auto_aim_component + # - rmcs::AutoAimComponent -> auto_aim_component value_broadcaster: ros__parameters: @@ -56,6 +56,7 @@ deformable_infantry: ros__parameters: serial_filter_rmcs_board: "AF-73B2-E8A1-A544-79ED-5BDA-D088-7F21-A6A6" serial_filter_top_board: "D4-0262-9E84-E715-CADB-9894-7241" + serial_filter_imu: "AF-8217-B05F-811B-6F3E-04EA-448E-9D03-CA2C" left_front_zero_point: 374 left_back_zero_point: 5801 right_back_zero_point: 7817 @@ -66,32 +67,13 @@ deformable_infantry: debug_log_wheel_motor: false debug_log_deformable_joint_motor: false - upper_limit: -0.61 # -35 deg - lower_limit: 0.05 # 6 deg - - yaw_angle_kp: 20.0 - yaw_angle_ki: 0.0 - yaw_angle_kd: 0.0 - - yaw_velocity_kp: 8.0 - yaw_velocity_ki: 0.0 - yaw_velocity_kd: 0.0 - - pitch_angle_kp: 40.0 - pitch_angle_ki: 0.0 - pitch_angle_kd: 0.0 - - pitch_velocity_kp: 3.0 - pitch_velocity_ki: 0.0 - pitch_velocity_kd: 0.0 - chassis_controller: ros__parameters: # Deploy geometry / chassis-owned joint intent min_angle: 8.0 max_angle: 58.0 active_suspension_enable: true - spin_ratio: 0.45 + spin_ratio: 1.0 # Suspension geometry / support model. active_suspension_mass: 22.5 @@ -130,10 +112,14 @@ chassis_controller: gimbal_controller: ros__parameters: + inertia: 2.0 # kg·m² + friction: 1.65 # Nm/(rad/s) + upper_limit: -0.61 # -35 deg lower_limit: 0.05 # 6 deg + use_encoder_pitch: false - yaw_angle_kp: 20.0 + yaw_angle_kp: 12.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 @@ -141,9 +127,6 @@ gimbal_controller: yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 - yaw_vel_ff_gain: 0.47 - yaw_acc_ff_gain: 0.00 - pitch_angle_kp: 40.0 pitch_angle_ki: 0.0 pitch_angle_kd: 0.0 @@ -152,13 +135,8 @@ gimbal_controller: pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - pitch_acc_ff_gain: 0.10 - pitch_torque_control: true - pitch_fusion_enabled: true - pitch_fusion_alpha: 0.98 - friction_wheel_controller: ros__parameters: friction_wheels: diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-v2.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-v2.yaml index a9ed5420..c4e1095a 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-v2.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry-v2.yaml @@ -22,14 +22,14 @@ rmcs_executor: - rmcs_core::controller::chassis::DeformableChassis -> chassis_controller - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - - rmcs_core::controller::chassis::WheelDemoController -> deformable_chassis_controller + - rmcs_core::controller::chassis::SteeringWheelController -> deformable_chassis_controller - rmcs_core::controller::chassis::DeformableJointController -> lf_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> lb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rb_joint_controller - rmcs_core::controller::chassis::DeformableJointController -> rf_joint_controller - - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster value_broadcaster: ros__parameters: @@ -53,57 +53,73 @@ value_broadcaster: deformable_infantry: ros__parameters: - serial_filter_rmcs_board: "AF-3CE0-2D2A-6CDC-89D0-CA40-90D7-46B5-4970" - serial_filter_top_board: "D4-0262-9E84-E715-CADB-9894-7241" - left_front_zero_point: 374 - left_back_zero_point: 5801 - right_back_zero_point: 7817 - right_front_zero_point: 7136 - yaw_motor_zero_point: 56216 - pitch_motor_zero_point: 10898 + serial_filter_rmcs_board: "AF-23FB-EE32-B892-1302-AE70-D640-7B4E-0CBF" + serial_filter_top_board: "AF-ABAC-786D-1B53-99F6-00A2-42A6-AA95-9D69" + left_front_zero_point: 7173 + left_back_zero_point: 5167 + right_back_zero_point: 3098 + right_front_zero_point: 6485 + yaw_motor_zero_point: 39442 + pitch_motor_zero_point: 56556 + debug_log_supercap: false + debug_log_wheel_motor: false + debug_log_deformable_joint_motor: false chassis_controller: ros__parameters: - min_angle: 10.0 - max_angle: 70.0 + # Deploy geometry / chassis-owned joint intent + min_angle: 20.0 + max_angle: 50.0 active_suspension_enable: true - active_suspension_mass: 22.5 - active_suspension_rod_length: 0.150 - active_suspension_spring_k: 150.0 - active_suspension_damping_k: 10.0 - active_suspension_gravity_comp_gain: 0.8 - active_suspension_airborne_torque_threshold: 5.0 - active_suspension_torque_limit: 80.0 + spin_ratio: 1.0 + + # IMU attitude correction at min-angle stance. + # pitch < 0: front high. roll > 0: right high. + active_suspension_Kp: 8.0 + active_suspension_pitch_ki: 0.35 + active_suspension_Dp: 0.28 + active_suspension_Kr: 8.0 + active_suspension_roll_ki: 0.35 + active_suspension_Dr: 0.28 + active_suspension_pitch_angle_diff_limit_deg: 45.0 + active_suspension_roll_angle_diff_limit_deg: 45.0 + active_suspension_pid_integral_limit_deg: 20.0 + + # Chassis-owned joint intent trajectory limits while attitude correction is active. + active_suspension_target_velocity_limit_deg: 80.0 + active_suspension_target_acceleration_limit_deg: 360.0 + + # Automatic IMU mounting-error calibration. + # When all four requested joint targets stay equal for 2s, average pitch/roll from 2s to 5s. + chassis_imu_calibration_wait_s: 2.0 + chassis_imu_calibration_sample_s: 3.0 gimbal_controller: ros__parameters: - upper_limit: -0.436332 # -25 deg - lower_limit: 0.05 # 10 deg + inertia: 1.0 # kg·m² + friction: 1.65 # Nm/(rad/s) - yaw_angle_kp: 14.2 + upper_limit: -0.61 # -35 deg + lower_limit: 0.05 # 6 deg + use_encoder_pitch: true + + yaw_angle_kp: 10.0 yaw_angle_ki: 0.0 yaw_angle_kd: 0.0 - yaw_velocity_kp: 6.4 + yaw_velocity_kp: 8.0 yaw_velocity_ki: 0.0 yaw_velocity_kd: 0.0 - yaw_vel_ff_gain: 0.47 - yaw_acc_ff_gain: 0.00 - - pitch_angle_kp: 8.0 + pitch_angle_kp: 40.0 pitch_angle_ki: 0.0 pitch_angle_kd: 0.0 - pitch_velocity_kp: 6.0 + pitch_velocity_kp: 3.0 pitch_velocity_ki: 0.0 pitch_velocity_kd: 0.0 - pitch_acc_ff_gain: 0.0 - - pitch_fusion_enabled: true - pitch_fusion_alpha: 0.98 - + pitch_torque_control: true friction_wheel_controller: ros__parameters: @@ -160,23 +176,29 @@ bullet_feeder_velocity_pid_controller: deformable_chassis_controller: ros__parameters: - mess: 22.5 + mess: 23.0 moment_of_inertia: 1.0 - chassis_radius: 0.2341741 - rod_length: 0.150 + vehicle_radius: 0.34678 wheel_radius: 0.055 - friction_coefficient: 6.6 + friction_coefficient: 0.6 k1: 2.958580e+00 k2: 3.082190e-03 no_load_power: 11.37 -lf_joint_adrc_controller: +lf_joint_controller: ros__parameters: - measurement: /chassis/left_front_joint/angle - setpoint: /chassis/left_front_joint/target_angle + # Joint-local servo inputs produced by chassis intent generation + measurement_angle: /chassis/left_front_joint/physical_angle + measurement_velocity: /chassis/left_front_joint/physical_velocity + setpoint_angle: /chassis/left_front_joint/target_physical_angle + setpoint_velocity: /chassis/left_front_joint/target_physical_velocity + mode_input: /chassis/left_front_joint/suspension_mode + suspension_torque: /chassis/left_front_joint/suspension_torque control: /chassis/left_front_joint/control_torque + + # Normal ADRC servo mode dt: 0.001 - b0: 0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -187,9 +209,32 @@ lf_joint_adrc_controller: alpha1: 0.75 alpha2: 0.7 delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + + # Suspension ADRC servo mode + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + + # Joint-local feedforward / limit shaping + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 lb_joint_controller: ros__parameters: + # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/left_back_joint/physical_angle measurement_velocity: /chassis/left_back_joint/physical_velocity setpoint_angle: /chassis/left_back_joint/target_physical_angle @@ -198,7 +243,7 @@ lb_joint_controller: suspension_torque: /chassis/left_back_joint/suspension_torque control: /chassis/left_back_joint/control_torque dt: 0.001 - b0: 0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -209,9 +254,28 @@ lb_joint_controller: alpha1: 0.75 alpha2: 0.7 delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 rb_joint_controller: ros__parameters: + # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_back_joint/physical_angle measurement_velocity: /chassis/right_back_joint/physical_velocity setpoint_angle: /chassis/right_back_joint/target_physical_angle @@ -220,7 +284,7 @@ rb_joint_controller: suspension_torque: /chassis/right_back_joint/suspension_torque control: /chassis/right_back_joint/control_torque dt: 0.001 - b0: 0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -231,9 +295,28 @@ rb_joint_controller: alpha1: 0.75 alpha2: 0.7 delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 rf_joint_controller: ros__parameters: + # Same joint-servo layout as lf_joint_controller measurement_angle: /chassis/right_front_joint/physical_angle measurement_velocity: /chassis/right_front_joint/physical_velocity setpoint_angle: /chassis/right_front_joint/target_physical_angle @@ -242,7 +325,7 @@ rf_joint_controller: suspension_torque: /chassis/right_front_joint/suspension_torque control: /chassis/right_front_joint/control_torque dt: 0.001 - b0: 0.60 + b0: -1.0 kt: 1.0 td_h: 0.001 td_r: 50.0 @@ -253,3 +336,21 @@ rf_joint_controller: alpha1: 0.75 alpha2: 0.7 delta: 0.02 + u_min: -200.0 + u_max: 200.0 + output_min: -200.0 + output_max: 200.0 + suspension_td_h: 0.001 + suspension_td_r: 12.0 + suspension_eso_w0: 80.0 + suspension_k1: 6.0 + suspension_k2: 3.0 + suspension_alpha1: 0.75 + suspension_alpha2: 0.7 + suspension_delta: 0.02 + suspension_u_min: -35.0 + suspension_u_max: 35.0 + suspension_output_min: -35.0 + suspension_output_max: 35.0 + torque_feedforward_gain: 0.0 + suspension_torque_feedforward_gain: -1.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry.yaml index 82fee7f4..ee769832 100644 --- a/rmcs_ws/src/rmcs_bringup/config/deformable-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/deformable-infantry.yaml @@ -98,6 +98,7 @@ gimbal_controller: ros__parameters: upper_limit: -0.436332 # -25 deg lower_limit: 0.174533 # 10 deg + use_encoder_pitch: false yaw_angle_kp: 14.2 yaw_angle_ki: 0.0 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp index 50d2942d..d7489b19 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/deformable_chassis.cpp @@ -446,7 +446,7 @@ class DeformableChassis static constexpr double kMinimumArmingTime_ = 0.02; static constexpr double translational_velocity_max_ = 10.0; - static constexpr double angular_velocity_max_ = 25.0; + static constexpr double angular_velocity_max_ = 30.0; static constexpr double rad_to_deg_ = 180.0 / std::numbers::pi; static constexpr std::array kPitchSigns_ = {-1.0, 1.0, 1.0, -1.0}; static constexpr std::array kRollSigns_ = {1.0, 1.0, -1.0, -1.0}; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel-demo.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel-demo.cpp index 9502a303..2564501a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel-demo.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel-demo.cpp @@ -154,9 +154,6 @@ class WheelDemoController register_output( "/chassis/right_front_wheel/control_torque", right_front_wheel_control_torque_); - register_output("/chassis/encoder/alpha", encoder_alpha_); - register_output("/chassis/encoder/alpha_dot", encoder_alpha_dot_); - register_output("/chassis/radius", radius_); } void update() override { @@ -169,9 +166,6 @@ class WheelDemoController const JointTargetStates joint_target = update_joint_target_states_(); if (joint_feedback.valid) { vehicle_radius_ = joint_feedback.radius; - *radius_ = vehicle_radius_.mean(); - *encoder_alpha_ = joint_feedback.alpha_rad.mean(); - *encoder_alpha_dot_ = joint_feedback.alpha_dot_rad.mean(); RCLCPP_INFO_THROTTLE( get_logger(), *get_clock(), 1000, "physical joint angle[deg] lf=%.2f lb=%.2f rb=%.2f rf=%.2f, radius[m] lf=%.3f " @@ -181,10 +175,6 @@ class WheelDemoController joint_feedback.alpha_rad[2] * 180.0 / std::numbers::pi, joint_feedback.alpha_rad[3] * 180.0 / std::numbers::pi, vehicle_radius_[0], vehicle_radius_[1], vehicle_radius_[2], vehicle_radius_[3]); - } else { - *radius_ = nan_; - *encoder_alpha_ = nan_; - *encoder_alpha_dot_ = nan_; } integral_yaw_angle_imu(); @@ -392,10 +382,6 @@ class WheelDemoController last_joint_target_angle_ = Eigen::Vector4d::Zero(); last_joint_target_angle_valid_ = false; - *encoder_alpha_ = nan_; - *encoder_alpha_dot_ = nan_; - *radius_ = nan_; - *left_front_steering_control_torque_ = 0.0; *left_back_steering_control_torque_ = 0.0; *right_back_steering_control_torque_ = 0.0; @@ -857,10 +843,6 @@ class WheelDemoController OutputInterface right_back_wheel_control_torque_; OutputInterface right_front_wheel_control_torque_; - OutputInterface encoder_alpha_; - OutputInterface encoder_alpha_dot_; - OutputInterface radius_; - QcpSolver qcp_solver_; filter::LowPassFilter<3> control_acceleration_filter_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp index 6d3fbb72..49d06674 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/deformable_infantry_gimbal_controller.cpp @@ -31,6 +31,10 @@ class DeformableInfantryGimbalController get_parameter("pitch_torque_control", pitch_torque_control_enabled_); get_parameter("manual_joystick_sensitivity", joystick_sensitivity_); get_parameter("manual_mouse_sensitivity", mouse_sensitivity_); + + // Feedforward parameters + yaw_inertia_ = get_parameter("inertia").as_double(); + yaw_friction_ = get_parameter("friction").as_double(); } auto update() -> void override { @@ -64,8 +68,27 @@ class DeformableInfantryGimbalController const auto yaw_velocity_ref = yaw_angle_pid_.update(angle_error.yaw_angle_error); const auto pitch_velocity_ref = pitch_angle_pid_.update(angle_error.pitch_angle_error); - *output_.yaw_control_torque = - yaw_velocity_pid_.update(yaw_velocity_ref - *input_.yaw_velocity_imu); + // Calculate acceleration from velocity derivative with low-pass filter + double yaw_accel_raw = 0.0; + if (std::isfinite(last_yaw_velocity_ref_)) { + yaw_accel_raw = (yaw_velocity_ref - last_yaw_velocity_ref_) * 1000.0; // dt = 1ms + } + last_yaw_velocity_ref_ = yaw_velocity_ref; + + // Low-pass filter for acceleration (alpha = 0.1 for smoothing) + const double alpha = 0.1; + if (std::isfinite(filtered_yaw_accel_)) { + filtered_yaw_accel_ = alpha * yaw_accel_raw + (1.0 - alpha) * filtered_yaw_accel_; + } else { + filtered_yaw_accel_ = 0.0; + } + + // Feedforward control: torque = J * accel + b * velocity + const auto yaw_velocity_error = yaw_velocity_ref - *input_.yaw_velocity_imu; + const auto yaw_feedforward = yaw_inertia_ * filtered_yaw_accel_ + yaw_friction_ * yaw_velocity_ref; + const auto yaw_feedback = yaw_velocity_pid_.update(yaw_velocity_error); + + *output_.yaw_control_torque = yaw_feedforward + yaw_feedback; if (pitch_torque_control_enabled_) { *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = @@ -167,6 +190,8 @@ class DeformableInfantryGimbalController yaw_velocity_pid_.reset(); pitch_angle_pid_.reset(); pitch_velocity_pid_.reset(); + last_yaw_velocity_ref_ = kNaN; + filtered_yaw_accel_ = kNaN; *output_.yaw_control_torque = kNaN; *output_.pitch_control_velocity = kNaN; *output_.pitch_control_torque = kNaN; @@ -181,7 +206,7 @@ class DeformableInfantryGimbalController TwoAxisGimbalSolver gimbal_solver_{ *this, get_parameter("upper_limit").as_double(), get_parameter("lower_limit").as_double(), - true}; + get_parameter("use_encoder_pitch").as_bool()}; pid::PidCalculator yaw_angle_pid_{ get_parameter("yaw_angle_kp").as_double(), @@ -207,6 +232,12 @@ class DeformableInfantryGimbalController double joystick_sensitivity_ = 0.003; double mouse_sensitivity_ = 0.5; bool pitch_torque_control_enabled_ = false; + + // Feedforward parameters + double yaw_inertia_; + double yaw_friction_; + double last_yaw_velocity_ref_ = kNaN; + double filtered_yaw_accel_ = kNaN; }; } // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 842a526f..92489cbb 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -1,7 +1,9 @@ #pragma once +#include +#include + #include -#include #include #include #include @@ -28,12 +30,6 @@ class TwoAxisGimbalSolver { component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); component.register_input("/tf", tf_); - - // Try to read pitch fusion parameters from ROS parameters (optional) - if (auto* node = dynamic_cast(&component)) { - node->get_parameter_or("pitch_fusion_enabled", pitch_fusion_enabled_, true); - node->get_parameter_or("pitch_fusion_alpha", pitch_fusion_alpha_, 0.98); - } } class SetDisabled : public Operation { @@ -154,38 +150,8 @@ class TwoAxisGimbalSolver { std::pair result; auto& [dir_yaw_link, pitch_cs] = result; - // Deformable infantry V2 mounts the IMU on the yaw link. The TF tree therefore exposes a - // synthesized PitchLink -> OdomImu transform, while the PitchLink -> YawLink relation - // here still comes directly from the pitch encoder. const double encoder_pitch = *gimbal_pitch_angle_; - double pitch_angle = encoder_pitch; // Default: use encoder only - - if (pitch_fusion_enabled_) { - // Extract pitch angle from IMU - double imu_pitch = extract_pitch_from_imu(); - - // Initialize fused angle on first call - if (!fusion_initialized_) { - fused_pitch_angle_ = encoder_pitch; - encoder_pitch_prev_ = encoder_pitch; - fusion_initialized_ = true; - } - - double encoder_delta = encoder_pitch - encoder_pitch_prev_; - - if (encoder_delta > std::numbers::pi) - encoder_delta -= 2.0 * std::numbers::pi; - if (encoder_delta < -std::numbers::pi) - encoder_delta += 2.0 * std::numbers::pi; - - fused_pitch_angle_ = pitch_fusion_alpha_ * (fused_pitch_angle_ + encoder_delta) - + (1.0 - pitch_fusion_alpha_) * imu_pitch; - - encoder_pitch_prev_ = encoder_pitch; - pitch_angle = fused_pitch_angle_; - } - - pitch_cs = {std::cos(pitch_angle), -std::sin(pitch_angle)}; + pitch_cs = {std::cos(encoder_pitch), -std::sin(encoder_pitch)}; const auto& [x, y, z] = *dir; dir_yaw_link = { @@ -197,21 +163,6 @@ class TwoAxisGimbalSolver { return result; } - double extract_pitch_from_imu() const { - auto pitch_x_in_odom = - fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); - - const auto& [x, y, z] = *pitch_x_in_odom; - - double horizontal_norm = std::sqrt(x * x + y * y); - - if (horizontal_norm < 1e-9) { - return 0.0; - } - - return std::atan2(-z, horizontal_norm); - } - static PitchLink::DirectionVector yaw_link_to_pitch_link(const YawLink::DirectionVector& dir, const Eigen::Vector2d& pitch) { @@ -263,12 +214,6 @@ class TwoAxisGimbalSolver { bool control_enabled_ = false; OdomImu::DirectionVector control_direction_; - // Pitch fusion parameters and state (mutable for use in const methods) - mutable bool pitch_fusion_enabled_ = true; - mutable double pitch_fusion_alpha_ = 0.98; - mutable double fused_pitch_angle_ = 0.0; - mutable double encoder_pitch_prev_ = 0.0; - mutable bool fusion_initialized_ = false; }; } // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp index 2aaa92f4..b2dbccd9 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-omni.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include @@ -33,115 +34,6 @@ namespace rmcs_core::hardware { using Clock = std::chrono::steady_clock; -namespace { - -class RmcsBoardLiteCompat : private librmcs::data::DataCallback { -public: - explicit RmcsBoardLiteCompat( - std::string_view serial_filter = {}, const librmcs::agent::AdvancedOptions& options = {}) - : handler_(0xA11C, 0xA801, serial_filter, options, *this) {} - - RmcsBoardLiteCompat(const RmcsBoardLiteCompat&) = delete; - RmcsBoardLiteCompat& operator=(const RmcsBoardLiteCompat&) = delete; - RmcsBoardLiteCompat(RmcsBoardLiteCompat&&) = delete; - RmcsBoardLiteCompat& operator=(RmcsBoardLiteCompat&&) = delete; - ~RmcsBoardLiteCompat() override = default; - - class PacketBuilder { - friend class RmcsBoardLiteCompat; - - public: - PacketBuilder& can0_transmit(const librmcs::data::CanDataView& data) { - if (!builder_.write_can(librmcs::data::DataId::kCan0, data)) [[unlikely]] - throw std::invalid_argument{"CAN0 transmission failed: Invalid CAN data"}; - return *this; - } - PacketBuilder& can1_transmit(const librmcs::data::CanDataView& data) { - if (!builder_.write_can(librmcs::data::DataId::kCan1, data)) [[unlikely]] - throw std::invalid_argument{"CAN1 transmission failed: Invalid CAN data"}; - return *this; - } - PacketBuilder& can2_transmit(const librmcs::data::CanDataView& data) { - if (!builder_.write_can(librmcs::data::DataId::kCan2, data)) [[unlikely]] - throw std::invalid_argument{"CAN2 transmission failed: Invalid CAN data"}; - return *this; - } - PacketBuilder& can3_transmit(const librmcs::data::CanDataView& data) { - if (!builder_.write_can(librmcs::data::DataId::kCan3, data)) [[unlikely]] - throw std::invalid_argument{"CAN3 transmission failed: Invalid CAN data"}; - return *this; - } - - PacketBuilder& uart0_transmit(const librmcs::data::UartDataView& data) { - if (!builder_.write_uart(librmcs::data::DataId::kUart0, data)) [[unlikely]] - throw std::invalid_argument{"UART0 transmission failed: Invalid UART data"}; - return *this; - } - PacketBuilder& uart1_transmit(const librmcs::data::UartDataView& data) { - if (!builder_.write_uart(librmcs::data::DataId::kUart1, data)) [[unlikely]] - throw std::invalid_argument{"UART1 transmission failed: Invalid UART data"}; - return *this; - } - - private: - explicit PacketBuilder(librmcs::host::protocol::Handler& handler) noexcept - : builder_(handler.start_transmit()) {} - - librmcs::host::protocol::Handler::PacketBuilder builder_; - }; - - PacketBuilder start_transmit() noexcept { return PacketBuilder{handler_}; } - -private: - bool can_receive_callback( - librmcs::data::DataId id, const librmcs::data::CanDataView& data) final { - switch (id) { - case librmcs::data::DataId::kCan0: can0_receive_callback(data); return true; - case librmcs::data::DataId::kCan1: can1_receive_callback(data); return true; - case librmcs::data::DataId::kCan2: can2_receive_callback(data); return true; - case librmcs::data::DataId::kCan3: can3_receive_callback(data); return true; - default: return false; - } - } - - virtual void can0_receive_callback(const librmcs::data::CanDataView& data) { (void)data; } - virtual void can1_receive_callback(const librmcs::data::CanDataView& data) { (void)data; } - virtual void can2_receive_callback(const librmcs::data::CanDataView& data) { (void)data; } - virtual void can3_receive_callback(const librmcs::data::CanDataView& data) { (void)data; } - - bool uart_receive_callback( - librmcs::data::DataId id, const librmcs::data::UartDataView& data) final { - switch (id) { - case librmcs::data::DataId::kUartDbus: dbus_receive_callback(data); return true; - case librmcs::data::DataId::kUart0: uart0_receive_callback(data); return true; - case librmcs::data::DataId::kUart1: uart1_receive_callback(data); return true; - default: return false; - } - } - - virtual void dbus_receive_callback(const librmcs::data::UartDataView& data) { (void)data; } - virtual void uart0_receive_callback(const librmcs::data::UartDataView& data) { (void)data; } - virtual void uart1_receive_callback(const librmcs::data::UartDataView& data) { (void)data; } - - void gpio_digital_read_result_callback(const librmcs::data::GpioDigitalDataView& data) final { - (void)data; - } - void gpio_analog_read_result_callback(const librmcs::data::GpioAnalogDataView& data) final { - (void)data; - } - - void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { - (void)data; - } - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - (void)data; - } - - librmcs::host::protocol::Handler handler_; -}; - -} // namespace - class DeformableInfantryOmni : public rmcs_executor::Component , public rclcpp::Node { @@ -177,6 +69,7 @@ class DeformableInfantryOmni top_board_ = std::make_unique( *this, *deformable_infantry_command_, get_parameter("serial_filter_top_board").as_string()); + imu_ = std::make_unique(*this, get_parameter("serial_filter_imu").as_string()); } ~DeformableInfantryOmni() override = default; @@ -189,6 +82,7 @@ class DeformableInfantryOmni void update() override { rmcs_board_lite->update(); top_board_->update(); + imu_->update(); update_hard_sync_snapshot(); } @@ -201,6 +95,7 @@ class DeformableInfantryOmni private: class DeformableInfantryOmniCommand; class BottomBoard; + class ImuBoard; class TopBoard; void update_hard_sync_snapshot() { @@ -209,10 +104,10 @@ class DeformableInfantryOmni hard_sync_snapshot_->valid = true; hard_sync_snapshot_->exposure_timestamp = *timestamp_; - hard_sync_snapshot_->qw = top_board_->bmi088_.q0(); - hard_sync_snapshot_->qx = top_board_->bmi088_.q1(); - hard_sync_snapshot_->qy = top_board_->bmi088_.q2(); - hard_sync_snapshot_->qz = top_board_->bmi088_.q3(); + hard_sync_snapshot_->qw = imu_->bmi088_.q0(); + hard_sync_snapshot_->qx = imu_->bmi088_.q1(); + hard_sync_snapshot_->qy = imu_->bmi088_.q2(); + hard_sync_snapshot_->qz = imu_->bmi088_.q3(); ++hard_sync_snapshot_count_; if (*timestamp_ >= next_hard_sync_log_time_) { @@ -264,7 +159,7 @@ class DeformableInfantryOmni DeformableInfantryOmni& deformableInfantry; }; - class BottomBoard final : private RmcsBoardLiteCompat { + class BottomBoard final : private librmcs::agent::RmcsBoardLite { public: friend class DeformableInfantryOmni; @@ -276,7 +171,7 @@ class DeformableInfantryOmni std::string serial_filter = { }) - : RmcsBoardLiteCompat( + : librmcs::agent::RmcsBoardLite( serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) , deformable_infantry_(deformableInfantry) @@ -753,6 +648,55 @@ class DeformableInfantryOmni OutputInterface radius_; }; + class ImuBoard final : private librmcs::agent::RmcsBoardLite { + public: + friend class DeformableInfantryOmni; + + explicit ImuBoard( + DeformableInfantryOmni& deformableInfantry, std::string serial_filter = {}) + : librmcs::agent::RmcsBoardLite( + serial_filter, + librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) + , tf_(deformableInfantry.tf_) + , bmi088_(1000, 0.2, 0.0) { + + deformableInfantry.register_output( + "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(x, z, -y); }); + } + + ~ImuBoard() override = default; + + void update() { + bmi088_.update_status(); + Eigen::Quaterniond const gimbal_imu_pose{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + } + + private: + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + + OutputInterface& tf_; + + OutputInterface gimbal_pitch_velocity_imu_; + + device::Bmi088 bmi088_; + }; + class TopBoard final : private librmcs::agent::CBoard { public: friend class DeformableInfantryOmni; @@ -792,12 +736,9 @@ class DeformableInfantryOmni device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); deformableInfantry.register_output( - "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - deformableInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_encoder_); + "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Top board BMI088 maps to gimbal frame as (-x, -y, z). return std::make_tuple(y, -x, z); }); } @@ -810,29 +751,14 @@ class DeformableInfantryOmni void update() { bmi088_.update_status(); - gimbal_pitch_motor_.update_status(); gimbal_left_friction_.update_status(); gimbal_right_friction_.update_status(); scope_motor_.update_status(); const double pitch_encoder_angle = gimbal_pitch_motor_.angle(); - Eigen::Quaterniond const odom_imu_to_yaw_link{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - Eigen::Quaterniond const yaw_link_to_odom_imu = odom_imu_to_yaw_link.conjugate(); - Eigen::Quaterniond pitch_link_to_odom_imu = - Eigen::Quaterniond{ - Eigen::AngleAxisd{-pitch_encoder_angle, Eigen::Vector3d::UnitY()} - } - * yaw_link_to_odom_imu; - pitch_link_to_odom_imu.normalize(); - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - *gimbal_pitch_velocity_encoder_ = gimbal_pitch_motor_.velocity(); - // The BMI088 is mounted on the yaw link. fast_tf stores PitchLink -> OdomImu, so use - // the encoder pitch from the TF tree to move the yaw-link pose back into PitchLink. - tf_->set_transform( - pitch_link_to_odom_imu); + *gimbal_yaw_velocity_imu_ = bmi088_.gz(); tf_->set_state( pitch_encoder_angle); @@ -890,10 +816,7 @@ class DeformableInfantryOmni std::atomic& hard_sync_pending_; OutputInterface& tf_; - - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_encoder_; - + OutputInterface gimbal_yaw_velocity_imu_; device::Bmi088 bmi088_; device::LkMotor gimbal_pitch_motor_; device::DjiMotor gimbal_left_friction_; @@ -910,6 +833,7 @@ class DeformableInfantryOmni std::shared_ptr deformable_infantry_command_; std::unique_ptr rmcs_board_lite; + std::unique_ptr imu_; std::unique_ptr top_board_; uint32_t cmd_tick_ = 0; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-v2.cpp b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-v2.cpp index f4fcd656..7b8060c5 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-v2.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/deformable-infantry-v2.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -21,7 +22,6 @@ #include #include -#include #include #include "hardware/device/bmi088.hpp" @@ -69,7 +69,7 @@ class DeformableInfantryV2 gimbal_calibrate_subscription_callback(std::move(msg)); }); - rmcs_board_ = std::make_unique( + rmcs_board_lite = std::make_unique( *this, *deformable_infantry_command_, get_parameter("serial_filter_rmcs_board").as_string()); top_board_ = std::make_unique( @@ -85,14 +85,14 @@ class DeformableInfantryV2 } void update() override { - rmcs_board_->update(); + rmcs_board_lite->update(); top_board_->update(); update_hard_sync_snapshot(); } void command_update() { const bool even = ((cmd_tick_++ & 1u) == 0u); - rmcs_board_->command_update(even); + rmcs_board_lite->command_update(even); top_board_->command_update(); } @@ -123,48 +123,48 @@ class DeformableInfantryV2 } void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - if (!rmcs_board_) + if (!rmcs_board_lite) return; RCLCPP_INFO( get_logger(), "New left front offset: %d", - rmcs_board_->chassis_steer_motors_[0].calibrate_zero_point()); + rmcs_board_lite->chassis_steer_motors_[0].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "New left back offset: %d", - rmcs_board_->chassis_steer_motors_[1].calibrate_zero_point()); + rmcs_board_lite->chassis_steer_motors_[1].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "New right back offset: %d", - rmcs_board_->chassis_steer_motors_[2].calibrate_zero_point()); + rmcs_board_lite->chassis_steer_motors_[2].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "New right front offset: %d", - rmcs_board_->chassis_steer_motors_[3].calibrate_zero_point()); + rmcs_board_lite->chassis_steer_motors_[3].calibrate_zero_point()); } void joints_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - if (!rmcs_board_) + if (!rmcs_board_lite) return; RCLCPP_INFO( get_logger(), "New left front offset: %ld", - rmcs_board_->chassis_joint_motors_[0].calibrate_zero_point()); + rmcs_board_lite->chassis_joint_motors_[0].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "New left back offset: %ld", - rmcs_board_->chassis_joint_motors_[1].calibrate_zero_point()); + rmcs_board_lite->chassis_joint_motors_[1].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "New right back offset: %ld", - rmcs_board_->chassis_joint_motors_[2].calibrate_zero_point()); + rmcs_board_lite->chassis_joint_motors_[2].calibrate_zero_point()); RCLCPP_INFO( get_logger(), "New right front offset: %ld", - rmcs_board_->chassis_joint_motors_[3].calibrate_zero_point()); + rmcs_board_lite->chassis_joint_motors_[3].calibrate_zero_point()); } void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - if (!rmcs_board_ || !top_board_) + if (!rmcs_board_lite || !top_board_) return; RCLCPP_INFO( get_logger(), "[gimbal calibration] New yaw offset: %ld", - rmcs_board_->gimbal_yaw_motor_.calibrate_zero_point()); + rmcs_board_lite->gimbal_yaw_motor_.calibrate_zero_point()); RCLCPP_INFO( get_logger(), "[gimbal calibration] New pitch offset: %ld", top_board_->gimbal_pitch_motor_.calibrate_zero_point()); @@ -220,6 +220,8 @@ class DeformableInfantryV2 deformableInfantry, deformableInfantry_command, "/chassis/right_back_joint"}, device::LkMotor{ deformableInfantry, deformableInfantry_command, "/chassis/right_front_joint"}}, + next_chassis_feedback_log_time_(Clock::now() + std::chrono::seconds(1)), + next_supercap_feedback_log_time_(Clock::now() + std::chrono::seconds(1)), supercap_(deformableInfantry, deformableInfantry_command), gimbal_bullet_feeder_( deformableInfantry, deformableInfantry_command, "/gimbal/bullet_feeder") { @@ -255,6 +257,12 @@ class DeformableInfantryV2 .set_reversed() .enable_multi_turn_angle()); + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Keep the existing chassis yaw axis mapping explicit until the bottom-board IMU + // installation is re-validated on hardware. + return std::make_tuple(-y, x, z); + }); + gimbal_bullet_feeder_.configure( device::DjiMotor::Config{device::DjiMotor::Type::kM2006}.enable_multi_turn_angle()); @@ -289,6 +297,12 @@ class DeformableInfantryV2 deformableInfantry.register_output( "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + deformableInfantry.register_output("/chassis/imu/pitch", chassis_imu_pitch_, 0.0); + deformableInfantry.register_output("/chassis/imu/roll", chassis_imu_roll_, 0.0); + deformableInfantry.register_output( + "/chassis/imu/pitch_rate", chassis_imu_pitch_rate_, 0.0); + deformableInfantry.register_output( + "/chassis/imu/roll_rate", chassis_imu_roll_rate_, 0.0); deformableInfantry.register_output( "/chassis/left_front_joint/physical_angle", left_front_joint_physical_angle_, nan_); deformableInfantry.register_output( @@ -314,6 +328,12 @@ class DeformableInfantryV2 deformableInfantry.register_output( "/chassis/encoder/alpha_dot", encoder_alpha_dot_, nan_); deformableInfantry.register_output("/chassis/radius", radius_, nan_); + + deformableInfantry.get_parameter_or("debug_log_supercap", debug_log_supercap_, false); + deformableInfantry.get_parameter_or( + "debug_log_wheel_motor", debug_log_wheel_motor_, false); + deformableInfantry.get_parameter_or( + "debug_log_deformable_joint_motor", debug_log_deformable_joint_motor_, false); } ~BottomBoard() override = default; @@ -321,6 +341,26 @@ class DeformableInfantryV2 void update() { imu_.update_status(); *chassis_yaw_velocity_imu_ = imu_.gz(); + { + const double q0 = imu_.q0(); + const double q1 = imu_.q1(); + const double q2 = imu_.q2(); + const double q3 = imu_.q3(); + + double sin_pitch = 2.0 * (q0 * q2 - q3 * q1); + sin_pitch = std::clamp(sin_pitch, -1.0, 1.0); + + const double standard_pitch = std::asin(sin_pitch); + const double standard_roll = + std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); + + // Export chassis attitude using the requested convention: + // pitch < 0 when the front is higher, roll > 0 when the left side is higher. + *chassis_imu_pitch_ = -standard_pitch; + *chassis_imu_roll_ = standard_roll; + *chassis_imu_pitch_rate_ = -imu_.gy(); + *chassis_imu_roll_rate_ = imu_.gx(); + } for (auto& motor : chassis_wheel_motors_) motor.update_status(); @@ -339,29 +379,15 @@ class DeformableInfantryV2 3, right_front_joint_physical_angle_, right_front_joint_physical_velocity_); update_geometry_feedback_(); - - // if (joint_status_received_[0].load(std::memory_order_relaxed) - // && joint_status_received_[1].load(std::memory_order_relaxed) - // && joint_status_received_[2].load(std::memory_order_relaxed) - // && joint_status_received_[3].load(std::memory_order_relaxed)) { - // RCLCPP_INFO_THROTTLE( - // deformable_infantry_.get_logger(), *deformable_infantry_.get_clock(), 1000, - // "joint raw angle[deg] lf=%.2f lb=%.2f rb=%.2f rf=%.2f | physical angle[deg] " - // "lf=%.2f lb=%.2f rb=%.2f rf=%.2f", - // chassis_joint_motors_[0].angle() * 180.0 / std::numbers::pi, - // chassis_joint_motors_[1].angle() * 180.0 / std::numbers::pi, - // chassis_joint_motors_[2].angle() * 180.0 / std::numbers::pi, - // chassis_joint_motors_[3].angle() * 180.0 / std::numbers::pi, - // *left_front_joint_physical_angle_ * 180.0 / std::numbers::pi, - // *left_back_joint_physical_angle_ * 180.0 / std::numbers::pi, - // *right_back_joint_physical_angle_ * 180.0 / std::numbers::pi, - // *right_front_joint_physical_angle_ * 180.0 / std::numbers::pi); - // } + if (debug_log_wheel_motor_ || debug_log_deformable_joint_motor_) + log_chassis_feedback_once_per_second_(); dr16_.update_status(); gimbal_yaw_motor_.update_status(); if (supercap_status_received_.load(std::memory_order_relaxed)) supercap_.update_status(); + if (debug_log_supercap_) + log_supercap_feedback_once_per_second_(); gimbal_bullet_feeder_.update_status(); tf_->set_state( @@ -463,7 +489,7 @@ class DeformableInfantryV2 .as_bytes(), }); - // V2: Joint LK motors — individual CAN frames + // V2: Joint LK motors - individual CAN frames builder.can0_transmit({ .can_id = 0x141, .can_data = chassis_joint_motors_[0].generate_command().as_bytes(), @@ -533,6 +559,78 @@ class DeformableInfantryV2 *radius_ = (chassis_radius_base_ + rod_length_ * alpha_rad.array().cos()).mean(); } + void log_chassis_feedback_once_per_second_() { + const auto now = Clock::now(); + if (now < next_chassis_feedback_log_time_) + return; + + const auto wheel_rx = [this](size_t index) { + return wheel_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; + }; + const auto joint_rx = [this](size_t index) { + return joint_status_received_[index].load(std::memory_order_relaxed) ? 'Y' : 'N'; + }; + + if (debug_log_wheel_motor_) { + RCLCPP_INFO( + deformable_infantry_.get_logger(), + "[wheel motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "encoder(deg) lf=% .1f lb=% .1f rb=% .1f rf=% .1f | " + "rx=[%c %c %c %c]", + chassis_wheel_motors_[0].angle(), chassis_wheel_motors_[1].angle(), + chassis_wheel_motors_[2].angle(), chassis_wheel_motors_[3].angle(), + chassis_wheel_motors_[0].encoder_angle(), + chassis_wheel_motors_[1].encoder_angle(), + chassis_wheel_motors_[2].encoder_angle(), + chassis_wheel_motors_[3].encoder_angle(), wheel_rx(0), wheel_rx(1), wheel_rx(2), + wheel_rx(3)); + } + + if (debug_log_deformable_joint_motor_) { + RCLCPP_INFO( + deformable_infantry_.get_logger(), + "[deformable joint motor] angle(rad) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "velocity(rad/s) lf=% .3f lb=% .3f rb=% .3f rf=% .3f | " + "rx=[%c %c %c %c]", + *left_front_joint_physical_angle_, *left_back_joint_physical_angle_, + *right_back_joint_physical_angle_, *right_front_joint_physical_angle_, + *left_front_joint_physical_velocity_, *left_back_joint_physical_velocity_, + *right_back_joint_physical_velocity_, *right_front_joint_physical_velocity_, + joint_rx(0), joint_rx(1), joint_rx(2), joint_rx(3)); + } + + next_chassis_feedback_log_time_ = now + std::chrono::seconds(1); + } + + void log_supercap_feedback_once_per_second_() { + const auto now = Clock::now(); + if (now < next_supercap_feedback_log_time_) + return; + + const bool supercap_rx = supercap_status_received_.load(std::memory_order_relaxed); + auto supercap_raw_packet = latest_supercap_status_.load(std::memory_order_relaxed); + const auto supercap_raw_bytes = supercap_raw_packet.as_bytes(); + + RCLCPP_INFO( + deformable_infantry_.get_logger(), + "[supercap] can1 rx=%c id=0x300 enabled=%d supercap_v=% .3f chassis_v=% .3f " + "power=% .3f raw=[%02X %02X %02X %02X %02X %02X %02X %02X]", + supercap_rx ? 'Y' : 'N', supercap_rx ? (supercap_.supercap_enabled() ? 1 : 0) : -1, + supercap_rx ? supercap_.supercap_voltage() : nan_, + supercap_rx ? supercap_.chassis_voltage() : nan_, + supercap_rx ? supercap_.chassis_power() : nan_, + std::to_integer(supercap_raw_bytes[0]), + std::to_integer(supercap_raw_bytes[1]), + std::to_integer(supercap_raw_bytes[2]), + std::to_integer(supercap_raw_bytes[3]), + std::to_integer(supercap_raw_bytes[4]), + std::to_integer(supercap_raw_bytes[5]), + std::to_integer(supercap_raw_bytes[6]), + std::to_integer(supercap_raw_bytes[7])); + + next_supercap_feedback_log_time_ = now + std::chrono::seconds(1); + } + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { dr16_.store_status(data.uart_data.data(), data.uart_data.size()); } @@ -540,9 +638,10 @@ class DeformableInfantryV2 void can0_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x201) + if (data.can_id == 0x201) { chassis_wheel_motors_[0].store_status(data.can_data); - else if (data.can_id == 0x141) { + wheel_status_received_[0].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { chassis_joint_motors_[0].store_status(data.can_data); joint_status_received_[0].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x205) @@ -552,14 +651,18 @@ class DeformableInfantryV2 void can1_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x201) + if (data.can_id == 0x201) { chassis_wheel_motors_[1].store_status(data.can_data); - else if (data.can_id == 0x141) { + wheel_status_received_[1].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { chassis_joint_motors_[1].store_status(data.can_data); joint_status_received_[1].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x205) chassis_steer_motors_[1].store_status(data.can_data); else if (data.can_id == 0x300) { + if (data.can_data.size() == 8) + latest_supercap_status_.store( + device::CanPacket8{data.can_data}, std::memory_order_relaxed); supercap_.store_status(data.can_data); supercap_status_received_.store(true, std::memory_order_relaxed); } @@ -568,9 +671,10 @@ class DeformableInfantryV2 void can2_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x201) + if (data.can_id == 0x201) { chassis_wheel_motors_[2].store_status(data.can_data); - else if (data.can_id == 0x141) { + wheel_status_received_[2].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { chassis_joint_motors_[2].store_status(data.can_data); joint_status_received_[2].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x142) { @@ -584,9 +688,10 @@ class DeformableInfantryV2 void can3_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) return; - if (data.can_id == 0x201) + if (data.can_id == 0x201) { chassis_wheel_motors_[3].store_status(data.can_data); - else if (data.can_id == 0x141) { + wheel_status_received_[3].store(true, std::memory_order_relaxed); + } else if (data.can_id == 0x141) { chassis_joint_motors_[3].store_status(data.can_data); joint_status_received_[3].store(true, std::memory_order_relaxed); } else if (data.can_id == 0x205) @@ -616,8 +721,15 @@ class DeformableInfantryV2 device::DjiMotor chassis_wheel_motors_[4]; device::DjiMotor chassis_steer_motors_[4]; device::LkMotor chassis_joint_motors_[4]; + std::atomic wheel_status_received_[4] = {false, false, false, false}; std::atomic joint_status_received_[4] = {false, false, false, false}; + bool debug_log_supercap_ = false; + bool debug_log_wheel_motor_ = false; + bool debug_log_deformable_joint_motor_ = false; + Clock::time_point next_chassis_feedback_log_time_; + Clock::time_point next_supercap_feedback_log_time_; device::Supercap supercap_; + std::atomic latest_supercap_status_{device::CanPacket8{uint64_t{0}}}; std::atomic supercap_status_received_{false}; device::DjiMotor gimbal_bullet_feeder_; @@ -625,6 +737,10 @@ class DeformableInfantryV2 OutputInterface referee_serial_; OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_imu_pitch_; + OutputInterface chassis_imu_roll_; + OutputInterface chassis_imu_pitch_rate_; + OutputInterface chassis_imu_roll_rate_; OutputInterface left_front_joint_physical_angle_; OutputInterface left_back_joint_physical_angle_; OutputInterface right_back_joint_physical_angle_; @@ -638,14 +754,14 @@ class DeformableInfantryV2 OutputInterface radius_; }; - class TopBoard final : private librmcs::agent::CBoard { + class TopBoard final : private librmcs::agent::RmcsBoardLite { public: friend class DeformableInfantryV2; explicit TopBoard( DeformableInfantryV2& deformableInfantry, DeformableInfantryV2Command& deformableInfantry_command, std::string serial_filter = {}) - : librmcs::agent::CBoard( + : librmcs::agent::RmcsBoardLite( serial_filter, librmcs::agent::AdvancedOptions{.dangerously_skip_version_checks = true}) , hard_sync_pending_(deformableInfantry.hard_sync_pending_) @@ -689,7 +805,7 @@ class DeformableInfantryV2 ~TopBoard() override = default; void request_hard_sync_read() { - // RMCS-board top board variant currently has no GPIO hard-sync request path. + // RMCS-lite top board variant currently has no GPIO hard-sync request path. } void update() { @@ -724,13 +840,12 @@ class DeformableInfantryV2 void command_update() { auto builder = start_transmit(); - ; - builder.can1_transmit({ + builder.can0_transmit({ .can_id = 0x141, .can_data = gimbal_pitch_motor_.generate_command().as_bytes(), }); - builder.can2_transmit({ + builder.can1_transmit({ .can_id = 0x200, .can_data = device::CanPacket8{ @@ -746,14 +861,14 @@ class DeformableInfantryV2 private: void uart1_receive_callback(const librmcs::data::UartDataView&) override {} - void can1_receive_callback(const librmcs::data::CanDataView& data) override { + void can0_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; if (data.can_id == 0x141) gimbal_pitch_motor_.store_status(data.can_data); } - void can2_receive_callback(const librmcs::data::CanDataView& data) override { + void can1_receive_callback(const librmcs::data::CanDataView& data) override { if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] return; if (data.can_id == 0x201) @@ -794,7 +909,7 @@ class DeformableInfantryV2 Clock::time_point next_hard_sync_log_time_{}; std::shared_ptr deformable_infantry_command_; - std::unique_ptr rmcs_board_; + std::unique_ptr rmcs_board_lite; std::unique_ptr top_board_; rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; diff --git a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp index 1ac5d4ee..2fc9d17b 100644 --- a/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp +++ b/rmcs_ws/src/rmcs_description/include/rmcs_description/sentry_description.hpp @@ -66,7 +66,7 @@ struct RightFrontWheelLink : fast_tf::Link { template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; @@ -114,37 +114,37 @@ struct fast_tf::Joint : fast_tf::ModificationTracka template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Translation3d transform = Eigen::Translation3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BottomYawLink; + using Parent = rmcs_description::BottomYawLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::PitchLink; + using Parent = rmcs_description::PitchLink; Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); }; template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} @@ -155,7 +155,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -166,7 +166,7 @@ struct fast_tf::Joint : fast_tf::Modificati template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} @@ -177,7 +177,7 @@ struct fast_tf::Joint : fast_tf::Modificat template <> struct fast_tf::Joint : fast_tf::ModificationTrackable { - using Parent = rmcs_description::BaseLink; + using Parent = rmcs_description::BaseLink; Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); void set_state(double angle) { auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} diff --git a/rmcs_ws/src/ros2-hikcamera b/rmcs_ws/src/ros2-hikcamera new file mode 160000 index 00000000..f0077f03 --- /dev/null +++ b/rmcs_ws/src/ros2-hikcamera @@ -0,0 +1 @@ +Subproject commit f0077f034800bcd0dde4fffeff270b733772a57e diff --git a/rmcs_ws/src/serial b/rmcs_ws/src/serial new file mode 160000 index 00000000..d50c4018 --- /dev/null +++ b/rmcs_ws/src/serial @@ -0,0 +1 @@ +Subproject commit d50c401809f17cc5862bdbe37cc6e15737ba7c49 diff --git a/rmcs_ws/toolchain.cmake b/rmcs_ws/toolchain.cmake new file mode 100644 index 00000000..0a2b90fa --- /dev/null +++ b/rmcs_ws/toolchain.cmake @@ -0,0 +1,160 @@ +cmake_minimum_required(VERSION 3.16) + +foreach(_rmcs_var IN ITEMS RMCS_TARGET_ARCH RMCS_SYSROOT RMCS_TARGET_TRIPLET) + if((NOT DEFINED ${_rmcs_var} OR "${${_rmcs_var}}" STREQUAL "") AND DEFINED ENV{${_rmcs_var}}) + set(${_rmcs_var} "$ENV{${_rmcs_var}}") + endif() +endforeach() +unset(_rmcs_var) + +set(RMCS_TARGET_ARCH "${RMCS_TARGET_ARCH}" CACHE STRING "RMCS target architecture") +set(RMCS_SYSROOT "${RMCS_SYSROOT}" CACHE PATH "RMCS target sysroot path") +set(RMCS_TARGET_TRIPLET "${RMCS_TARGET_TRIPLET}" CACHE STRING "RMCS target compiler triplet") +list(APPEND CMAKE_TRY_COMPILE_PLATFORM_VARIABLES + RMCS_TARGET_ARCH + RMCS_SYSROOT + RMCS_TARGET_TRIPLET +) + +if(NOT RMCS_TARGET_ARCH) + message(FATAL_ERROR "RMCS_TARGET_ARCH is required. Supported values: arm64, amd64.") +endif() + +if(NOT RMCS_TARGET_TRIPLET) + if(RMCS_TARGET_ARCH STREQUAL "arm64") + set(RMCS_TARGET_TRIPLET "aarch64-linux-gnu") + elseif(RMCS_TARGET_ARCH STREQUAL "amd64") + set(RMCS_TARGET_TRIPLET "x86_64-linux-gnu") + else() + message(FATAL_ERROR "Unsupported RMCS_TARGET_ARCH: ${RMCS_TARGET_ARCH}") + endif() +endif() + +if(NOT RMCS_SYSROOT) + set(RMCS_SYSROOT "/opt/sysroots/${RMCS_TARGET_ARCH}") +endif() + +if(NOT EXISTS "${RMCS_SYSROOT}") + message(FATAL_ERROR "RMCS_SYSROOT does not exist: ${RMCS_SYSROOT}") +endif() + +set(CMAKE_SYSTEM_NAME Linux) +if(RMCS_TARGET_ARCH STREQUAL "arm64") + set(CMAKE_SYSTEM_PROCESSOR aarch64) +elseif(RMCS_TARGET_ARCH STREQUAL "amd64") + set(CMAKE_SYSTEM_PROCESSOR x86_64) +else() + message(FATAL_ERROR "Unsupported RMCS_TARGET_ARCH: ${RMCS_TARGET_ARCH}") +endif() + +set(CMAKE_SYSROOT "${RMCS_SYSROOT}") +set(_rmcs_find_root_path "${RMCS_SYSROOT}") +foreach(_rmcs_prefix_env IN ITEMS AMENT_PREFIX_PATH CMAKE_PREFIX_PATH) + if(DEFINED ENV{${_rmcs_prefix_env}} AND NOT "$ENV{${_rmcs_prefix_env}}" STREQUAL "") + string(REPLACE ":" ";" _rmcs_env_prefixes "$ENV{${_rmcs_prefix_env}}") + list(APPEND _rmcs_find_root_path ${_rmcs_env_prefixes}) + endif() +endforeach() +list(REMOVE_DUPLICATES _rmcs_find_root_path) +set(CMAKE_FIND_ROOT_PATH + "${_rmcs_find_root_path}" +) +set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) +set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) +set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY) + +function(rmcs_find_host_program output_var) + set(options) + set(oneValueArgs ENV_VAR) + set(multiValueArgs NAMES) + cmake_parse_arguments(RMCS_FIND_HOST_PROGRAM "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(NOT RMCS_FIND_HOST_PROGRAM_NAMES) + message(FATAL_ERROR "rmcs_find_host_program requires at least one candidate in NAMES.") + endif() + + unset(_rmcs_resolved_program CACHE) + unset(_rmcs_resolved_program) + string(REPLACE ":" ";" _rmcs_host_path_entries "$ENV{PATH}") + if(RMCS_FIND_HOST_PROGRAM_ENV_VAR + AND DEFINED ENV{${RMCS_FIND_HOST_PROGRAM_ENV_VAR}} + AND NOT "$ENV{${RMCS_FIND_HOST_PROGRAM_ENV_VAR}}" STREQUAL "") + set(_rmcs_program_candidates "$ENV{${RMCS_FIND_HOST_PROGRAM_ENV_VAR}}") + if(IS_ABSOLUTE "${_rmcs_program_candidates}") + if(NOT EXISTS "${_rmcs_program_candidates}" OR IS_DIRECTORY "${_rmcs_program_candidates}") + message(FATAL_ERROR + "${RMCS_FIND_HOST_PROGRAM_ENV_VAR} points to a missing executable: ${_rmcs_program_candidates}" + ) + endif() + set(_rmcs_resolved_program "${_rmcs_program_candidates}") + else() + find_program(_rmcs_resolved_program + NAMES "${_rmcs_program_candidates}" + PATHS ${_rmcs_host_path_entries} + NO_DEFAULT_PATH + ) + endif() + else() + set(_rmcs_program_candidates ${RMCS_FIND_HOST_PROGRAM_NAMES}) + find_program(_rmcs_resolved_program + NAMES ${_rmcs_program_candidates} + PATHS ${_rmcs_host_path_entries} + NO_DEFAULT_PATH + ) + endif() + + if(NOT _rmcs_resolved_program) + list(JOIN _rmcs_program_candidates ", " _rmcs_program_candidates_str) + message(FATAL_ERROR "Cannot resolve host program. Tried: ${_rmcs_program_candidates_str}") + endif() + + set(${output_var} "${_rmcs_resolved_program}" PARENT_SCOPE) +endfunction() + +rmcs_find_host_program(RMCS_C_COMPILER ENV_VAR CC NAMES "${RMCS_TARGET_TRIPLET}-gcc") +rmcs_find_host_program(RMCS_CXX_COMPILER ENV_VAR CXX NAMES "${RMCS_TARGET_TRIPLET}-g++") +rmcs_find_host_program(RMCS_PYTHON_EXECUTABLE NAMES python3) +rmcs_find_host_program(RMCS_PKG_CONFIG_EXECUTABLE NAMES pkg-config pkgconf) + +if(NOT RMCS_C_COMPILER) + message(FATAL_ERROR "Cannot find C compiler: ${RMCS_TARGET_TRIPLET}-gcc") +endif() +if(NOT RMCS_CXX_COMPILER) + message(FATAL_ERROR "Cannot find CXX compiler: ${RMCS_TARGET_TRIPLET}-g++") +endif() + +set(CMAKE_C_COMPILER "${RMCS_C_COMPILER}") +set(CMAKE_CXX_COMPILER "${RMCS_CXX_COMPILER}") +set(Python3_EXECUTABLE "${RMCS_PYTHON_EXECUTABLE}" CACHE FILEPATH "Host Python interpreter" FORCE) +set(AMENT_PYTHON_EXECUTABLE "${RMCS_PYTHON_EXECUTABLE}" CACHE FILEPATH "Host Python interpreter for ament" FORCE) +set(PKG_CONFIG_EXECUTABLE "${RMCS_PKG_CONFIG_EXECUTABLE}" CACHE FILEPATH "Host pkg-config executable" FORCE) + +if(CMAKE_GENERATOR MATCHES "Makefiles") + rmcs_find_host_program(RMCS_MAKE_PROGRAM NAMES gmake make) + set(CMAKE_MAKE_PROGRAM "${RMCS_MAKE_PROGRAM}" CACHE FILEPATH "Host make program" FORCE) +endif() + +set(CMAKE_PREFIX_PATH + "/opt/ros/jazzy" + "/usr/local" + "/usr" + CACHE STRING "Target-side prefix path for cross-compilation" +) + +set(ENV{PKG_CONFIG_SYSROOT_DIR} "${RMCS_SYSROOT}") +set(_rmcs_pkg_config_libdirs + "${RMCS_SYSROOT}/usr/local/lib/${RMCS_TARGET_TRIPLET}/pkgconfig" + "${RMCS_SYSROOT}/usr/local/lib/pkgconfig" + "${RMCS_SYSROOT}/usr/local/share/pkgconfig" + "${RMCS_SYSROOT}/usr/lib/${RMCS_TARGET_TRIPLET}/pkgconfig" + "${RMCS_SYSROOT}/usr/lib/pkgconfig" + "${RMCS_SYSROOT}/usr/share/pkgconfig" + "${RMCS_SYSROOT}/lib/${RMCS_TARGET_TRIPLET}/pkgconfig" + "${RMCS_SYSROOT}/lib/pkgconfig" + "${RMCS_SYSROOT}/opt/ros/jazzy/lib/${RMCS_TARGET_TRIPLET}/pkgconfig" + "${RMCS_SYSROOT}/opt/ros/jazzy/lib/pkgconfig" + "${RMCS_SYSROOT}/opt/ros/jazzy/share/pkgconfig" +) +list(JOIN _rmcs_pkg_config_libdirs ":" _rmcs_pkg_config_libdir) +set(ENV{PKG_CONFIG_LIBDIR} "${_rmcs_pkg_config_libdir}")