diff --git a/.github/include/base_image_config.json b/.github/include/base_image_config.json new file mode 100644 index 0000000..0356006 --- /dev/null +++ b/.github/include/base_image_config.json @@ -0,0 +1,176 @@ +{ "include": +[ + { + "tag": "humble-ubuntu22.04", + "external_image": "ubuntu:22.04", + "injections": { + "ros2": "humble" + } + }, + { + "tag": "foxy-ubuntu20.04", + "external_image": "ubuntu:20.04", + "injections": { + "ros2": "foxy" + } + }, + + + + + + { + "tag": "cuda11.0-foxy-ubuntu20.04", + "external_image": "nvcr.io/nvidia/cuda:11.0.3-runtime-ubuntu20.04", + "injections": { + "ros2": "foxy" + } + }, + { + "tag": "cuda11.2-foxy-ubuntu20.04", + "external_image": "nvcr.io/nvidia/cuda:11.2.2-runtime-ubuntu20.04", + "injections": { + "ros2": "foxy" + } + }, + { + "tag": "cuda11.4-foxy-ubuntu20.04", + "external_image": "nvcr.io/nvidia/cuda:11.4.3-runtime-ubuntu20.04", + "injections": { + "ros2": "foxy" + } + }, + { + "tag": "cuda11.7-humble-ubuntu22.04", + "external_image": "nvcr.io/nvidia/cuda:11.7.1-runtime-ubuntu22.04", + "injections": { + "ros2": "humble" + } + }, + { + "tag": "cuda11.8-humble-ubuntu22.04", + "external_image": "nvcr.io/nvidia/cuda:11.8.0-runtime-ubuntu22.04", + "injections": { + "ros2": "humble" + } + }, + { + "tag": "cuda12.0-humble-ubuntu22.04", + "external_image": "nvcr.io/nvidia/cuda:12.0.0-runtime-ubuntu22.04", + "injections": { + "ros2": "humble" + } + }, + { + "tag": "cuda12.2-humble-ubuntu22.04", + "external_image": "nvcr.io/nvidia/cuda:12.2.2-runtime-ubuntu22.04", + "injections": { + "ros2": "humble" + } + }, + + + { + "tag": "cuda11.0-foxy-ubuntu20.04-devel", + "external_image": "nvcr.io/nvidia/cuda:11.0.3-devel-ubuntu20.04", + "injections": { + "ros2": "foxy" + } + }, + { + "tag": "cuda11.2-foxy-ubuntu20.04-devel", + "external_image": "nvcr.io/nvidia/cuda:11.2.2-devel-ubuntu20.04", + "injections": { + "ros2": "foxy" + } + }, + { + "tag": "cuda11.4-foxy-ubuntu20.04-devel", + "external_image": "nvcr.io/nvidia/cuda:11.4.3-devel-ubuntu20.04", + "injections": { + "ros2": "foxy" + } + }, + { + "tag": "cuda11.7-humble-ubuntu22.04-devel", + "external_image": "nvcr.io/nvidia/cuda:11.7.1-devel-ubuntu22.04", + "injections": { + "ros2": "humble" + } + }, + { + "tag": "cuda11.8-humble-ubuntu22.04-devel", + "external_image": "nvcr.io/nvidia/cuda:11.8.0-devel-ubuntu22.04", + "injections": { + "ros2": "humble" + } + }, + { + "tag": "cuda12.0-humble-ubuntu22.04-devel", + "external_image": "nvcr.io/nvidia/cuda:12.0.0-devel-ubuntu22.04", + "injections": { + "ros2": "humble" + } + }, + { + "tag": "cuda12.2-humble-ubuntu22.04-devel", + "external_image": "nvcr.io/nvidia/cuda:12.2.2-devel-ubuntu22.04", + "injections": { + "ros2": "humble" + } + }, + + + + + { + "tag": "cuda11.0-cudnn8-foxy-ubuntu20.04", + "external_image": "nvcr.io/nvidia/cuda:11.0.3-cudnn8-runtime-ubuntu20.04", + "injections": { + "ros2": "foxy" + } + }, + { + "tag": "cuda11.2-cudnn8-foxy-ubuntu20.04", + "external_image": "nvcr.io/nvidia/cuda:11.2.2-cudnn8-runtime-ubuntu20.04", + "injections": { + "ros2": "foxy" + } + }, + { + "tag": "cuda11.4-cudnn8-foxy-ubuntu20.04", + "external_image": "nvcr.io/nvidia/cuda:11.4.3-cudnn8-runtime-ubuntu20.04", + "injections": { + "ros2": "foxy" + } + }, + { + "tag": "cuda11.7-cudnn8-humble-ubuntu22.04", + "external_image": "nvcr.io/nvidia/cuda:11.7.1-cudnn8-runtime-ubuntu22.04", + "injections": { + "ros2": "humble" + } + }, + { + "tag": "cuda11.8-cudnn8-humble-ubuntu22.04", + "external_image": "nvcr.io/nvidia/cuda:11.8.0-cudnn8-runtime-ubuntu22.04", + "injections": { + "ros2": "humble" + } + }, + { + "tag": "cuda12.0-cudnn8-humble-ubuntu22.04", + "external_image": "nvcr.io/nvidia/cuda:12.0.0-cudnn8-runtime-ubuntu22.04", + "injections": { + "ros2": "humble" + } + }, + { + "tag": "cuda12.2-cudnn8-humble-ubuntu22.04", + "external_image": "nvcr.io/nvidia/cuda:12.2.2-cudnn8-runtime-ubuntu22.04", + "injections": { + "ros2": "humble" + } + } +] +} diff --git a/.github/templates/check_src_changes/action.yml b/.github/templates/check_src_changes/action.yml new file mode 100644 index 0000000..0d00a30 --- /dev/null +++ b/.github/templates/check_src_changes/action.yml @@ -0,0 +1,79 @@ +name: Check source file changes + +outputs: + modified_modules: + description: "Space deliminated list of modified modules" + value: ${{ steps.output-changes.outputs.modified_modules }} + +runs: + using: "composite" + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Find changed files outside of development folders + id: changed-files-outside-development + uses: tj-actions/changed-files@v42 + with: + files: | + !autonomy/** + !embedded/** + + - name: Find changed files inside autonomy/wato_msgs folder + id: changed-files-wato-msgs + uses: tj-actions/changed-files@v42 + with: + files: autonomy/wato_msgs/** + + - name: Find changed files inside autonomy/controller folder + id: changed-files-controller + uses: tj-actions/changed-files@v42 + with: + files: autonomy/controller/** + + - name: Get changed files inside autonomy/interfacing folder + id: changed-files-interfacing + uses: tj-actions/changed-files@v42 + with: + files: autonomy/interfacing/** + + - name: Get changed files inside autonomy/perception folder + id: changed-files-perception + uses: tj-actions/changed-files@v42 + with: + files: autonomy/perception/** + + - name: Get changed files inside autonomy/samples folder + id: changed-files-samples + uses: tj-actions/changed-files@v42 + with: + files: autonomy/samples/** + + - name: Get changed files inside autonomy/simulation folder + id: changed-files-simulation + uses: tj-actions/changed-files@v42 + with: + files: autonomy/simulation/** + + - name: Find changed files inside embedded folder + id: changed-files-embedded + uses: tj-actions/changed-files@v42 + with: + files: embedded/** + + - name: Create list of changed modules + id: output-changes + env: + INFRASTRUCTURE_CHANGED: > + ${{ steps.changed-files-wato-msgs.outputs.any_changed == 'true' + || steps.changed-files-outside-development.outputs.any_changed == 'true' }} + CONTROLLER_CHANGED: ${{ steps.changed-files-controller.outputs.any_changed }} + INTERFACING_CHANGED: ${{ steps.changed-files-interfacing.outputs.any_changed }} + PERCEPTION_CHANGED: ${{ steps.changed-files-perception.outputs.any_changed }} + SAMPLES_CHANGED: ${{ steps.changed-files-samples.outputs.any_changed }} + SIMULATION_CHANGED: ${{ steps.changed-files-simulation.outputs.any_changed }} + EMBEDDED_CHANGED: ${{ steps.changed-files-embedded.outputs.any_changed }} + + run: ${{ github.action_path }}/check_src_changes.sh + shell: bash \ No newline at end of file diff --git a/.github/templates/check_src_changes/check_src_changes.sh b/.github/templates/check_src_changes/check_src_changes.sh new file mode 100755 index 0000000..d5f26c1 --- /dev/null +++ b/.github/templates/check_src_changes/check_src_changes.sh @@ -0,0 +1,60 @@ +#!/bin/bash +set -e + +################# Create a space delimited list of modified modules ################# +# Outputs a list of modified modules by comparing changes between main and current commit +# References previous GitHub workflow steps + +# Controller +if [ $CONTROLLER_CHANGED == 'true' ]; then + echo "Detected controller changes" + MODIFIED_MODULES+="controller " +fi + +# Interfacing +if [ $INTERFACING_CHANGED == 'true' ]; then + echo "Detected interfacing changes" + MODIFIED_MODULES+="interfacing " +fi + +# Perception +if [ $PERCEPTION_CHANGED == 'true' ]; then + echo "Detected perception changes" + MODIFIED_MODULES+="perception " +fi + +# Samples +if [ $SAMPLES_CHANGED == 'true' ]; then + echo "Detected samples changes" + MODIFIED_MODULES+="samples " +fi + +# Simulation +if [ $SIMULATION_CHANGED == 'true' ]; then + echo "Detected simulation changes" + MODIFIED_MODULES+="simulation " +fi +# Embedded +if [ $EMBEDDED_CHANGED == 'true' ]; then + echo "::notice:: Detected infrastructure changes" + MODIFIED_MODULES+="embedded" +fi + +# Infrastructure +if [ $INFRASTRUCTURE_CHANGED == 'true' ]; then + echo "::notice:: Detected infrastructure changes" + MODIFIED_MODULES+="infrastructure" +fi + +# Embedded +if [ $EMBEDDED_CHANGED == 'true' ]; then + echo "::notice:: Detected infrastructure changes" + MODIFIED_MODULES="embedded" +else + echo "::notice:: MODIFIED_MODULES are $MODIFIED_MODULES" +fi + + + +# Output lis +echo "modified_modules=$MODIFIED_MODULES" >> $GITHUB_OUTPUT \ No newline at end of file diff --git a/.github/templates/docker_context/action.yml b/.github/templates/docker_context/action.yml new file mode 100644 index 0000000..c933283 --- /dev/null +++ b/.github/templates/docker_context/action.yml @@ -0,0 +1,31 @@ +name: Generate Docker Environment + +inputs: + modified_modules: + description: "Space deliminated list of modified modules" + required: true + default: '' + modules_dir: + description: "Path to modules folder" + required: false + default: "/" + +outputs: + docker_matrix: + description: "list of docker compose services" + value: ${{ steps.environment-generator.outputs.docker_matrix }} + registry: + description: "name of the docker registry we are using" + value: ${{ steps.environment-generator.outputs.registry }} + repository: + description: "name of the docker repository we are using" + value: ${{ steps.environment-generator.outputs.repository }} + +runs: + using: "composite" + steps: + - id: environment-generator + env: + MODIFIED_MODULES: ${{ inputs.modified_modules }} + run: ${{ github.action_path }}/docker_context.sh + shell: bash diff --git a/.github/templates/docker_context/docker_context.sh b/.github/templates/docker_context/docker_context.sh new file mode 100755 index 0000000..30fcd57 --- /dev/null +++ b/.github/templates/docker_context/docker_context.sh @@ -0,0 +1,76 @@ +#!/bin/bash +set -e + +################# Sweep for Docker Services and Modules ################# +# Scans for services and modules in the wato_monorepo, +# dynamically builds a json matrix for downstream CI build and testing + +# Find docker compose files in 'modules' directory +modules=$(find ${GITHUB_WORKSPACE}/modules -maxdepth 1 -name "docker-compose*") + +# Initialize an empty array for JSON objects +json_objects=() + +# Check for infrastructure changes +TEST_ALL=false + +if [[ $MODIFIED_MODULES = "infrastructure" ]]; then + TEST_ALL=true + echo Testing all "$MODULES_DIR" +fi + +# Loop through each module +while read -r module; do + + # Retrieve docker compose service names + services=$(docker compose -f "$module" --profile deploy --profile develop config --services) + module_out=$(basename $(echo "$module" | sed -n 's/modules\/docker-compose\.\(.*\)\.yaml/\1/p')) + + # Skip simulation module + # TODO: Add custom handling for embedded testing + if [[ 'simulation' = $module_out || 'embedded' = $module_out ]]; then + continue + fi + + # Only work with modules that are modified + if [[ $MODIFIED_MODULES != *$module_out* && $TEST_ALL = "false" ]]; then + continue + fi + + # Loop through each service + while read -r service_out; do + # Temporarily skip perception services that have too large image size + # if [[ "$service_out" == "lane_detection" ]] || \ + # [[ "$service_out" == "camera_object_detection" ]] || \ + # continue + # fi + + # TODO: Expose whole profile object to env + dockerfile=$(docker compose -f "$module" --profile deploy --profile develop config | yq ".services.$service_out.build.dockerfile") + json_object=$(jq -nc --arg module_out "$module_out" --arg service_out "$service_out" --arg dockerfile "$dockerfile" \ + '{module: $module_out, service: $service_out, dockerfile: $dockerfile}') + # Append JSON object to the array + json_objects+=($json_object) + + done <<< "$services" +done <<< "$modules" + +# Convert the array of JSON objects to a single JSON array +json_services=$(jq -nc '[( $ARGS.positional[] | fromjson )]' --args -- ${json_objects[*]}) + +if [ -z $json_services ]; then + exit 1 +fi + +echo output "docker_matrix=$(echo $json_services | jq -c '{include: .}')" +echo "docker_matrix=$(echo $json_services | jq -c '{include: .}')" >> $GITHUB_OUTPUT + +################# Setup Docker Registry and Repository Name ################# +# Docker Registry to pull/push images +REGISTRY_URL="ghcr.io/watonomous/humanoid" + +REGISTRY=$(echo "$REGISTRY_URL" | sed 's|^\(.*\)/.*$|\1|') +REPOSITORY=$(echo "$REGISTRY_URL" | sed 's|^.*/\(.*\)$|\1|') + +echo "registry=$REGISTRY" >> $GITHUB_OUTPUT +echo "repository=$REPOSITORY" >> $GITHUB_OUTPUT \ No newline at end of file diff --git a/.github/templates/github_context/action.yml b/.github/templates/github_context/action.yml new file mode 100644 index 0000000..735b2d6 --- /dev/null +++ b/.github/templates/github_context/action.yml @@ -0,0 +1,19 @@ +name: Generate GitHub Environment + +outputs: + source_branch: + description: "branch we are currently on" + value: ${{ steps.environment-generator.outputs.source_branch }} + target_branch: + description: "branch we are trying to merge into" + value: ${{ steps.environment-generator.outputs.target_branch }} + +runs: + using: "composite" + steps: + - id: environment-generator + run: ${{ github.action_path }}/branch_sanitation.sh + shell: bash + env: + SOURCE_BRANCH: ${{ github.head_ref || github.ref_name }} + TARGET_BRANCH: ${{ github.base_ref }} \ No newline at end of file diff --git a/.github/templates/github_context/branch_sanitation.sh b/.github/templates/github_context/branch_sanitation.sh new file mode 100755 index 0000000..bfd485f --- /dev/null +++ b/.github/templates/github_context/branch_sanitation.sh @@ -0,0 +1,23 @@ +#!/bin/bash +set -e + +# Takes in a branch name and sanitizes the name for docker tagging +# (eg. no / in the branch name) +sanitize_branch_name() { + echo $(echo $1 | sed 's/[^a-zA-Z0-9._]/-/g' | cut -c 1-128) +} + +################# Setup Source and Target Branch ################# +SOURCE_BRANCH_NAME=$(sanitize_branch_name $SOURCE_BRANCH) +TARGET_BRANCH_NAME=$(sanitize_branch_name $TARGET_BRANCH) + +if [ -z "$TARGET_BRANCH_NAME" ]; then + TARGET_BRANCH_NAME=$SOURCE_BRANCH_NAME +fi + +echo "source_branch=$SOURCE_BRANCH_NAME" >> $GITHUB_OUTPUT +echo "target_branch=$TARGET_BRANCH_NAME" >> $GITHUB_OUTPUT + +################# Debug notices ################# +echo "::notice:: Using $SOURCE_BRANCH_NAME as the source branch" +echo "::notice:: Using $TARGET_BRANCH_NAME as the target branch" diff --git a/.github/templates/test/action.yml b/.github/templates/test/action.yml new file mode 100644 index 0000000..386d6a4 --- /dev/null +++ b/.github/templates/test/action.yml @@ -0,0 +1,18 @@ +name: Docker test + +inputs: + image: + description: "monorepo image to test" + required: true + tag: + description: "monorepo image tag to test" + required: true + +runs: + using: "composite" + steps: + - run: ${{ github.action_path }}/test_image.sh + shell: bash + env: + IMAGE: ${{ inputs.image }} + TAG: ${{ inputs.tag }} \ No newline at end of file diff --git a/.github/templates/test/test_image.sh b/.github/templates/test/test_image.sh new file mode 100755 index 0000000..cb7e2a8 --- /dev/null +++ b/.github/templates/test/test_image.sh @@ -0,0 +1,5 @@ +#!/bin/bash +set -e + +docker pull -q $IMAGE:$TAG +docker run $IMAGE:$TAG /bin/bash -c "source /home/bolty/ament_ws/install/setup.bash; colcon test; colcon test-result --verbose" diff --git a/.github/workflows/build_and_unitest.yml b/.github/workflows/build_and_unitest.yml new file mode 100644 index 0000000..b79ae42 --- /dev/null +++ b/.github/workflows/build_and_unitest.yml @@ -0,0 +1,161 @@ +name: Build and Test Monorepo + +on: + push: + branches: + - main + pull_request: + branches: + - main + +jobs: + setup-environment: + name: Setup Environment + runs-on: ubuntu-latest + + outputs: + docker_matrix: ${{ steps.docker-environment.outputs.docker_matrix }} + registry: ${{ steps.docker-environment.outputs.registry }} + repository: ${{ steps.docker-environment.outputs.repository }} + source_branch: ${{ steps.github-environment.outputs.source_branch }} + target_branch: ${{ steps.github-environment.outputs.target_branch }} + + steps: + - name: Checkout Repository + uses: actions/checkout@v4 + + - name: Get Module Changes + id: get-module-changes + uses: "./.github/templates/check_src_changes" + + - name: Setup Watod Environment + run: | + MODULES_DIR="$GITHUB_WORKSPACE/modules" + . ./watod_scripts/watod-setup-docker-env.sh + shell: bash + + - name: Generate Docker Environment + id: docker-environment + uses: "./.github/templates/docker_context" + with: + modified_modules: ${{ steps.get-module-changes.outputs.modified_modules }} + - name: Generate GitHub Environment + id: github-environment + uses: "./.github/templates/github_context" + + build-and-unittest: + name: Build/Test + runs-on: ubuntu-latest + needs: setup-environment + + env: + DOCKER_REGISTRY: ${{ needs.setup-environment.outputs.registry }} + DOCKER_REPOSITORY: ${{ needs.setup-environment.outputs.repository }} + SOURCE_BRANCH: ${{ needs.setup-environment.outputs.source_branch }} + TARGET_BRANCH: ${{ needs.setup-environment.outputs.target_branch }} + + strategy: + fail-fast: false + matrix: ${{ fromJSON(needs.setup-environment.outputs.docker_matrix) }} + + concurrency: + group: ${{ matrix.service }}-${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Construct Registry URL + id: construct-registry-url + run: | + echo "url=${{ env.DOCKER_REGISTRY }}/${{ env.DOCKER_REPOSITORY }}/${{ matrix.module }}/${{ matrix.service }}" \ + >> $GITHUB_OUTPUT + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + id: buildx + + - name: Docker Login + uses: docker/login-action@v3 + with: + registry: ${{ env.DOCKER_REGISTRY }} + username: ${{ secrets.GHCR_USER }} + password: ${{ secrets.GHCR_PWD }} + + - name: Prepare Image Dependencies + if: ${{ matrix.module != 'infrastructure' }} + uses: docker/build-push-action@v5 + with: + context: . + file: ${{ matrix.dockerfile }} + push: true + tags: | + ${{ steps.construct-registry-url.outputs.url }}:source_${{ env.SOURCE_BRANCH }} + cache-from: | + ${{ steps.construct-registry-url.outputs.url }}:source_${{ env.SOURCE_BRANCH }} + ${{ steps.construct-registry-url.outputs.url }}:source_${{ env.TARGET_BRANCH }} + cache-to: type=inline + builder: ${{ steps.buildx.outputs.name }} + target: dependencies + + - name: Build Image from Source + uses: docker/build-push-action@v5 + with: + context: . + file: ${{ matrix.dockerfile }} + push: true + tags: | + ${{ steps.construct-registry-url.outputs.url }}:build_${{ env.SOURCE_BRANCH }} + cache-from: | + ${{ steps.construct-registry-url.outputs.url }}:source_${{ env.SOURCE_BRANCH }} + ${{ steps.construct-registry-url.outputs.url }}:build_${{ env.SOURCE_BRANCH }} + ${{ steps.construct-registry-url.outputs.url }}:build_${{ env.TARGET_BRANCH }} + cache-to: type=inline + builder: ${{ steps.buildx.outputs.name }} + target: build + + # TODO: Expose the whole profile to the environment and check via compose flags + # - name: Check if deploy stage exists + # id: check_deploy_stage + # run: | + # if ! grep -q 'FROM.*AS deploy' ${{ matrix.dockerfile }}; then + # echo "Deploy stage not found, skipping build" + # echo "skip_build=true" >> $GITHUB_OUTPUT + # else + # echo "Deploy stage found, proceeding with build" + # echo "skip_build=false" >> $GITHUB_OUTPUT + + # - name: Security Sanitation for Deployment + # if: ${{ env.skip_build != 'true' }} + # uses: docker/build-push-action@v5 + # with: + # context: . + # file: ${{ matrix.dockerfile }} + # push: true + # tags: | + # ${{ steps.construct-registry-url.outputs.url }}:${{ env.SOURCE_BRANCH }} + # cache-from: | + # ${{ steps.construct-registry-url.outputs.url }}:build_${{ env.SOURCE_BRANCH }} + # ${{ steps.construct-registry-url.outputs.url }}:build_${{ env.TARGET_BRANCH }} + # builder: ${{ steps.buildx.outputs.name }} + # target: deploy + + - name: Run testing suite + uses: "./.github/templates/test" + env: + DOCKER_BUILDKIT: 1 + COMPOSE_DOCKER_CLI_BUILD: 1 + BUILDKIT_INLINE_CACHE: 1 + with: + image: ${{ steps.construct-registry-url.outputs.url }} + tag: build_${{ env.SOURCE_BRANCH }} + + confirm-build-and-unittest-complete: + name: Confirm Build and Unit Tests Completed + needs: build-and-unittest + runs-on: ubuntu-latest + steps: + - name: Ending + run: | + echo "::notice:: All builds and unit tests completed!" diff --git a/.github/workflows/build_base_images.yml b/.github/workflows/build_base_images.yml new file mode 100644 index 0000000..0c32cb3 --- /dev/null +++ b/.github/workflows/build_base_images.yml @@ -0,0 +1,112 @@ +name: Build Monorepo Base Images + +on: + workflow_dispatch: + +jobs: + setup-environment: + name: Setup environment + runs-on: ubuntu-latest + + outputs: + registry: ${{ steps.docker-environment.outputs.registry }} + repository: ${{ steps.docker-environment.outputs.repository }} + + steps: + - name: Checkout Repository + uses: actions/checkout@v4 + + - name: Setup Watod Environment + run: | + MODULES_DIR="$GITHUB_WORKSPACE/modules" + . ./watod_scripts/watod-setup-docker-env.sh + shell: bash + + - name: Generate Docker Environment + id: docker-environment + uses: "./.github/templates/docker_context" + + base-matrix-prep: + name: Prepare Base Image Matrix JSON + runs-on: ubuntu-latest + outputs: + matrix: ${{ steps.prep.outputs.matrix }} + auth_users: ${{ steps.prep.outputs.auth_users }} + steps: + - name: Check out code + uses: actions/checkout@v4 + - name: Setup Base Image Matrix and Auth + id: prep + run: | + MATRIX=$(jq -c . .github/include/base_image_config.json) + + echo "Base Image Matrix: $MATRIX" + echo "matrix=$MATRIX" >> $GITHUB_OUTPUT + + build: + name: Build Base Images + runs-on: ubuntu-latest + needs: [ setup-environment, base-matrix-prep ] + + env: + DOCKER_REGISTRY: ${{ needs.setup-environment.outputs.registry }} + DOCKER_REPOSITORY: ${{ needs.setup-environment.outputs.repository }} + + strategy: + fail-fast: false + matrix: ${{ fromJSON(needs.base-matrix-prep.outputs.matrix) }} + + concurrency: + group: ${{ matrix.tag }}-${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + + steps: + # Initial workflow setup + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Construct Registry URL + id: construct-registry-url + run: | + echo "url=${{ env.DOCKER_REGISTRY }}/${{ env.DOCKER_REPOSITORY }}/base:${{ matrix.tag }}" \ + >> $GITHUB_OUTPUT + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + id: buildx + + - name: Docker Login + uses: docker/login-action@v3 + with: + registry: ${{ env.DOCKER_REGISTRY }} + username: ${{ secrets.GHCR_USER }} + password: ${{ secrets.GHCR_PWD }} + + - name: Inject ROS2 into Image + if: ${{ matrix.injections.ros2 != '' }} + uses: docker/build-push-action@v5 + with: + context: . + file: docker/base/inject_ros2.Dockerfile + push: true + tags: | + ${{ steps.construct-registry-url.outputs.url }}_generic + builder: ${{ steps.buildx.outputs.name }} + target: devel + build-args: | + GENERIC_IMAGE=${{ matrix.external_image }} + ROS_DISTRO=${{ matrix.injections.ros2 }} + + - name: Prepare Generic Image as Monorepo Base Image + uses: docker/build-push-action@v5 + with: + context: . + file: docker/base/inject_wato_base.Dockerfile + push: true + tags: | + ${{ steps.construct-registry-url.outputs.url }} + builder: ${{ steps.buildx.outputs.name }} + target: wato_base + build-args: | + GENERIC_IMAGE=${{ steps.construct-registry-url.outputs.url }}_generic + USER_PASSWD=${{ secrets.HUMANOIDREPO_PASSWD }} diff --git a/.github/workflows/linting_auto.yml b/.github/workflows/linting_auto.yml new file mode 100644 index 0000000..46d19da --- /dev/null +++ b/.github/workflows/linting_auto.yml @@ -0,0 +1,55 @@ +name: Monorepo Code Linting - Auto Linter + +on: + push: + branches: + - main + pull_request: + branches: + - main + types: + - unlabeled + - labeled + - synchronize + +jobs: + run-linters: + name: Run C++/Python linters + runs-on: ubuntu-latest + + steps: + - name: Check out Git repository + uses: actions/checkout@v4 + with: + token: ${{ secrets.GHCR_PWD }} + + - name: Set up Python + uses: actions/setup-python@v1 + with: + python-version: 3.10.18 + + - name: Install Python dependencies + run: pip install autopep8 clang-format + + - name: Run linters, make changes + continue-on-error: true + uses: WATonomous/wato-lint-action@v1 + with: + auto_fix: ${{ contains(github.event.pull_request.labels.*.name, 'auto-lint') }} + Autopep8: true + Autopep8_args: "--max-line-length 100" + clang_format: true + clang_format_auto_fix: ${{ contains(github.event.pull_request.labels.*.name, 'auto-lint') }} + clang_format_args: "-style=file" + + - name: Remove 'auto-lint' label to stop possible inf loop + if: contains(github.event.pull_request.labels.*.name, 'auto-lint') + uses: actions/github-script@v6 + with: + script: | + await github.rest.issues.removeLabel({ + owner: context.repo.owner, + repo: context.repo.repo, + issue_number: context.issue.number, + name: 'auto-lint' + }); diff --git a/README.md b/README.md index 0fb89fe..0fe747b 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,55 @@ # WATonomous + UWRL Humanoid Project +Dockerized ROS2 setup controlling and interfacing with Humanoid Hand + +## Repo Structure +``` +humanoid +├── watod-setup-env.sh +├── docker +│ ├── samples +│ │ └── cpp +│ │ ├── Dockerfile.aggregator +│ │ ├── Dockerfile.producer +│ │ └── Dockerfile.transformer +│ └── wato_ros_entrypoint.sh +├── docs +├── modules +│ └── docker-compose.samples.yaml +├── scripts +├── autonomy +│ ├── perception +│ ├── wato_msgs +│ │ └── sample_msgs +│ │ ├── CMakeLists.txt +│ │ ├── msg +│ │ └── package.xml +│ ├── samples +│ │ ├── README.md +│ │ └── cpp +│ │ ├── aggregator +│ │ ├── image +│ │ ├── producer +│ │ ├── README.md +│ │ └── transformer +│ ├── controller +│ ├── interfacing +│ ├── simulation +├── embedded +│ ├── ESP32 +│ ├── STM32 +│ ├── torque-testing +└── watod +``` +## Documentation +Documentation structure of this repo can be found [docs/README.md](docs/README.md) + +Before developing please read these documents. + +1. [Documentation Structure of Repo](docs/README.md) +2. [WATO Infrastructure Development Docs](https://github.com/WATonomous/wato_monorepo/tree/main/docs/dev/) +3. [ROS Node/Core Structure Docs](autonomy/samples/README.md) + +## Dependencies: +- Ubuntu >= 22.04, Windows (WSL), and MacOS. -## Prerequisite Installation -These steps are to setup the repo to work on your own PC. We utilize docker to enable ease of reproducibility and deployability. -1. Need Linux Ubuntu >= 22.04, Windows (WSL), and MacOS. diff --git a/autonomy/controller/README.md b/autonomy/controller/README.md new file mode 100644 index 0000000..30ee9d3 --- /dev/null +++ b/autonomy/controller/README.md @@ -0,0 +1,24 @@ +# Teleop +## Samples +To run rosbridge listener: +``` +ros2 run rosbridge_listener rosbridge_listener_node +``` + +To run rosbridge publisher: + +``` +ros2 run rosbridge_publisher rosbridge_publisher_node +``` + +## Running Rosbridge Server +Rosbridge server will relay ros topics to humanoid-server. To run, use: +``` +ros2 launch rosbridge_server rosbridge_websocket_launch.xml +``` +make sure to +``` +source install/setup.bash +``` +any packages containing messages etc. that you need + diff --git a/autonomy/controller/controller.launch.py b/autonomy/controller/controller.launch.py new file mode 100644 index 0000000..694f2f6 --- /dev/null +++ b/autonomy/controller/controller.launch.py @@ -0,0 +1,20 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + """Launch rosbridge listener and publisher nodes.""" + rosbridge_listener_node = Node( + package='rosbridge_listener', + executable='rosbridge_listener', + ) + + rosbridge_publisher_node = Node( + package='rosbridge_publisher', + executable='rosbridge_publisher', + ) + + return LaunchDescription([ + rosbridge_listener_node, + rosbridge_publisher_node + ]) \ No newline at end of file diff --git a/autonomy/controller/package.xml b/autonomy/controller/package.xml new file mode 100644 index 0000000..70e0deb --- /dev/null +++ b/autonomy/controller/package.xml @@ -0,0 +1,19 @@ + + + + controller + 0.0.0 + Controller launch package + WATonomous + MIT + + rosbridge_listener + rosbridge_publisher + + ament_lint_auto + ament_lint_common + + + ament_python + + \ No newline at end of file diff --git a/autonomy/controller/rosbridge_listener/CMakeLists.txt b/autonomy/controller/rosbridge_listener/CMakeLists.txt new file mode 100644 index 0000000..1a56c21 --- /dev/null +++ b/autonomy/controller/rosbridge_listener/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.8) +project(rosbridge_listener) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sample_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_library(rosbridge_listener_lib + src/rosbridge_listener_core.cpp) + +target_include_directories(rosbridge_listener_lib + PUBLIC include) + +add_executable(rosbridge_listener_node src/rosbridge_listener_node.cpp) + +ament_target_dependencies(rosbridge_listener_node rclcpp std_msgs sample_msgs) +target_include_directories(rosbridge_listener_node PUBLIC + $ + $) +target_compile_features(rosbridge_listener_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS rosbridge_listener_node + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/samples/python/aggregator/aggregator/__init__.py b/autonomy/controller/rosbridge_listener/include/rosbridge_listener_core.hpp old mode 100755 new mode 100644 similarity index 100% rename from src/samples/python/aggregator/aggregator/__init__.py rename to autonomy/controller/rosbridge_listener/include/rosbridge_listener_core.hpp diff --git a/autonomy/controller/rosbridge_listener/include/rosbridge_listener_node.hpp b/autonomy/controller/rosbridge_listener/include/rosbridge_listener_node.hpp new file mode 100644 index 0000000..5808f83 --- /dev/null +++ b/autonomy/controller/rosbridge_listener/include/rosbridge_listener_node.hpp @@ -0,0 +1,27 @@ +#ifndef ROSBRIDGE_LISTENER_NODE_HPP_ +#define ROSBRIDGE_LISTENER_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "sample_msgs/msg/hand_pose.hpp" + +#include "rosbridge_listener_core.hpp" + +/** + * Implementation of a ROS2 node that listens to the "unfiltered" and "filtered" + * topics and echoes the operating frequency of the topic to the console. + */ +class ROSbridgeListenerNode : public rclcpp::Node { +public: + ROSbridgeListenerNode(); + +private: + std::string + hand_pose_to_string(const sample_msgs::msg::HandPose::SharedPtr pose); + + void hand_pose_callback(const sample_msgs::msg::HandPose::SharedPtr msg); + + rclcpp::Subscription::SharedPtr hand_pose_sub_; +}; + +#endif \ No newline at end of file diff --git a/autonomy/controller/rosbridge_listener/launch/rosbridge_listener.launch.py b/autonomy/controller/rosbridge_listener/launch/rosbridge_listener.launch.py new file mode 100755 index 0000000..a5e6ea3 --- /dev/null +++ b/autonomy/controller/rosbridge_listener/launch/rosbridge_listener.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + """Launch rosbridge listener node.""" + rosbridge_listener_node = Node( + package='rosbridge_listener', + executable='rosbridge_listener', + ) + + return LaunchDescription([ + rosbridge_listener_node + ]) diff --git a/autonomy/controller/rosbridge_listener/package.xml b/autonomy/controller/rosbridge_listener/package.xml new file mode 100644 index 0000000..e70774a --- /dev/null +++ b/autonomy/controller/rosbridge_listener/package.xml @@ -0,0 +1,21 @@ + + + + rosbridge_listener + 0.0.0 + TODO: Package description + yichen + TODO: License declaration + + ament_cmake + + rclcpp + sample_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/samples/python/aggregator/resource/aggregator b/autonomy/controller/rosbridge_listener/src/rosbridge_listener_core.cpp similarity index 100% rename from src/samples/python/aggregator/resource/aggregator rename to autonomy/controller/rosbridge_listener/src/rosbridge_listener_core.cpp diff --git a/autonomy/controller/rosbridge_listener/src/rosbridge_listener_node.cpp b/autonomy/controller/rosbridge_listener/src/rosbridge_listener_node.cpp new file mode 100644 index 0000000..7c2be64 --- /dev/null +++ b/autonomy/controller/rosbridge_listener/src/rosbridge_listener_node.cpp @@ -0,0 +1,38 @@ +#include "rosbridge_listener_node.hpp" + +#include +#include + +ROSbridgeListenerNode::ROSbridgeListenerNode() : Node("rosbridge_listener") { + + hand_pose_sub_ = this->create_subscription( + "teleop", 10, + std::bind(&ROSbridgeListenerNode::hand_pose_callback, this, + std::placeholders::_1)); +} + +std::string ROSbridgeListenerNode::hand_pose_to_string( + const sample_msgs::msg::HandPose::SharedPtr pose) { + std::string s; + std::vector hand_pose = pose->data; + s += "["; + for (int i = 0; i < hand_pose.size(); i++) { + s += std::to_string(hand_pose[i]); + s += ","; + } + s += "]"; + return s; +} + +void ROSbridgeListenerNode::hand_pose_callback( + const sample_msgs::msg::HandPose::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received Hand Data: %s", + hand_pose_to_string(msg).c_str()); +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/autonomy/controller/rosbridge_publisher/CMakeLists.txt b/autonomy/controller/rosbridge_publisher/CMakeLists.txt new file mode 100644 index 0000000..61a2d10 --- /dev/null +++ b/autonomy/controller/rosbridge_publisher/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.8) +project(rosbridge_publisher) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sample_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_library(rosbridge_publisher_lib src/rosbridge_publisher_core.cpp) + +target_include_directories(rosbridge_publisher_lib PUBLIC include) + +add_executable(rosbridge_publisher_node src/rosbridge_publisher_node.cpp) +ament_target_dependencies(rosbridge_publisher_lib rclcpp std_msgs sample_msgs) +target_link_libraries(rosbridge_publisher_node rosbridge_publisher_lib) +target_include_directories(rosbridge_publisher_node PUBLIC + $ + $) +target_compile_features(rosbridge_publisher_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS rosbridge_publisher_node + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/autonomy/controller/rosbridge_publisher/include/rosbridge_publisher_core.hpp b/autonomy/controller/rosbridge_publisher/include/rosbridge_publisher_core.hpp new file mode 100644 index 0000000..bde748f --- /dev/null +++ b/autonomy/controller/rosbridge_publisher/include/rosbridge_publisher_core.hpp @@ -0,0 +1,17 @@ +#ifndef ROSBRIDGE_PUBLISHER_CORE_HPP_ +#define ROSBRIDGE_PUBLISHER_CORE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "sample_msgs/msg/hand_pose.hpp" + +class ROSbridgePublisherCore { +public: + explicit ROSbridgePublisherCore(); + + std::string hand_pose_to_string(const sample_msgs::msg::HandPose pose); + +private: +}; + +#endif // AGGREGATOR_CORE_HPP_ diff --git a/autonomy/controller/rosbridge_publisher/include/rosbridge_publisher_node.hpp b/autonomy/controller/rosbridge_publisher/include/rosbridge_publisher_node.hpp new file mode 100644 index 0000000..e2d85ab --- /dev/null +++ b/autonomy/controller/rosbridge_publisher/include/rosbridge_publisher_node.hpp @@ -0,0 +1,28 @@ +#ifndef ROSBRIDGE_PUBLISHER_NODE_HPP_ +#define ROSBRIDGE_PUBLISHER_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "sample_msgs/msg/hand_pose.hpp" + +#include "rosbridge_publisher_core.hpp" + +/** + * Implementation of a ROS2 node that listens to the "unfiltered" and "filtered" + * topics and echoes the operating frequency of the topic to the console. + */ +class ROSbridgePublisherNode : public rclcpp::Node { +public: + ROSbridgePublisherNode(); + +private: + void timer_callback(); + + rclcpp::TimerBase::SharedPtr timer_; + + rclcpp::Publisher::SharedPtr hand_pose_pub_; + + ROSbridgePublisherCore rosbridge_publisher_; +}; + +#endif \ No newline at end of file diff --git a/autonomy/controller/rosbridge_publisher/launch/rosbridge_publisher.launch.py b/autonomy/controller/rosbridge_publisher/launch/rosbridge_publisher.launch.py new file mode 100755 index 0000000..88a546e --- /dev/null +++ b/autonomy/controller/rosbridge_publisher/launch/rosbridge_publisher.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + """Launch costmap node.""" + costmap_node = Node( + package='rosbridge_publisher', + executable='rosbridge_publisher', + ) + + return LaunchDescription([ + costmap_node + ]) diff --git a/autonomy/controller/rosbridge_publisher/package.xml b/autonomy/controller/rosbridge_publisher/package.xml new file mode 100644 index 0000000..1137e05 --- /dev/null +++ b/autonomy/controller/rosbridge_publisher/package.xml @@ -0,0 +1,22 @@ + + + + rosbridge_publisher + 0.0.0 + TODO: Package description + yichen + TODO: License declaration + + ament_cmake + + rclcpp + sample_msgs + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/autonomy/controller/rosbridge_publisher/src/rosbridge_publisher_core.cpp b/autonomy/controller/rosbridge_publisher/src/rosbridge_publisher_core.cpp new file mode 100644 index 0000000..649e6a6 --- /dev/null +++ b/autonomy/controller/rosbridge_publisher/src/rosbridge_publisher_core.cpp @@ -0,0 +1,19 @@ +#include +#include + +#include "rosbridge_publisher_core.hpp" + +ROSbridgePublisherCore::ROSbridgePublisherCore() {} + +std::string ROSbridgePublisherCore::hand_pose_to_string( + const sample_msgs::msg::HandPose pose) { + std::string s; + std::vector hand_pose = pose.data; + s += "["; + for (int i = 0; i < hand_pose.size(); i++) { + s += std::to_string(hand_pose[i]); + s += ","; + } + s += "]"; + return s; +} \ No newline at end of file diff --git a/autonomy/controller/rosbridge_publisher/src/rosbridge_publisher_node.cpp b/autonomy/controller/rosbridge_publisher/src/rosbridge_publisher_node.cpp new file mode 100644 index 0000000..0fe923a --- /dev/null +++ b/autonomy/controller/rosbridge_publisher/src/rosbridge_publisher_node.cpp @@ -0,0 +1,31 @@ +#include "rosbridge_publisher_node.hpp" + +#include +#include + +using namespace std::chrono_literals; + +ROSbridgePublisherNode::ROSbridgePublisherNode() + : Node("rosbridge_publisher"), + rosbridge_publisher_(ROSbridgePublisherCore()) { + + hand_pose_pub_ = + this->create_publisher("teleop", 10); + timer_ = this->create_wall_timer( + 500ms, std::bind(&ROSbridgePublisherNode::timer_callback, this)); +} + +void ROSbridgePublisherNode::timer_callback() { + auto message = sample_msgs::msg::HandPose(); + message.data = std::vector(17); + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", + rosbridge_publisher_.hand_pose_to_string(message).c_str()); + hand_pose_pub_->publish(message); +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/autonomy/controller/setup.cfg b/autonomy/controller/setup.cfg new file mode 100644 index 0000000..118c521 --- /dev/null +++ b/autonomy/controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/controller +[install] +install-scripts=$base/lib/controller diff --git a/autonomy/controller/setup.py b/autonomy/controller/setup.py new file mode 100755 index 0000000..07b0856 --- /dev/null +++ b/autonomy/controller/setup.py @@ -0,0 +1,34 @@ +import os +from glob import glob +from setuptools import setup + +package_name = 'controller' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + # Install marker file in the package index + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + # Include our package.xml file + (os.path.join('share', package_name), ['package.xml']), + # Include all launch files + (os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*.launch.py'))), + # Include config files for parameters + (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.yaml'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='eddyzhou, aryanafrouzi', + maintainer_email='miekale@watonomous.ca', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'controller_node = controller.controller_node:main' + ], + }, +) diff --git a/src/samples/README.md b/autonomy/samples/README.md similarity index 100% rename from src/samples/README.md rename to autonomy/samples/README.md diff --git a/src/samples/cpp/aggregator/CMakeLists.txt b/autonomy/samples/cpp/aggregator/CMakeLists.txt similarity index 100% rename from src/samples/cpp/aggregator/CMakeLists.txt rename to autonomy/samples/cpp/aggregator/CMakeLists.txt diff --git a/src/samples/cpp/aggregator/include/aggregator_core.hpp b/autonomy/samples/cpp/aggregator/include/aggregator_core.hpp similarity index 87% rename from src/samples/cpp/aggregator/include/aggregator_core.hpp rename to autonomy/samples/cpp/aggregator/include/aggregator_core.hpp index 7912f7d..9dcea65 100644 --- a/src/samples/cpp/aggregator/include/aggregator_core.hpp +++ b/autonomy/samples/cpp/aggregator/include/aggregator_core.hpp @@ -3,18 +3,16 @@ #include "rclcpp/rclcpp.hpp" -#include "sample_msgs/msg/unfiltered.hpp" #include "sample_msgs/msg/filtered_array.hpp" +#include "sample_msgs/msg/unfiltered.hpp" -namespace samples -{ +namespace samples { /** * Implementation of the internal logic used by the Aggregator Node to * calculate the operating frequency of topics. */ -class AggregatorCore -{ +class AggregatorCore { public: /** * Aggregator constructor. @@ -44,16 +42,14 @@ class AggregatorCore * * @param msg */ - void add_raw_msg( - const sample_msgs::msg::Unfiltered::SharedPtr msg); + void add_raw_msg(const sample_msgs::msg::Unfiltered::SharedPtr msg); /** * Used to keep track of latest timestamp and number of messages received * over the "filtered" topic. Should be called before filtered_frequency(). * * @param msg */ - void add_filtered_msg( - const sample_msgs::msg::FilteredArray::SharedPtr msg); + void add_filtered_msg(const sample_msgs::msg::FilteredArray::SharedPtr msg); private: // Number of message received on "unfiltered" and "filtered" topics. @@ -70,6 +66,6 @@ class AggregatorCore int64_t latest_filtered_time_; }; -} // namespace samples +} // namespace samples -#endif // AGGREGATOR_CORE_HPP_ +#endif // AGGREGATOR_CORE_HPP_ diff --git a/src/samples/cpp/aggregator/include/aggregator_node.hpp b/autonomy/samples/cpp/aggregator/include/aggregator_node.hpp similarity index 78% rename from src/samples/cpp/aggregator/include/aggregator_node.hpp rename to autonomy/samples/cpp/aggregator/include/aggregator_node.hpp index 0255fed..a273a2e 100644 --- a/src/samples/cpp/aggregator/include/aggregator_node.hpp +++ b/autonomy/samples/cpp/aggregator/include/aggregator_node.hpp @@ -3,8 +3,8 @@ #include "rclcpp/rclcpp.hpp" -#include "sample_msgs/msg/unfiltered.hpp" #include "sample_msgs/msg/filtered_array.hpp" +#include "sample_msgs/msg/unfiltered.hpp" #include "aggregator_core.hpp" @@ -12,8 +12,7 @@ * Implementation of a ROS2 node that listens to the "unfiltered" and "filtered" * topics and echoes the operating frequency of the topic to the console. */ -class AggregatorNode : public rclcpp::Node -{ +class AggregatorNode : public rclcpp::Node { public: // Configure pubsub nodes to keep last 20 messages. // https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html @@ -26,31 +25,32 @@ class AggregatorNode : public rclcpp::Node private: /** - * A ROS2 subscription node callback used to unpack raw data from the "unfiltered" - * topic and echo the operating frequency of the topic to the console. + * A ROS2 subscription node callback used to unpack raw data from the + * "unfiltered" topic and echo the operating frequency of the topic to the + * console. * * @param msg a raw message from the "unfiltered" topic */ - void unfiltered_callback( - const sample_msgs::msg::Unfiltered::SharedPtr msg); + void unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg); /** * A ROS2 subscription node callback used to unpack processed data from the - * "filtered" topic and echo the operating frequency of the topic to the console. + * "filtered" topic and echo the operating frequency of the topic to the + * console. * * @param msg a processed message from the "filtered" topic */ - void filtered_callback( - const sample_msgs::msg::FilteredArray::SharedPtr msg); + void filtered_callback(const sample_msgs::msg::FilteredArray::SharedPtr msg); // ROS2 subscriber listening to the unfiltered topic. rclcpp::Subscription::SharedPtr raw_sub_; // ROS2 subscriber listening to the filtered topic. - rclcpp::Subscription::SharedPtr filtered_sub_; + rclcpp::Subscription::SharedPtr + filtered_sub_; // Object containing methods to determine the operating frequency on topics. samples::AggregatorCore aggregator_; }; -#endif // AGGREGATOR_NODE_HPP_ +#endif // AGGREGATOR_NODE_HPP_ diff --git a/src/samples/cpp/aggregator/launch/aggregator.launch.py b/autonomy/samples/cpp/aggregator/launch/aggregator.launch.py similarity index 100% rename from src/samples/cpp/aggregator/launch/aggregator.launch.py rename to autonomy/samples/cpp/aggregator/launch/aggregator.launch.py diff --git a/src/samples/cpp/aggregator/package.xml b/autonomy/samples/cpp/aggregator/package.xml similarity index 100% rename from src/samples/cpp/aggregator/package.xml rename to autonomy/samples/cpp/aggregator/package.xml diff --git a/autonomy/samples/cpp/aggregator/src/aggregator_core.cpp b/autonomy/samples/cpp/aggregator/src/aggregator_core.cpp new file mode 100644 index 0000000..58f9e6a --- /dev/null +++ b/autonomy/samples/cpp/aggregator/src/aggregator_core.cpp @@ -0,0 +1,42 @@ +#include + +#include "aggregator_core.hpp" + +namespace samples { + +AggregatorCore::AggregatorCore(int64_t timestamp) + : raw_msg_count_(0), filtered_msg_count_(0), start_(timestamp), + latest_raw_time_(-1), latest_filtered_time_(-1) {} + +double AggregatorCore::raw_frequency() const { + if (latest_raw_time_ <= start_) { + return 0.0; + } + return static_cast(raw_msg_count_) / (latest_raw_time_ - start_); +} + +double AggregatorCore::filtered_frequency() const { + if (latest_filtered_time_ <= start_) { + return 0.0; + } + return static_cast(filtered_msg_count_) / + (latest_filtered_time_ - start_); +} + +void AggregatorCore::add_raw_msg( + const sample_msgs::msg::Unfiltered::SharedPtr msg) { + latest_raw_time_ = + std::max(static_cast(msg->timestamp), latest_raw_time_); + raw_msg_count_++; +} + +void AggregatorCore::add_filtered_msg( + const sample_msgs::msg::FilteredArray::SharedPtr msg) { + for (auto filtered_msg : msg->packets) { + latest_filtered_time_ = std::max( + static_cast(filtered_msg.timestamp), latest_filtered_time_); + } + filtered_msg_count_++; +} + +} // namespace samples diff --git a/autonomy/samples/cpp/aggregator/src/aggregator_node.cpp b/autonomy/samples/cpp/aggregator/src/aggregator_node.cpp new file mode 100644 index 0000000..8bf5b87 --- /dev/null +++ b/autonomy/samples/cpp/aggregator/src/aggregator_node.cpp @@ -0,0 +1,41 @@ +#include +#include + +#include "aggregator_node.hpp" + +AggregatorNode::AggregatorNode() + : Node("aggregator"), + aggregator_(samples::AggregatorCore( + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count())) { + raw_sub_ = this->create_subscription( + "/unfiltered_topic", ADVERTISING_FREQ, + std::bind(&AggregatorNode::unfiltered_callback, this, + std::placeholders::_1)); + filtered_sub_ = this->create_subscription( + "/filtered_topic", ADVERTISING_FREQ, + std::bind(&AggregatorNode::filtered_callback, this, + std::placeholders::_1)); +} + +void AggregatorNode::unfiltered_callback( + const sample_msgs::msg::Unfiltered::SharedPtr msg) { + aggregator_.add_raw_msg(msg); + RCLCPP_INFO(this->get_logger(), "Raw Frequency(msg/s): %f", + aggregator_.raw_frequency() * 1000); +} + +void AggregatorNode::filtered_callback( + const sample_msgs::msg::FilteredArray::SharedPtr msg) { + aggregator_.add_filtered_msg(msg); + RCLCPP_INFO(this->get_logger(), "Filtered Frequency(msg/s): %f", + aggregator_.filtered_frequency() * 1000); +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/samples/cpp/aggregator/test/aggregator_test.cpp b/autonomy/samples/cpp/aggregator/test/aggregator_test.cpp similarity index 88% rename from src/samples/cpp/aggregator/test/aggregator_test.cpp rename to autonomy/samples/cpp/aggregator/test/aggregator_test.cpp index a7add4b..3e5c2db 100644 --- a/src/samples/cpp/aggregator/test/aggregator_test.cpp +++ b/autonomy/samples/cpp/aggregator/test/aggregator_test.cpp @@ -1,13 +1,12 @@ #include #include -#include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" +#include "gtest/gtest.h" #include "aggregator_core.hpp" -TEST(AggregatorTest, RawDivisionByZero) -{ +TEST(AggregatorTest, RawDivisionByZero) { samples::AggregatorCore aggregator(0); auto msg = std::make_shared(); msg->timestamp = 0; @@ -16,8 +15,7 @@ TEST(AggregatorTest, RawDivisionByZero) EXPECT_DOUBLE_EQ(0.0, aggregator.raw_frequency()); } -TEST(AggregatorTest, FilteredDivisionByZero) -{ +TEST(AggregatorTest, FilteredDivisionByZero) { samples::AggregatorCore aggregator(1); auto filtered_packet = std::make_shared(); std::vector msgs; @@ -30,8 +28,7 @@ TEST(AggregatorTest, FilteredDivisionByZero) EXPECT_DOUBLE_EQ(0.0, aggregator.filtered_frequency()); } -TEST(AggregatorTest, RawFrequencyAddSingleMessage) -{ +TEST(AggregatorTest, RawFrequencyAddSingleMessage) { samples::AggregatorCore aggregator(0); auto msg = std::make_shared(); @@ -40,8 +37,7 @@ TEST(AggregatorTest, RawFrequencyAddSingleMessage) EXPECT_DOUBLE_EQ(0.5, aggregator.raw_frequency()); } -TEST(AggregatorTest, RawFrequencyAddMultipleMessages) -{ +TEST(AggregatorTest, RawFrequencyAddMultipleMessages) { samples::AggregatorCore aggregator(0); auto msg = std::make_shared(); @@ -58,8 +54,7 @@ TEST(AggregatorTest, RawFrequencyAddMultipleMessages) EXPECT_DOUBLE_EQ(0.6, aggregator.raw_frequency()); } -TEST(AggregatorTest, FilteredUnorderedTimestamps) -{ +TEST(AggregatorTest, FilteredUnorderedTimestamps) { samples::AggregatorCore aggregator(0); auto filtered_packet = std::make_shared(); std::vector msgs; diff --git a/src/samples/cpp/producer/CMakeLists.txt b/autonomy/samples/cpp/producer/CMakeLists.txt similarity index 100% rename from src/samples/cpp/producer/CMakeLists.txt rename to autonomy/samples/cpp/producer/CMakeLists.txt diff --git a/src/samples/cpp/producer/config/params.yaml b/autonomy/samples/cpp/producer/config/params.yaml similarity index 100% rename from src/samples/cpp/producer/config/params.yaml rename to autonomy/samples/cpp/producer/config/params.yaml diff --git a/src/samples/cpp/producer/include/producer_core.hpp b/autonomy/samples/cpp/producer/include/producer_core.hpp similarity index 90% rename from src/samples/cpp/producer/include/producer_core.hpp rename to autonomy/samples/cpp/producer/include/producer_core.hpp index 8a12649..806f08a 100644 --- a/src/samples/cpp/producer/include/producer_core.hpp +++ b/autonomy/samples/cpp/producer/include/producer_core.hpp @@ -3,15 +3,13 @@ #include "sample_msgs/msg/unfiltered.hpp" -namespace samples -{ +namespace samples { /** * Implementation of the internal logic used by the Producer Node to * serialize and update coordinates. */ -class ProducerCore -{ +class ProducerCore { public: /** * Producer constructor. @@ -51,7 +49,7 @@ class ProducerCore * * @param[out] msg an unfiltered message with empty data field */ - void serialize_coordinates(sample_msgs::msg::Unfiltered & msg) const; + void serialize_coordinates(sample_msgs::msg::Unfiltered &msg) const; private: // Coordinate values @@ -63,6 +61,6 @@ class ProducerCore double velocity_; }; -} // namespace samples +} // namespace samples -#endif // PRODUCER_CORE_HPP_ +#endif // PRODUCER_CORE_HPP_ diff --git a/src/samples/cpp/producer/include/producer_node.hpp b/autonomy/samples/cpp/producer/include/producer_node.hpp similarity index 86% rename from src/samples/cpp/producer/include/producer_node.hpp rename to autonomy/samples/cpp/producer/include/producer_node.hpp index a52ab95..7ad7807 100644 --- a/src/samples/cpp/producer/include/producer_node.hpp +++ b/autonomy/samples/cpp/producer/include/producer_node.hpp @@ -3,8 +3,8 @@ #include -#include "rclcpp/rclcpp.hpp" #include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "rclcpp/rclcpp.hpp" #include "sample_msgs/msg/unfiltered.hpp" @@ -14,8 +14,7 @@ * Implementation of a ROS2 node that generates unfiltered ROS2 messages on a * time interval. */ -class ProducerNode : public rclcpp::Node -{ +class ProducerNode : public rclcpp::Node { public: // Configure pubsub nodes to keep last 20 messages. // https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html @@ -38,11 +37,12 @@ class ProducerNode : public rclcpp::Node /** * Callback used to dynamically update velocity data at runtime. * - * @param parameters list of parameters (only velocity in this case) that were modified + * @param parameters list of parameters (only velocity in this case) that were + * modified * @returns status message indicating whether update was successful */ - rcl_interfaces::msg::SetParametersResult parameters_callback( - const std::vector & parameters); + rcl_interfaces::msg::SetParametersResult + parameters_callback(const std::vector ¶meters); // ROS2 publisher sending raw messages to the unfiltered topic. rclcpp::Publisher::SharedPtr data_pub_; @@ -53,8 +53,9 @@ class ProducerNode : public rclcpp::Node // Callback to dynamically modify node parameters. rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_; - // Producer implementation containing logic for coordinate serialization and management. + // Producer implementation containing logic for coordinate serialization and + // management. samples::ProducerCore producer_; }; -#endif // PRODUCER_NODE_HPP_ +#endif // PRODUCER_NODE_HPP_ diff --git a/src/samples/cpp/producer/launch/producer.launch.py b/autonomy/samples/cpp/producer/launch/producer.launch.py similarity index 100% rename from src/samples/cpp/producer/launch/producer.launch.py rename to autonomy/samples/cpp/producer/launch/producer.launch.py diff --git a/src/samples/cpp/producer/package.xml b/autonomy/samples/cpp/producer/package.xml similarity index 100% rename from src/samples/cpp/producer/package.xml rename to autonomy/samples/cpp/producer/package.xml diff --git a/src/samples/cpp/producer/src/producer_core.cpp b/autonomy/samples/cpp/producer/src/producer_core.cpp similarity index 54% rename from src/samples/cpp/producer/src/producer_core.cpp rename to autonomy/samples/cpp/producer/src/producer_core.cpp index ffa27d7..0b0a1db 100644 --- a/src/samples/cpp/producer/src/producer_core.cpp +++ b/autonomy/samples/cpp/producer/src/producer_core.cpp @@ -3,38 +3,30 @@ #include "producer_core.hpp" -namespace samples -{ +namespace samples { ProducerCore::ProducerCore(float x, float y, float z) -: pos_x_(x), pos_y_(y), pos_z_(z), velocity_(0) -{ -} + : pos_x_(x), pos_y_(y), pos_z_(z), velocity_(0) {} -void ProducerCore::update_velocity(int velocity) -{ - velocity_ = velocity; -} +void ProducerCore::update_velocity(int velocity) { velocity_ = velocity; } -void ProducerCore::update_position(double pos_x, double pos_y, double pos_z) -{ +void ProducerCore::update_position(double pos_x, double pos_y, double pos_z) { pos_x_ = pos_x; pos_y_ = pos_y; pos_z_ = pos_z; } -void ProducerCore::update_coordinates() -{ +void ProducerCore::update_coordinates() { pos_x_ += velocity_ / sqrt(3); pos_y_ += velocity_ / sqrt(3); pos_z_ += velocity_ / sqrt(3); } -void ProducerCore::serialize_coordinates(sample_msgs::msg::Unfiltered & msg) const -{ +void ProducerCore::serialize_coordinates( + sample_msgs::msg::Unfiltered &msg) const { msg.data = "x:" + std::to_string(pos_x_) + ";y:" + std::to_string(pos_y_) + - ";z:" + std::to_string(pos_z_) + ";"; + ";z:" + std::to_string(pos_z_) + ";"; msg.valid = true; } -} // namespace samples +} // namespace samples diff --git a/src/samples/cpp/producer/src/producer_node.cpp b/autonomy/samples/cpp/producer/src/producer_node.cpp similarity index 62% rename from src/samples/cpp/producer/src/producer_node.cpp rename to autonomy/samples/cpp/producer/src/producer_node.cpp index b313eca..e0a46c3 100644 --- a/src/samples/cpp/producer/src/producer_node.cpp +++ b/autonomy/samples/cpp/producer/src/producer_node.cpp @@ -5,14 +5,13 @@ #include "producer_node.hpp" ProducerNode::ProducerNode(int delay_ms) -: Node("producer"), producer_(samples::ProducerCore()) -{ - data_pub_ = - this->create_publisher("/unfiltered_topic", ADVERTISING_FREQ); + : Node("producer"), producer_(samples::ProducerCore()) { + data_pub_ = this->create_publisher( + "/unfiltered_topic", ADVERTISING_FREQ); - timer_ = this->create_wall_timer( - std::chrono::milliseconds(delay_ms), - std::bind(&ProducerNode::timer_callback, this)); + timer_ = + this->create_wall_timer(std::chrono::milliseconds(delay_ms), + std::bind(&ProducerNode::timer_callback, this)); // Define the default values for parameters if not defined in params.yaml this->declare_parameter("pos_x", 0.0); @@ -25,20 +24,21 @@ ProducerNode::ProducerNode(int delay_ms) rclcpp::Parameter pos_z = this->get_parameter("pos_z"); rclcpp::Parameter velocity = this->get_parameter("velocity"); - producer_.update_position(pos_x.as_double(), pos_y.as_double(), pos_z.as_double()); + producer_.update_position(pos_x.as_double(), pos_y.as_double(), + pos_z.as_double()); producer_.update_velocity(velocity.as_double()); - param_cb_ = this->add_on_set_parameters_callback( - std::bind(&ProducerNode::parameters_callback, this, std::placeholders::_1)); + param_cb_ = this->add_on_set_parameters_callback(std::bind( + &ProducerNode::parameters_callback, this, std::placeholders::_1)); } -void ProducerNode::timer_callback() -{ +void ProducerNode::timer_callback() { producer_.update_coordinates(); auto msg = sample_msgs::msg::Unfiltered(); msg.timestamp = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()).count(); + std::chrono::system_clock::now().time_since_epoch()) + .count(); producer_.serialize_coordinates(msg); RCLCPP_INFO(this->get_logger(), "Publishing: %s", msg.data.c_str()); @@ -46,26 +46,24 @@ void ProducerNode::timer_callback() } rcl_interfaces::msg::SetParametersResult ProducerNode::parameters_callback( - const std::vector & parameters) -{ + const std::vector ¶meters) { rcl_interfaces::msg::SetParametersResult result; result.successful = false; result.reason = ""; - for (const auto & parameter : parameters) { + for (const auto ¶meter : parameters) { if (parameter.get_name() == "velocity" && - parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) - { + parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) { producer_.update_velocity(parameter.as_int()); - RCLCPP_INFO(this->get_logger(), "Velocity successfully set to %d", parameter.as_int()); + RCLCPP_INFO(this->get_logger(), "Velocity successfully set to %d", + parameter.as_int()); result.successful = true; } } return result; } -int main(int argc, char ** argv) -{ +int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared(500)); rclcpp::shutdown(); diff --git a/src/samples/cpp/producer/test/producer_test.cpp b/autonomy/samples/cpp/producer/test/producer_test.cpp similarity index 87% rename from src/samples/cpp/producer/test/producer_test.cpp rename to autonomy/samples/cpp/producer/test/producer_test.cpp index fd59440..6dac461 100644 --- a/src/samples/cpp/producer/test/producer_test.cpp +++ b/autonomy/samples/cpp/producer/test/producer_test.cpp @@ -2,8 +2,7 @@ #include "producer_core.hpp" -TEST(ProducerTest, ValidSerialization) -{ +TEST(ProducerTest, ValidSerialization) { auto producer = samples::ProducerCore(11, -12, 0); auto msg = sample_msgs::msg::Unfiltered(); producer.serialize_coordinates(msg); @@ -12,8 +11,7 @@ TEST(ProducerTest, ValidSerialization) EXPECT_EQ(msg.data, "x:11.000000;y:-12.000000;z:0.000000;"); } -TEST(ProducerTest, NoCoordinateUpdate) -{ +TEST(ProducerTest, NoCoordinateUpdate) { auto producer = samples::ProducerCore(); producer.update_coordinates(); @@ -22,8 +20,7 @@ TEST(ProducerTest, NoCoordinateUpdate) EXPECT_EQ(msg.data, "x:0.000000;y:0.000000;z:0.000000;"); } -TEST(ProducerTest, MultipleCoordinateUpdate) -{ +TEST(ProducerTest, MultipleCoordinateUpdate) { auto producer = samples::ProducerCore(); auto msg = sample_msgs::msg::Unfiltered(); diff --git a/src/samples/cpp/transformer/CMakeLists.txt b/autonomy/samples/cpp/transformer/CMakeLists.txt similarity index 100% rename from src/samples/cpp/transformer/CMakeLists.txt rename to autonomy/samples/cpp/transformer/CMakeLists.txt diff --git a/src/samples/cpp/transformer/config/params.yaml b/autonomy/samples/cpp/transformer/config/params.yaml similarity index 100% rename from src/samples/cpp/transformer/config/params.yaml rename to autonomy/samples/cpp/transformer/config/params.yaml diff --git a/src/samples/cpp/transformer/include/transformer_core.hpp b/autonomy/samples/cpp/transformer/include/transformer_core.hpp similarity index 83% rename from src/samples/cpp/transformer/include/transformer_core.hpp rename to autonomy/samples/cpp/transformer/include/transformer_core.hpp index 8bb2d38..1c97418 100644 --- a/src/samples/cpp/transformer/include/transformer_core.hpp +++ b/autonomy/samples/cpp/transformer/include/transformer_core.hpp @@ -3,18 +3,16 @@ #include -#include "sample_msgs/msg/unfiltered.hpp" #include "sample_msgs/msg/filtered.hpp" +#include "sample_msgs/msg/unfiltered.hpp" -namespace samples -{ +namespace samples { /** * Implementation for the internal logic for the Transformer ROS2 * node performing data processing and validation. */ -class TransformerCore -{ +class TransformerCore { public: // Size of buffer before processed messages are published. static constexpr int BUFFER_CAPACITY = 10; @@ -45,8 +43,8 @@ class TransformerCore * @param unfiltered a raw message * @returns whether message's 'valid' field is set */ - bool validate_message( - const sample_msgs::msg::Unfiltered::SharedPtr unfiltered); + bool + validate_message(const sample_msgs::msg::Unfiltered::SharedPtr unfiltered); /** * Enqueue message into an array of processed messages to "filtered" topic. @@ -55,7 +53,7 @@ class TransformerCore * @param msg a processed message to be published * @returns whether buffer is full after adding new message */ - bool enqueue_message(const sample_msgs::msg::Filtered & msg); + bool enqueue_message(const sample_msgs::msg::Filtered &msg); /** * Deserializes the data field of the unfiltered ROS2 message. @@ -66,8 +64,8 @@ class TransformerCore * @returns whether deserialization was successful */ bool deserialize_coordinate( - const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, - sample_msgs::msg::Filtered & filtered); + const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, + sample_msgs::msg::Filtered &filtered); private: // Buffer storing processed messages until BUFFER_CAPACITY. Clear after @@ -75,6 +73,6 @@ class TransformerCore std::vector buffer_; }; -} // namespace samples +} // namespace samples -#endif // TRANSFORMER_CORE_HPP_ +#endif // TRANSFORMER_CORE_HPP_ diff --git a/src/samples/cpp/transformer/include/transformer_node.hpp b/autonomy/samples/cpp/transformer/include/transformer_node.hpp similarity index 86% rename from src/samples/cpp/transformer/include/transformer_node.hpp rename to autonomy/samples/cpp/transformer/include/transformer_node.hpp index ffda282..2af4766 100644 --- a/src/samples/cpp/transformer/include/transformer_node.hpp +++ b/autonomy/samples/cpp/transformer/include/transformer_node.hpp @@ -10,15 +10,15 @@ #include "transformer_core.hpp" /** - * Implementation of a ROS2 node that converts unfiltered messages to filtered_array - * messages. + * Implementation of a ROS2 node that converts unfiltered messages to + * filtered_array messages. * * Listens to the "unfiltered" topic and filters out data with invalid fields * and odd timestamps. Once the node collects BUFFER_CAPACITY messages it packs - * the processed messages into an array and publishes it to the "filtered" topic. + * the processed messages into an array and publishes it to the "filtered" + * topic. */ -class TransformerNode : public rclcpp::Node -{ +class TransformerNode : public rclcpp::Node { public: // Configure pubsub nodes to keep last 20 messages. // https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html @@ -36,8 +36,7 @@ class TransformerNode : public rclcpp::Node * * @param msg a raw message from the "unfiltered" topic */ - void unfiltered_callback( - const sample_msgs::msg::Unfiltered::SharedPtr msg); + void unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg); // ROS2 subscriber listening to the unfiltered topic. rclcpp::Subscription::SharedPtr raw_sub_; @@ -49,4 +48,4 @@ class TransformerNode : public rclcpp::Node samples::TransformerCore transformer_; }; -#endif // TRANSFORMER_NODE_HPP_ +#endif // TRANSFORMER_NODE_HPP_ diff --git a/src/samples/cpp/transformer/launch/transformer.launch.py b/autonomy/samples/cpp/transformer/launch/transformer.launch.py similarity index 100% rename from src/samples/cpp/transformer/launch/transformer.launch.py rename to autonomy/samples/cpp/transformer/launch/transformer.launch.py diff --git a/src/samples/cpp/transformer/package.xml b/autonomy/samples/cpp/transformer/package.xml similarity index 100% rename from src/samples/cpp/transformer/package.xml rename to autonomy/samples/cpp/transformer/package.xml diff --git a/src/samples/cpp/transformer/src/transformer_core.cpp b/autonomy/samples/cpp/transformer/src/transformer_core.cpp similarity index 64% rename from src/samples/cpp/transformer/src/transformer_core.cpp rename to autonomy/samples/cpp/transformer/src/transformer_core.cpp index e62d3f9..a0a8f58 100644 --- a/src/samples/cpp/transformer/src/transformer_core.cpp +++ b/autonomy/samples/cpp/transformer/src/transformer_core.cpp @@ -3,30 +3,23 @@ #include "transformer_core.hpp" -namespace samples -{ +namespace samples { -TransformerCore::TransformerCore() -{} +TransformerCore::TransformerCore() {} -std::vector TransformerCore::buffer_messages() const -{ +std::vector +TransformerCore::buffer_messages() const { return buffer_; } -void TransformerCore::clear_buffer() -{ - buffer_.clear(); -} +void TransformerCore::clear_buffer() { buffer_.clear(); } bool TransformerCore::validate_message( - const sample_msgs::msg::Unfiltered::SharedPtr unfiltered) -{ + const sample_msgs::msg::Unfiltered::SharedPtr unfiltered) { return unfiltered->valid; } -bool TransformerCore::enqueue_message(const sample_msgs::msg::Filtered & msg) -{ +bool TransformerCore::enqueue_message(const sample_msgs::msg::Filtered &msg) { if (buffer_.size() < BUFFER_CAPACITY) { buffer_.push_back(msg); } @@ -34,44 +27,43 @@ bool TransformerCore::enqueue_message(const sample_msgs::msg::Filtered & msg) } bool TransformerCore::deserialize_coordinate( - const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, - sample_msgs::msg::Filtered & filtered) -{ + const sample_msgs::msg::Unfiltered::SharedPtr unfiltered, + sample_msgs::msg::Filtered &filtered) { std::string serialized_position = unfiltered->data; auto start_pos = serialized_position.find("x:"); auto end_pos = serialized_position.find(";"); // Validate that the substrings were found if (start_pos == std::string::npos || end_pos == std::string::npos || - end_pos < start_pos) - { + end_pos < start_pos) { return false; } // Offset index to start of x_pos start_pos += 2; // Splice string and convert position to float - float x = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); + float x = + std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); start_pos = serialized_position.find("y:", end_pos + 1); end_pos = serialized_position.find(";", end_pos + 1); if (start_pos == std::string::npos || end_pos == std::string::npos || - end_pos < start_pos) - { + end_pos < start_pos) { return false; } // Offset index to start of y_pos start_pos += 2; - float y = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); + float y = + std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); start_pos = serialized_position.find("z:", end_pos + 1); end_pos = serialized_position.find(";", end_pos + 1); if (start_pos == std::string::npos || end_pos == std::string::npos || - end_pos < start_pos) - { + end_pos < start_pos) { return false; } // Offset index to start of z_pos start_pos += 2; - float z = std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); + float z = + std::stof(serialized_position.substr(start_pos, end_pos - start_pos)); filtered.pos_x = x; filtered.pos_y = y; @@ -79,4 +71,4 @@ bool TransformerCore::deserialize_coordinate( return true; } -} // namespace samples +} // namespace samples diff --git a/src/samples/cpp/transformer/src/transformer_node.cpp b/autonomy/samples/cpp/transformer/src/transformer_node.cpp similarity index 69% rename from src/samples/cpp/transformer/src/transformer_node.cpp rename to autonomy/samples/cpp/transformer/src/transformer_node.cpp index 95cada4..f67b02d 100644 --- a/src/samples/cpp/transformer/src/transformer_node.cpp +++ b/autonomy/samples/cpp/transformer/src/transformer_node.cpp @@ -3,23 +3,21 @@ #include "transformer_node.hpp" TransformerNode::TransformerNode() -: Node("transformer"), transformer_(samples::TransformerCore()) -{ + : Node("transformer"), transformer_(samples::TransformerCore()) { raw_sub_ = this->create_subscription( - "/unfiltered_topic", ADVERTISING_FREQ, - std::bind( - &TransformerNode::unfiltered_callback, this, - std::placeholders::_1)); - transform_pub_ = - this->create_publisher("/filtered_topic", ADVERTISING_FREQ); + "/unfiltered_topic", ADVERTISING_FREQ, + std::bind(&TransformerNode::unfiltered_callback, this, + std::placeholders::_1)); + transform_pub_ = this->create_publisher( + "/filtered_topic", ADVERTISING_FREQ); // Define the default values for parameters if not defined in params.yaml this->declare_parameter("version", rclcpp::ParameterValue(0)); this->declare_parameter("compression_method", rclcpp::ParameterValue(0)); } -void TransformerNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg) -{ +void TransformerNode::unfiltered_callback( + const sample_msgs::msg::Unfiltered::SharedPtr msg) { if (!transformer_.validate_message(msg)) { return; } @@ -32,7 +30,7 @@ void TransformerNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::Sh filtered.timestamp = msg->timestamp; filtered.metadata.version = this->get_parameter("version").as_int(); filtered.metadata.compression_method = - this->get_parameter("compression_method").as_int(); + this->get_parameter("compression_method").as_int(); if (transformer_.enqueue_message(filtered)) { RCLCPP_INFO(this->get_logger(), "Buffer Capacity Reached. PUBLISHING..."); @@ -42,15 +40,14 @@ void TransformerNode::unfiltered_callback(const sample_msgs::msg::Unfiltered::Sh transformer_.clear_buffer(); // Construct FilteredArray object - for (auto & packet : buffer) { + for (auto &packet : buffer) { filtered_msgs.packets.push_back(packet); } transform_pub_->publish(filtered_msgs); } } -int main(int argc, char ** argv) -{ +int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/src/samples/cpp/transformer/test/transformer_test.cpp b/autonomy/samples/cpp/transformer/test/transformer_test.cpp similarity index 68% rename from src/samples/cpp/transformer/test/transformer_test.cpp rename to autonomy/samples/cpp/transformer/test/transformer_test.cpp index 48fe847..fde4855 100644 --- a/src/samples/cpp/transformer/test/transformer_test.cpp +++ b/autonomy/samples/cpp/transformer/test/transformer_test.cpp @@ -2,22 +2,21 @@ #include #include -#include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" +#include "gtest/gtest.h" #include "transformer_core.hpp" /** * When writing a large number of tests it is desirable to wrap any common - * setup or cleanup logic in a test fixture. This improves readability and reduces - * the amount of boilerplate code in each test. For more information checkout + * setup or cleanup logic in a test fixture. This improves readability and + * reduces the amount of boilerplate code in each test. For more information + * checkout * https://google.github.io/googletest/primer.html#same-data-multiple-tests */ -class TransformerFixtureTest : public ::testing::Test -{ +class TransformerFixtureTest : public ::testing::Test { public: - void SetUp(int enqueue_count) - { + void SetUp(int enqueue_count) { auto filtered = sample_msgs::msg::Filtered(); for (int i = 0; i < enqueue_count; i++) { transformer.enqueue_message(filtered); @@ -29,19 +28,18 @@ class TransformerFixtureTest : public ::testing::Test }; /** - * Parameterized tests let us test the same logic with different parameters without - * having to write boilerplate for multiple tests. For more information checkout + * Parameterized tests let us test the same logic with different parameters + * without having to write boilerplate for multiple tests. For more information + * checkout * https://google.github.io/googletest/advanced.html#value-parameterized-tests */ class TransformerParameterizedTest - : public ::testing::TestWithParam> -{ + : public ::testing::TestWithParam> { protected: samples::TransformerCore transformer; }; -TEST(TransformerTest, FilterInvalidField) -{ +TEST(TransformerTest, FilterInvalidField) { auto unfiltered = std::make_shared(); unfiltered->valid = false; @@ -50,8 +48,7 @@ TEST(TransformerTest, FilterInvalidField) EXPECT_FALSE(valid); } -TEST_F(TransformerFixtureTest, BufferCapacity) -{ +TEST_F(TransformerFixtureTest, BufferCapacity) { SetUp(samples::TransformerCore::BUFFER_CAPACITY - 1); // Place last message that fits in buffer @@ -68,8 +65,7 @@ TEST_F(TransformerFixtureTest, BufferCapacity) EXPECT_EQ(size, 10); } -TEST_F(TransformerFixtureTest, ClearBuffer) -{ +TEST_F(TransformerFixtureTest, ClearBuffer) { // Place 3 messages in buffer SetUp(3); @@ -81,8 +77,7 @@ TEST_F(TransformerFixtureTest, ClearBuffer) EXPECT_EQ(size, 0); } -TEST_P(TransformerParameterizedTest, SerializationValidation) -{ +TEST_P(TransformerParameterizedTest, SerializationValidation) { auto [serialized_field, valid] = GetParam(); auto filtered = sample_msgs::msg::Filtered(); auto unfiltered = std::make_shared(); @@ -93,11 +88,10 @@ TEST_P(TransformerParameterizedTest, SerializationValidation) // Parameterized testing lets us validate all edge cases for serialization // using one test case. INSTANTIATE_TEST_CASE_P( - Serialization, TransformerParameterizedTest, - ::testing::Values( - std::make_tuple("x:1;y:2;z:3", false), - std::make_tuple("z:1;y:2;x:3;", false), - std::make_tuple("x:1,y:2,z:3", false), - std::make_tuple("x:3;", false), - std::make_tuple("x:3;y:2;z:3;", true), - std::make_tuple("x:3;y:22; z:11;", true))); + Serialization, TransformerParameterizedTest, + ::testing::Values(std::make_tuple("x:1;y:2;z:3", false), + std::make_tuple("z:1;y:2;x:3;", false), + std::make_tuple("x:1,y:2,z:3", false), + std::make_tuple("x:3;", false), + std::make_tuple("x:3;y:2;z:3;", true), + std::make_tuple("x:3;y:22; z:11;", true))); diff --git a/src/samples/python/producer/producer/__init__.py b/autonomy/samples/python/aggregator/aggregator/__init__.py similarity index 100% rename from src/samples/python/producer/producer/__init__.py rename to autonomy/samples/python/aggregator/aggregator/__init__.py diff --git a/src/samples/python/aggregator/aggregator/aggregator_core.py b/autonomy/samples/python/aggregator/aggregator/aggregator_core.py similarity index 100% rename from src/samples/python/aggregator/aggregator/aggregator_core.py rename to autonomy/samples/python/aggregator/aggregator/aggregator_core.py diff --git a/src/samples/python/aggregator/aggregator/aggregator_node.py b/autonomy/samples/python/aggregator/aggregator/aggregator_node.py similarity index 100% rename from src/samples/python/aggregator/aggregator/aggregator_node.py rename to autonomy/samples/python/aggregator/aggregator/aggregator_node.py diff --git a/src/samples/python/aggregator/launch/aggregator.launch.py b/autonomy/samples/python/aggregator/launch/aggregator.launch.py similarity index 100% rename from src/samples/python/aggregator/launch/aggregator.launch.py rename to autonomy/samples/python/aggregator/launch/aggregator.launch.py diff --git a/src/samples/python/aggregator/package.xml b/autonomy/samples/python/aggregator/package.xml similarity index 100% rename from src/samples/python/aggregator/package.xml rename to autonomy/samples/python/aggregator/package.xml diff --git a/src/samples/python/producer/resource/producer b/autonomy/samples/python/aggregator/resource/aggregator similarity index 100% rename from src/samples/python/producer/resource/producer rename to autonomy/samples/python/aggregator/resource/aggregator diff --git a/src/samples/python/aggregator/setup.cfg b/autonomy/samples/python/aggregator/setup.cfg similarity index 100% rename from src/samples/python/aggregator/setup.cfg rename to autonomy/samples/python/aggregator/setup.cfg diff --git a/src/samples/python/aggregator/setup.py b/autonomy/samples/python/aggregator/setup.py similarity index 94% rename from src/samples/python/aggregator/setup.py rename to autonomy/samples/python/aggregator/setup.py index b0afb9f..f77c180 100755 --- a/src/samples/python/aggregator/setup.py +++ b/autonomy/samples/python/aggregator/setup.py @@ -14,7 +14,7 @@ # Include our package.xml file (os.path.join('share', package_name), ['package.xml']), # Include all launch files. - (os.path.join('share', package_name, 'launch'), \ + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))), ], install_requires=['setuptools'], diff --git a/src/samples/python/aggregator/test/test_aggregator.py b/autonomy/samples/python/aggregator/test/test_aggregator.py similarity index 100% rename from src/samples/python/aggregator/test/test_aggregator.py rename to autonomy/samples/python/aggregator/test/test_aggregator.py diff --git a/src/samples/python/aggregator/test/test_copyright.py b/autonomy/samples/python/aggregator/test/test_copyright.py similarity index 100% rename from src/samples/python/aggregator/test/test_copyright.py rename to autonomy/samples/python/aggregator/test/test_copyright.py diff --git a/src/samples/python/aggregator/test/test_flake8.py b/autonomy/samples/python/aggregator/test/test_flake8.py similarity index 100% rename from src/samples/python/aggregator/test/test_flake8.py rename to autonomy/samples/python/aggregator/test/test_flake8.py diff --git a/src/samples/python/aggregator/test/test_pep257.py b/autonomy/samples/python/aggregator/test/test_pep257.py similarity index 100% rename from src/samples/python/aggregator/test/test_pep257.py rename to autonomy/samples/python/aggregator/test/test_pep257.py diff --git a/src/samples/python/producer/config/params.yaml b/autonomy/samples/python/producer/config/params.yaml similarity index 100% rename from src/samples/python/producer/config/params.yaml rename to autonomy/samples/python/producer/config/params.yaml diff --git a/src/samples/python/producer/launch/producer.launch.py b/autonomy/samples/python/producer/launch/producer.launch.py similarity index 100% rename from src/samples/python/producer/launch/producer.launch.py rename to autonomy/samples/python/producer/launch/producer.launch.py diff --git a/src/samples/python/producer/package.xml b/autonomy/samples/python/producer/package.xml similarity index 100% rename from src/samples/python/producer/package.xml rename to autonomy/samples/python/producer/package.xml diff --git a/src/samples/python/transformer/transformer/__init__.py b/autonomy/samples/python/producer/producer/__init__.py similarity index 100% rename from src/samples/python/transformer/transformer/__init__.py rename to autonomy/samples/python/producer/producer/__init__.py diff --git a/src/samples/python/producer/producer/producer_core.py b/autonomy/samples/python/producer/producer/producer_core.py similarity index 100% rename from src/samples/python/producer/producer/producer_core.py rename to autonomy/samples/python/producer/producer/producer_core.py diff --git a/src/samples/python/producer/producer/producer_node.py b/autonomy/samples/python/producer/producer/producer_node.py similarity index 100% rename from src/samples/python/producer/producer/producer_node.py rename to autonomy/samples/python/producer/producer/producer_node.py diff --git a/src/samples/python/transformer/resource/transformer b/autonomy/samples/python/producer/resource/producer similarity index 100% rename from src/samples/python/transformer/resource/transformer rename to autonomy/samples/python/producer/resource/producer diff --git a/src/samples/python/producer/setup.cfg b/autonomy/samples/python/producer/setup.cfg similarity index 100% rename from src/samples/python/producer/setup.cfg rename to autonomy/samples/python/producer/setup.cfg diff --git a/src/samples/python/producer/setup.py b/autonomy/samples/python/producer/setup.py similarity index 100% rename from src/samples/python/producer/setup.py rename to autonomy/samples/python/producer/setup.py diff --git a/src/samples/python/producer/test/test_copyright.py b/autonomy/samples/python/producer/test/test_copyright.py similarity index 100% rename from src/samples/python/producer/test/test_copyright.py rename to autonomy/samples/python/producer/test/test_copyright.py diff --git a/src/samples/python/producer/test/test_flake8.py b/autonomy/samples/python/producer/test/test_flake8.py similarity index 100% rename from src/samples/python/producer/test/test_flake8.py rename to autonomy/samples/python/producer/test/test_flake8.py diff --git a/src/samples/python/producer/test/test_pep257.py b/autonomy/samples/python/producer/test/test_pep257.py similarity index 100% rename from src/samples/python/producer/test/test_pep257.py rename to autonomy/samples/python/producer/test/test_pep257.py diff --git a/src/samples/python/producer/test/test_producer.py b/autonomy/samples/python/producer/test/test_producer.py similarity index 100% rename from src/samples/python/producer/test/test_producer.py rename to autonomy/samples/python/producer/test/test_producer.py diff --git a/src/samples/python/transformer/config/params.yaml b/autonomy/samples/python/transformer/config/params.yaml similarity index 100% rename from src/samples/python/transformer/config/params.yaml rename to autonomy/samples/python/transformer/config/params.yaml diff --git a/src/samples/python/transformer/launch/transformer.launch.py b/autonomy/samples/python/transformer/launch/transformer.launch.py similarity index 100% rename from src/samples/python/transformer/launch/transformer.launch.py rename to autonomy/samples/python/transformer/launch/transformer.launch.py diff --git a/src/samples/python/transformer/package.xml b/autonomy/samples/python/transformer/package.xml similarity index 100% rename from src/samples/python/transformer/package.xml rename to autonomy/samples/python/transformer/package.xml diff --git a/autonomy/samples/python/transformer/resource/transformer b/autonomy/samples/python/transformer/resource/transformer new file mode 100644 index 0000000..e69de29 diff --git a/src/samples/python/transformer/setup.cfg b/autonomy/samples/python/transformer/setup.cfg similarity index 100% rename from src/samples/python/transformer/setup.cfg rename to autonomy/samples/python/transformer/setup.cfg diff --git a/src/samples/python/transformer/setup.py b/autonomy/samples/python/transformer/setup.py similarity index 100% rename from src/samples/python/transformer/setup.py rename to autonomy/samples/python/transformer/setup.py diff --git a/src/samples/python/transformer/test/test_copyright.py b/autonomy/samples/python/transformer/test/test_copyright.py similarity index 100% rename from src/samples/python/transformer/test/test_copyright.py rename to autonomy/samples/python/transformer/test/test_copyright.py diff --git a/src/samples/python/transformer/test/test_flake8.py b/autonomy/samples/python/transformer/test/test_flake8.py similarity index 100% rename from src/samples/python/transformer/test/test_flake8.py rename to autonomy/samples/python/transformer/test/test_flake8.py diff --git a/src/samples/python/transformer/test/test_pep257.py b/autonomy/samples/python/transformer/test/test_pep257.py similarity index 100% rename from src/samples/python/transformer/test/test_pep257.py rename to autonomy/samples/python/transformer/test/test_pep257.py diff --git a/src/samples/python/transformer/test/test_transformer.py b/autonomy/samples/python/transformer/test/test_transformer.py similarity index 100% rename from src/samples/python/transformer/test/test_transformer.py rename to autonomy/samples/python/transformer/test/test_transformer.py diff --git a/autonomy/samples/python/transformer/transformer/__init__.py b/autonomy/samples/python/transformer/transformer/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/samples/python/transformer/transformer/transformer_core.py b/autonomy/samples/python/transformer/transformer/transformer_core.py similarity index 100% rename from src/samples/python/transformer/transformer/transformer_core.py rename to autonomy/samples/python/transformer/transformer/transformer_core.py diff --git a/src/samples/python/transformer/transformer/transformer_node.py b/autonomy/samples/python/transformer/transformer/transformer_node.py similarity index 100% rename from src/samples/python/transformer/transformer/transformer_node.py rename to autonomy/samples/python/transformer/transformer/transformer_node.py diff --git a/src/samples/sample_msgs/CMakeLists.txt b/autonomy/samples/sample_msgs/CMakeLists.txt similarity index 100% rename from src/samples/sample_msgs/CMakeLists.txt rename to autonomy/samples/sample_msgs/CMakeLists.txt diff --git a/src/samples/sample_msgs/msg/Filtered.msg b/autonomy/samples/sample_msgs/msg/Filtered.msg similarity index 100% rename from src/samples/sample_msgs/msg/Filtered.msg rename to autonomy/samples/sample_msgs/msg/Filtered.msg diff --git a/src/samples/sample_msgs/msg/FilteredArray.msg b/autonomy/samples/sample_msgs/msg/FilteredArray.msg similarity index 100% rename from src/samples/sample_msgs/msg/FilteredArray.msg rename to autonomy/samples/sample_msgs/msg/FilteredArray.msg diff --git a/src/samples/sample_msgs/msg/Metadata.msg b/autonomy/samples/sample_msgs/msg/Metadata.msg similarity index 100% rename from src/samples/sample_msgs/msg/Metadata.msg rename to autonomy/samples/sample_msgs/msg/Metadata.msg diff --git a/src/samples/sample_msgs/msg/Unfiltered.msg b/autonomy/samples/sample_msgs/msg/Unfiltered.msg similarity index 100% rename from src/samples/sample_msgs/msg/Unfiltered.msg rename to autonomy/samples/sample_msgs/msg/Unfiltered.msg diff --git a/src/samples/sample_msgs/package.xml b/autonomy/samples/sample_msgs/package.xml similarity index 100% rename from src/samples/sample_msgs/package.xml rename to autonomy/samples/sample_msgs/package.xml diff --git a/src/samples/samples_diagram.svg b/autonomy/samples/samples_diagram.svg similarity index 100% rename from src/samples/samples_diagram.svg rename to autonomy/samples/samples_diagram.svg diff --git a/autonomy/simulation/CMakeLists.txt b/autonomy/simulation/CMakeLists.txt new file mode 100644 index 0000000..7a11161 --- /dev/null +++ b/autonomy/simulation/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.10) +project(sample_msgs) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() \ No newline at end of file diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/config/extension.toml b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/config/extension.toml new file mode 100644 index 0000000..686f819 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/config/extension.toml @@ -0,0 +1,22 @@ +[package] + +# Note: Semantic Versioning is used: https://semver.org/ +version = "0.10.31" + +# Description +title = "Isaac Lab Environments" +description="Extension containing suite of environments for robot learning." +readme = "docs/README.md" +repository = "https://github.com/isaac-sim/IsaacLab" +category = "robotics" +keywords = ["robotics", "rl", "il", "learning"] + +[dependencies] +"isaaclab" = {} +"isaaclab_assets" = {} + +[core] +reloadable = false + +[[python.module]] +name = "isaaclab_tasks" diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid.py new file mode 100644 index 0000000..a6840b6 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/modelCfg/humanoid.py @@ -0,0 +1,44 @@ +# Code for specifying custom model parameters + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg + +HAND_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path="/home/hy/Downloads/Humanoid_Wato/ModelAssets/hand.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_depenetration_velocity=5.0, + ), + activate_contact_sensors=False, + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "Revolute_1": 0.0, + "Revolute_2": 0.0, + "Revolute_3": 0.0, + "Revolute_4": 0.0, + "Revolute_5": 0.0, + "Revolute_6": 0.0, + "Revolute_7": 0.0, + "Revolute_8": 0.0, + "Revolute_9": 0.0, + "Revolute_10": 0.0, + "Revolute_11": 0.0, + "Revolute_12": 0.0, + "Revolute_13": 0.0, + "Revolute_14": 0.0, + "Revolute_15": 0.0, + } + ), + actuators={ + "arm": ImplicitActuatorCfg( + joint_names_expr=[".*"], + # velocity_limit=100.0, + # effort_limit=87.0, + stiffness=0.5, + damping=0.5, + ), + }, +) diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/setup.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/setup.py new file mode 100644 index 0000000..a37d989 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/setup.py @@ -0,0 +1,55 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Installation script for the 'isaaclab_tasks' python package.""" + +import os +import toml + +from setuptools import setup + +# Obtain the extension data from the extension.toml file +EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__)) +# Read the extension.toml file +EXTENSION_TOML_DATA = toml.load(os.path.join(EXTENSION_PATH, "config", "extension.toml")) + +# Minimum dependencies required prior to installation +INSTALL_REQUIRES = [ + # generic + "numpy", + "torch==2.5.1", + "torchvision>=0.14.1", # ensure compatibility with torch 1.13.1 + # 5.26.0 introduced a breaking change, so we restricted it for now. + # See issue https://github.com/tensorflow/tensorboard/issues/6808 for details. + "protobuf>=3.20.2, < 5.0.0", + # basic logger + "tensorboard", +] + +PYTORCH_INDEX_URL = ["https://download.pytorch.org/whl/cu118"] + +# Installation operation +setup( + name="skyentific_poclegs", + author="Isaac Lab Project Developers", + maintainer="Isaac Lab Project Developers", + url=EXTENSION_TOML_DATA["package"]["repository"], + version=EXTENSION_TOML_DATA["package"]["version"], + description=EXTENSION_TOML_DATA["package"]["description"], + keywords=EXTENSION_TOML_DATA["package"]["keywords"], + include_package_data=True, + python_requires=">=3.10", + install_requires=INSTALL_REQUIRES, + dependency_links=PYTORCH_INDEX_URL, + packages=["skyentific_poclegs"], + classifiers=[ + "Natural Language :: English", + "Programming Language :: Python :: 3.10", + "Isaac Sim :: 4.5.0", + ], + zip_safe=False, +) + +# From Isaac Lab source/isaaclab_tasks/setup.py diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/__init__.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/__init__.py new file mode 100644 index 0000000..acca6c4 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/__init__.py @@ -0,0 +1,21 @@ +# Copyright (c) 2022-2024, The Berkeley Humanoid Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Package containing task implementations for various robotic environments.""" + +import os +import toml + +from isaaclab_tasks.utils import import_packages + +## +# Register Gym environments. +## + + +# The blacklist is used to prevent importing configs from sub-packages +_BLACKLIST_PKGS = ["utils"] +# Import all configs in this package +import_packages(__name__, _BLACKLIST_PKGS) diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/__init__.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/__init__.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/__init__.py new file mode 100644 index 0000000..37d9b69 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/__init__.py @@ -0,0 +1,26 @@ +import gymnasium as gym +from . import agents + +gym.register( + id="Isaac-Reach-Humanoid-Hand-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:HumanoidHandReachEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:HumanoidHandReachPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Reach-UR10-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.joint_pos_env_cfg:HumanoidHandReachEnvCfg_PLAY", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:HumanoidHandReachPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", + }, +) diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/agents/__init__.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/agents/__init__.py new file mode 100644 index 0000000..732feda --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/agents/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# From Isaac Lab source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config//agents/__init__.py diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/agents/rsl_rl_ppo_cfg.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 0000000..ec8a196 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,33 @@ +from isaaclab.utils import configclass +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class HumanoidHandReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1000 + save_interval = 50 + experiment_name = "reach_humanoid_hand" + run_name = "" + resume = False + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_hidden_dims=[64, 64], + critic_hidden_dims=[64, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.01, + num_learning_epochs=8, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/joint_pos_env_cfg.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/joint_pos_env_cfg.py new file mode 100644 index 0000000..61c186e --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/HumanoidRLEnv/joint_pos_env_cfg.py @@ -0,0 +1,111 @@ +import math +from isaaclab.utils import configclass +import HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp as mdp +from HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.reach_env_cfg import ReachEnvCfg +from HumanoidRLPackage.HumanoidRLSetup.modelCfg.universal_robots import UR10_CFG +from HumanoidRLPackage.HumanoidRLSetup.modelCfg.humanoid import HAND_CFG + + +@configclass +class HumanoidHandReachEnvCfg(ReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # self.scene.robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot = HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + marker_scale = 0.02 + + self.commands.ee_pose.goal_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + self.commands.ee_pose.current_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + + self.commands.ee_pose_2.goal_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + self.commands.ee_pose_2.current_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + + self.commands.ee_pose_3.goal_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + self.commands.ee_pose_3.current_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + + self.commands.ee_pose_4.goal_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + self.commands.ee_pose_4.current_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + + self.commands.ee_pose_5.goal_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + self.commands.ee_pose_5.current_pose_visualizer_cfg.markers["frame"].scale = ( + marker_scale, marker_scale, marker_scale) + + # override events + self.events.reset_robot_joints.params["position_range"] = (0.75, 1.25) + # override rewards + self.rewards.end_effector_position_tracking.params["asset_cfg"].body_names = ["TIP_B_1"] + self.rewards.end_effector_position_tracking_fine_grained.params["asset_cfg"].body_names = [ + "TIP_B_1"] + # self.rewards.end_effector_orientation_tracking.params["asset_cfg"].body_names = ["TIP_B_1"] + + self.rewards.end_effector_2_position_tracking.params["asset_cfg"].body_names = ["TIP_B_2"] + self.rewards.end_effector_2_position_tracking_fine_grained.params["asset_cfg"].body_names = [ + "TIP_B_2"] + # self.rewards.end_effector_2_orientation_tracking.params["asset_cfg"].body_names = ["TIP_B_2"] + + self.rewards.end_effector_3_position_tracking.params["asset_cfg"].body_names = ["TIP_B_3"] + self.rewards.end_effector_3_position_tracking_fine_grained.params["asset_cfg"].body_names = [ + "TIP_B_3"] + # self.rewards.end_effector_3_orientation_tracking.params["asset_cfg"].body_names = ["TIP_B_3"] + + self.rewards.end_effector_4_position_tracking.params["asset_cfg"].body_names = ["TIP_B_4"] + self.rewards.end_effector_4_position_tracking_fine_grained.params["asset_cfg"].body_names = [ + "TIP_B_4"] + # self.rewards.end_effector_4_orientation_tracking.params["asset_cfg"].body_names = ["TIP_B_4"] + + self.rewards.end_effector_5_position_tracking.params["asset_cfg"].body_names = ["TIP_B_5"] + self.rewards.end_effector_5_position_tracking_fine_grained.params["asset_cfg"].body_names = [ + "TIP_B_5"] + # self.rewards.end_effector_5_orientation_tracking.params["asset_cfg"].body_names = ["TIP_B_5"] + + # override actions + self.actions.arm_action = mdp.JointPositionActionCfg( + asset_name="robot", joint_names=[".*"], scale=0.5, use_default_offset=True + ) + # override command generator body + # end-effector is along x-direction + self.commands.ee_pose.body_name = "TIP_B_1" + # self.commands.ee_pose.ranges.pitch = (math.pi / 2, math.pi / 2) # this rotate the pose + + self.commands.ee_pose_2.body_name = "TIP_B_2" + # self.commands.ee_pose_2.ranges.pitch = (math.pi / 2, math.pi / 2) + + self.commands.ee_pose_3.body_name = "TIP_B_3" + # self.commands.ee_pose_3.ranges.pitch = (math.pi / 2, math.pi / 2) + + self.commands.ee_pose_4.body_name = "TIP_B_4" + # self.commands.ee_pose_4.ranges.pitch = (math.pi / 2, math.pi / 2) + + self.commands.ee_pose_5.body_name = "TIP_B_5" + # self.commands.ee_pose_5.ranges.pitch = (math.pi / 2, math.pi / 2) + + # self.commands.ee_pose.ranges.yaw = (-math.pi / 2, -math.pi / 2) + # self.commands.ee_pose_2.ranges.yaw = (-math.pi / 2, -math.pi / 2) + + +@configclass +class HumanoidHandReachEnvCfg_PLAY(HumanoidHandReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + +# (env_isaaclab) hy@hy-LOQ-15IRX9:~/Downloads/Humanoid_Wato/HumanoidRL$ PYTHONPATH=$(pwd) /home/hy/IsaacLab/isaaclab.sh -p HumanoidRLPackage/rsl_rl_scripts/train.py --task=Isaac-Reach-Humanoid-Hand-v0 --headless + +# (env_isaaclab) hy@hy-LOQ-15IRX9:~/Downloads/Humanoid_Wato/HumanoidRL$ PYTHONPATH=$(pwd) /home/hy/IsaacLab/isaaclab.sh -p HumanoidRLPackage/rsl_rl_scripts/play.py --task=Isaac-Reach-Humanoid-Hand-v0 --num_envs=1 diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/__init__.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/config/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/mdp/__init__.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/mdp/__init__.py new file mode 100644 index 0000000..cb8b07b --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/mdp/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022-2024, The Berkeley Humanoid Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the locomotion environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .rewards import * # noqa: F401, F403 + +# From Isaac Lab source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/__init__.py diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/mdp/rewards.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/mdp/rewards.py new file mode 100644 index 0000000..433d69a --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/mdp/rewards.py @@ -0,0 +1,71 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.math import combine_frame_transforms, quat_error_magnitude, quat_mul + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def position_command_error(env: ManagerBasedRLEnv, command_name: str, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize tracking of the position error using L2-norm. + + The function computes the position error between the desired position (from the command) and the + current position of the asset's body (in world frame). The position error is computed as the L2-norm + of the difference between the desired and current positions. + """ + # extract the asset (to enable type hinting) + asset: RigidObject = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + # obtain the desired and current positions + des_pos_b = command[:, :3] + des_pos_w, _ = combine_frame_transforms( + asset.data.root_state_w[:, :3], asset.data.root_state_w[:, 3:7], des_pos_b) + curr_pos_w = asset.data.body_state_w[:, asset_cfg.body_ids[0], :3] # type: ignore + return torch.norm(curr_pos_w - des_pos_w, dim=1) + + +def position_command_error_tanh( + env: ManagerBasedRLEnv, std: float, command_name: str, asset_cfg: SceneEntityCfg +) -> torch.Tensor: + """Reward tracking of the position using the tanh kernel. + + The function computes the position error between the desired position (from the command) and the + current position of the asset's body (in world frame) and maps it with a tanh kernel. + """ + # extract the asset (to enable type hinting) + asset: RigidObject = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + # obtain the desired and current positions + des_pos_b = command[:, :3] + des_pos_w, _ = combine_frame_transforms( + asset.data.root_state_w[:, :3], asset.data.root_state_w[:, 3:7], des_pos_b) + curr_pos_w = asset.data.body_state_w[:, asset_cfg.body_ids[0], :3] # type: ignore + distance = torch.norm(curr_pos_w - des_pos_w, dim=1) + return 1 - torch.tanh(distance / std) + + +def orientation_command_error(env: ManagerBasedRLEnv, command_name: str, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize tracking orientation error using shortest path. + + The function computes the orientation error between the desired orientation (from the command) and the + current orientation of the asset's body (in world frame). The orientation error is computed as the shortest + path between the desired and current orientations. + """ + # extract the asset (to enable type hinting) + asset: RigidObject = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + # obtain the desired and current orientations + des_quat_b = command[:, 3:7] + des_quat_w = quat_mul(asset.data.root_state_w[:, 3:7], des_quat_b) + curr_quat_w = asset.data.body_state_w[:, asset_cfg.body_ids[0], 3:7] # type: ignore + return quat_error_magnitude(curr_quat_w, des_quat_w) diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/reach_env_cfg.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/reach_env_cfg.py new file mode 100644 index 0000000..45646ef --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/HumanoidRLSetup/tasks/manipulation/reach_env_cfg.py @@ -0,0 +1,343 @@ +from dataclasses import MISSING + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ActionTermCfg as ActionTerm +from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise + +import HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp as mdp + + +@configclass +class ReachSceneCfg(InteractiveSceneCfg): + """Configuration for the scene with a robotic arm.""" + + # world + ground = AssetBaseCfg( + prim_path="/World/ground", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), + ) + + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd", + ), + init_state=AssetBaseCfg.InitialStateCfg( + pos=(0.55, 0.0, 0.0), rot=(0.70711, 0.0, 0.0, 0.70711)), + ) + + # robots + robot: ArticulationCfg = MISSING + + # lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=2500.0), + ) + + +## +# MDP settings +## + + +@configclass +class CommandsCfg: + """Command terms for the MDP.""" + + ee_pose = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name="TIP_B_1", # end_effector name + resampling_time_range=(4.0, 4.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( # within task space of finger + pos_x=(0.07346, 0.07346), + pos_y=(0.13, 0.1455), + pos_z=(0.09, 0.1005), + # no rotational variation + roll=(0.0, 0.0), + pitch=(0.0, 0.0), + yaw=(0.0, 0.0), + ), + ) + + ee_pose_2 = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name="TIP_B_2", + resampling_time_range=(4.0, 4.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( + pos_x=(0.04746, 0.04746), + # pos_y=(0.135, 0.14), + pos_y=(0.15406, 0.15558), + # pos_z=(0.09, 0.136), + pos_z=(0.098, 0.10058), + roll=(0.0, 0.0), + pitch=(0.0, 0.0), + yaw=(0.0, 0.0), + ), + ) + + ee_pose_3 = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name="TIP_B_3", + resampling_time_range=(4.0, 4.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( + pos_x=(0.01996, 0.01996), + pos_y=(0.134, 0.145), + pos_z=(0.09, 0.11), + roll=(0.0, 0.0), + pitch=(0.0, 0.0), + yaw=(0.0, 0.0), + ), + ) + + ee_pose_4 = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name="TIP_B_4", + resampling_time_range=(4.0, 4.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( + pos_x=(-0.00754, -0.00754), + pos_y=(0.119, 0.13), + pos_z=(0.09, 0.1), + roll=(0.0, 0.0), + pitch=(0.0, 0.0), + yaw=(0.0, 0.0), + ), + ) + + ee_pose_5 = mdp.UniformPoseCommandCfg( + asset_name="robot", + body_name="TIP_B_5", + resampling_time_range=(4.0, 4.0), + debug_vis=True, + ranges=mdp.UniformPoseCommandCfg.Ranges( + pos_x=(0.0869, 0.0869), + pos_y=(0.04, 0.052), + pos_z=(0.17, 0.17248), + roll=(0.0, 0.0), + pitch=(0.0, 0.0), + yaw=(0.0, 0.0), + ), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + arm_action: ActionTerm = MISSING + gripper_action: ActionTerm | None = None + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + joint_pos = ObsTerm(func=mdp.joint_pos_rel, noise=Unoise(n_min=-0.01, n_max=0.01)) + joint_vel = ObsTerm(func=mdp.joint_vel_rel, noise=Unoise(n_min=-0.01, n_max=0.01)) + + pose_command = ObsTerm(func=mdp.generated_commands, params={"command_name": "ee_pose"}) + pose_command_2 = ObsTerm(func=mdp.generated_commands, params={"command_name": "ee_pose_2"}) + pose_command_3 = ObsTerm(func=mdp.generated_commands, params={"command_name": "ee_pose_3"}) + pose_command_4 = ObsTerm(func=mdp.generated_commands, params={"command_name": "ee_pose_4"}) + pose_command_5 = ObsTerm(func=mdp.generated_commands, params={"command_name": "ee_pose_5"}) + + actions = ObsTerm(func=mdp.last_action) + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_robot_joints = EventTerm( + func=mdp.reset_joints_by_scale, + mode="reset", + params={ + "position_range": (0.5, 1.5), + "velocity_range": (0.0, 0.0), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # task terms + end_effector_position_tracking = RewTerm( + func=mdp.position_command_error, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_1"), "command_name": "ee_pose"}, + ) + end_effector_2_position_tracking = RewTerm( + func=mdp.position_command_error, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_2"), "command_name": "ee_pose_2"}, + ) + end_effector_3_position_tracking = RewTerm( + func=mdp.position_command_error, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_3"), "command_name": "ee_pose_3"}, + ) + end_effector_4_position_tracking = RewTerm( + func=mdp.position_command_error, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_4"), "command_name": "ee_pose_4"}, + ) + end_effector_5_position_tracking = RewTerm( + func=mdp.position_command_error, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_5"), "command_name": "ee_pose_5"}, + ) + + end_effector_position_tracking_fine_grained = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.1, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_1"), "std": 0.1, "command_name": "ee_pose"}, + ) + end_effector_2_position_tracking_fine_grained = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.1, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_2"), "std": 0.1, "command_name": "ee_pose_2"}, + ) + end_effector_3_position_tracking_fine_grained = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.1, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_3"), "std": 0.1, "command_name": "ee_pose_3"}, + ) + end_effector_4_position_tracking_fine_grained = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.1, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_4"), "std": 0.1, "command_name": "ee_pose_4"}, + ) + end_effector_5_position_tracking_fine_grained = RewTerm( + func=mdp.position_command_error_tanh, + weight=0.1, + params={"asset_cfg": SceneEntityCfg( + "robot", body_names="TIP_B_5"), "std": 0.1, "command_name": "ee_pose_5"}, + ) + + # end_effector_orientation_tracking = RewTerm( + # func=mdp.orientation_command_error, + # weight=-0.1, + # params={"asset_cfg": SceneEntityCfg("robot", body_names="TIP_B_1"), "command_name": "ee_pose"}, + # ) + # end_effector_2_orientation_tracking = RewTerm( + # func=mdp.orientation_command_error, + # weight=-0.1, + # params={"asset_cfg": SceneEntityCfg("robot", body_names="TIP_B_2"), "command_name": "ee_pose_2"}, + # ) + # end_effector_3_orientation_tracking = RewTerm( + # func=mdp.orientation_command_error, + # weight=-0.1, + # params={"asset_cfg": SceneEntityCfg("robot", body_names="TIP_B_3"), "command_name": "ee_pose_3"}, + # ) + # end_effector_4_orientation_tracking = RewTerm( + # func=mdp.orientation_command_error, + # weight=-0.1, + # params={"asset_cfg": SceneEntityCfg("robot", body_names="TIP_B_4"), "command_name": "ee_pose_4"}, + # ) + # end_effector_5_orientation_tracking = RewTerm( + # func=mdp.orientation_command_error, + # weight=-0.1, + # params={"asset_cfg": SceneEntityCfg("robot", body_names="TIP_B_5"), "command_name": "ee_pose_5"}, + # ) + + # action penalty + action_rate = RewTerm(func=mdp.action_rate_l2, weight=-0.0001) + joint_vel = RewTerm( + func=mdp.joint_vel_l2, + weight=-0.0001, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + +@configclass +class CurriculumCfg: + """Curriculum terms for the MDP.""" + + action_rate = CurrTerm( + func=mdp.modify_reward_weight, params={ + "term_name": "action_rate", "weight": -0.005, "num_steps": 4500} + ) + + joint_vel = CurrTerm( + func=mdp.modify_reward_weight, params={ + "term_name": "joint_vel", "weight": -0.001, "num_steps": 4500} + ) + + +## +# Environment configuration +## + + +@configclass +class ReachEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the reach end-effector pose tracking environment.""" + + # Scene settings + scene: ReachSceneCfg = ReachSceneCfg(num_envs=4096, env_spacing=2.5) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands: CommandsCfg = CommandsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + curriculum: CurriculumCfg = CurriculumCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 2 + self.sim.render_interval = self.decimation + self.episode_length_s = 12.0 + self.viewer.eye = (3.5, 3.5, 3.5) + # simulation settings + self.sim.dt = 1.0 / 60.0 diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/cli_args.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/cli_args.py new file mode 100644 index 0000000..75b0705 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/cli_args.py @@ -0,0 +1,97 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import argparse +import random +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg + + +def add_rsl_rl_args(parser: argparse.ArgumentParser): + """Add RSL-RL arguments to the parser. + + Args: + parser: The parser to add the arguments to. + """ + # create a new argument group + arg_group = parser.add_argument_group("rsl_rl", description="Arguments for RSL-RL agent.") + # -- experiment arguments + arg_group.add_argument( + "--experiment_name", type=str, default=None, help="Name of the experiment folder where logs will be stored." + ) + arg_group.add_argument("--run_name", type=str, default=None, + help="Run name suffix to the log directory.") + # -- load arguments + arg_group.add_argument("--resume", type=bool, default=None, + help="Whether to resume from a checkpoint.") + arg_group.add_argument("--load_run", type=str, default=None, + help="Name of the run folder to resume from.") + arg_group.add_argument("--checkpoint", type=str, default=None, + help="Checkpoint file to resume from.") + # -- logger arguments + arg_group.add_argument( + "--logger", type=str, default=None, choices={"wandb", "tensorboard", "neptune"}, help="Logger module to use." + ) + arg_group.add_argument( + "--log_project_name", type=str, default=None, help="Name of the logging project when using wandb or neptune." + ) + + +def parse_rsl_rl_cfg(task_name: str, args_cli: argparse.Namespace) -> RslRlOnPolicyRunnerCfg: + """Parse configuration for RSL-RL agent based on inputs. + + Args: + task_name: The name of the environment. + args_cli: The command line arguments. + + Returns: + The parsed configuration for RSL-RL agent based on inputs. + """ + from isaaclab_tasks.utils.parse_cfg import load_cfg_from_registry + + # load the default configuration + rslrl_cfg: RslRlOnPolicyRunnerCfg = load_cfg_from_registry(task_name, "rsl_rl_cfg_entry_point") + rslrl_cfg = update_rsl_rl_cfg(rslrl_cfg, args_cli) + return rslrl_cfg + + +def update_rsl_rl_cfg(agent_cfg: RslRlOnPolicyRunnerCfg, args_cli: argparse.Namespace): + """Update configuration for RSL-RL agent based on inputs. + + Args: + agent_cfg: The configuration for RSL-RL agent. + args_cli: The command line arguments. + + Returns: + The updated configuration for RSL-RL agent based on inputs. + """ + # override the default configuration with CLI arguments + if hasattr(args_cli, "seed") and args_cli.seed is not None: + # randomly sample a seed if seed = -1 + if args_cli.seed == -1: + args_cli.seed = random.randint(0, 10000) + agent_cfg.seed = args_cli.seed + if args_cli.resume is not None: + agent_cfg.resume = args_cli.resume + if args_cli.load_run is not None: + agent_cfg.load_run = args_cli.load_run + if args_cli.checkpoint is not None: + agent_cfg.load_checkpoint = args_cli.checkpoint + if args_cli.run_name is not None: + agent_cfg.run_name = args_cli.run_name + if args_cli.logger is not None: + agent_cfg.logger = args_cli.logger + # set the project name for wandb and neptune + if agent_cfg.logger in {"wandb", "neptune"} and args_cli.log_project_name: + agent_cfg.wandb_project = args_cli.log_project_name + agent_cfg.neptune_project = args_cli.log_project_name + + return agent_cfg + +# From Isaac Lab scripts/reinforcement_learning/rsl_rl/cli_args.py diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/play.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/play.py new file mode 100644 index 0000000..d45a014 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/play.py @@ -0,0 +1,166 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to play a checkpoint if an RL agent from RSL-RL.""" + +"""Launch Isaac Sim Simulator first.""" + + +from isaaclab.app import AppLauncher +import argparse +import cli_args # isort: skip +import gymnasium as gym +import os +import time +import torch +from rsl_rl.runners import OnPolicyRunner +from isaaclab.envs import DirectMARLEnv, multi_agent_to_single_agent +from isaaclab.utils.assets import retrieve_file_path +from isaaclab.utils.dict import print_dict +from isaaclab.utils.pretrained_checkpoint import get_published_pretrained_checkpoint +from isaaclab_tasks.utils import get_checkpoint_path, parse_env_cfg +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlVecEnvWrapper, export_policy_as_jit, export_policy_as_onnx + +# local imports + +# add argparse arguments +parser = argparse.ArgumentParser(description="Train an RL agent with RSL-RL.") +parser.add_argument("--video", action="store_true", default=False, + help="Record videos during training.") +parser.add_argument("--video_length", type=int, default=200, + help="Length of the recorded video (in steps).") +parser.add_argument( + "--disable_fabric", action="store_true", default=False, help="Disable fabric and use USD I/O operations." +) +parser.add_argument("--num_envs", type=int, default=None, + help="Number of environments to simulate.") +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +parser.add_argument( + "--use_pretrained_checkpoint", + action="store_true", + help="Use the pre-trained checkpoint from Nucleus.", +) +parser.add_argument("--real-time", action="store_true", default=False, + help="Run in real-time, if possible.") +# append RSL-RL cli arguments +cli_args.add_rsl_rl_args(parser) +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() +# always enable cameras to record video +if args_cli.video: + args_cli.enable_cameras = True + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + + +import HumanoidRLPackage.HumanoidRLSetup.tasks # noqa: F401 + + +def main(): + """Play with RSL-RL agent.""" + # parse configuration + env_cfg = parse_env_cfg( + args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric + ) + agent_cfg: RslRlOnPolicyRunnerCfg = cli_args.parse_rsl_rl_cfg(args_cli.task, args_cli) + + # specify directory for logging experiments + log_root_path = os.path.join("logs", "rsl_rl", agent_cfg.experiment_name) + log_root_path = os.path.abspath(log_root_path) + print(f"[INFO] Loading experiment from directory: {log_root_path}") + if args_cli.use_pretrained_checkpoint: + resume_path = get_published_pretrained_checkpoint("rsl_rl", args_cli.task) + if not resume_path: + print("[INFO] Unfortunately a pre-trained checkpoint is currently unavailable for this task.") + return + elif args_cli.checkpoint: + resume_path = retrieve_file_path(args_cli.checkpoint) + else: + resume_path = get_checkpoint_path( + log_root_path, agent_cfg.load_run, agent_cfg.load_checkpoint) + + log_dir = os.path.dirname(resume_path) + + # create isaac environment + env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) + + # convert to single-agent instance if required by the RL algorithm + if isinstance(env.unwrapped, DirectMARLEnv): + env = multi_agent_to_single_agent(env) + + # wrap for video recording + if args_cli.video: + video_kwargs = { + "video_folder": os.path.join(log_dir, "videos", "play"), + "step_trigger": lambda step: step == 0, + "video_length": args_cli.video_length, + "disable_logger": True, + } + print("[INFO] Recording videos during training.") + print_dict(video_kwargs, nesting=4) + env = gym.wrappers.RecordVideo(env, **video_kwargs) + + # wrap around environment for rsl-rl + env = RslRlVecEnvWrapper(env) + + print(f"[INFO]: Loading model checkpoint from: {resume_path}") + # load previously trained model + ppo_runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device) + ppo_runner.load(resume_path) + + # obtain the trained policy for inference + policy = ppo_runner.get_inference_policy(device=env.unwrapped.device) + + policy_nn = ppo_runner.alg.policy + + # export policy to onnx/jit + export_model_dir = os.path.join(os.path.dirname(resume_path), "exported") + export_policy_as_jit(policy_nn, ppo_runner.obs_normalizer, + path=export_model_dir, filename="policy.pt") + export_policy_as_onnx( + policy_nn, normalizer=ppo_runner.obs_normalizer, path=export_model_dir, filename="policy.onnx" + ) + + dt = env.unwrapped.physics_dt + + # reset environment + obs, _ = env.get_observations() + timestep = 0 + # simulate environment + while simulation_app.is_running(): + start_time = time.time() + # run everything in inference mode + with torch.inference_mode(): + # agent stepping + actions = policy(obs) + # env stepping + obs, _, _, _ = env.step(actions) + if args_cli.video: + timestep += 1 + # Exit the play loop after recording one video + if timestep == args_cli.video_length: + break + + # time delay for real-time evaluation + sleep_time = dt - (time.time() - start_time) + if args_cli.real_time and sleep_time > 0: + time.sleep(sleep_time) + + # close the simulator + env.close() + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() + +# From Isaac Lab scripts/reinforcement_learning/rsl_rl/play.py diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/train.py b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/train.py new file mode 100644 index 0000000..c51f544 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/HumanoidRLPackage/rsl_rl_scripts/train.py @@ -0,0 +1,161 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to train RL agent with RSL-RL.""" + +"""Launch Isaac Sim Simulator first.""" + +import sys +import argparse +from isaaclab.app import AppLauncher +import cli_args # isort: skip +import gymnasium as gym +import os +import torch +from datetime import datetime +from rsl_rl.runners import OnPolicyRunner +from isaaclab.envs import ( + DirectMARLEnv, + DirectMARLEnvCfg, + DirectRLEnvCfg, + ManagerBasedRLEnvCfg, + multi_agent_to_single_agent, +) +from isaaclab.utils.dict import print_dict +from isaaclab.utils.io import dump_pickle, dump_yaml +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlVecEnvWrapper +from isaaclab_tasks.utils.hydra import hydra_task_config +from isaaclab_tasks.utils import get_checkpoint_path + + +# local imports + + +# add argparse arguments +parser = argparse.ArgumentParser(description="Train an RL agent with RSL-RL.") +parser.add_argument("--video", action="store_true", default=False, + help="Record videos during training.") +parser.add_argument("--video_length", type=int, default=200, + help="Length of the recorded video (in steps).") +parser.add_argument("--video_interval", type=int, default=2000, + help="Interval between video recordings (in steps).") +parser.add_argument("--num_envs", type=int, default=None, + help="Number of environments to simulate.") +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +parser.add_argument("--seed", type=int, default=None, help="Seed used for the environment") +parser.add_argument("--max_iterations", type=int, default=None, + help="RL Policy training iterations.") +# append RSL-RL cli arguments +cli_args.add_rsl_rl_args(parser) +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +args_cli, hydra_args = parser.parse_known_args() + +# always enable cameras to record video +if args_cli.video: + args_cli.enable_cameras = True + +# clear out sys.argv for Hydra +sys.argv = [sys.argv[0]] + hydra_args + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + + +import HumanoidRLPackage.HumanoidRLSetup.tasks # noqa: F401 + +torch.backends.cuda.matmul.allow_tf32 = True +torch.backends.cudnn.allow_tf32 = True +torch.backends.cudnn.deterministic = False +torch.backends.cudnn.benchmark = False + + +@hydra_task_config(args_cli.task, "rsl_rl_cfg_entry_point") +def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: RslRlOnPolicyRunnerCfg): + """Train with RSL-RL agent.""" + # override configurations with non-hydra CLI arguments + agent_cfg = cli_args.update_rsl_rl_cfg(agent_cfg, args_cli) + env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs + agent_cfg.max_iterations = ( + args_cli.max_iterations if args_cli.max_iterations is not None else agent_cfg.max_iterations + ) + + # set the environment seed + # note: certain randomizations occur in the environment initialization so we set the seed here + env_cfg.seed = agent_cfg.seed + env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device + + # specify directory for logging experiments + log_root_path = os.path.join("logs", "rsl_rl", agent_cfg.experiment_name) + log_root_path = os.path.abspath(log_root_path) + print(f"[INFO] Logging experiment in directory: {log_root_path}") + # specify directory for logging runs: {time-stamp}_{run_name} + log_dir = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + # This way, the Ray Tune workflow can extract experiment name. + print(f"Exact experiment name requested from command line: {log_dir}") + if agent_cfg.run_name: + log_dir += f"_{agent_cfg.run_name}" + log_dir = os.path.join(log_root_path, log_dir) + + # create isaac environment + env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None) + + # convert to single-agent instance if required by the RL algorithm + if isinstance(env.unwrapped, DirectMARLEnv): + env = multi_agent_to_single_agent(env) + + # save resume path before creating a new log_dir + if agent_cfg.resume: + resume_path = get_checkpoint_path( + log_root_path, agent_cfg.load_run, agent_cfg.load_checkpoint) + + # wrap for video recording + if args_cli.video: + video_kwargs = { + "video_folder": os.path.join(log_dir, "videos", "train"), + "step_trigger": lambda step: step % args_cli.video_interval == 0, + "video_length": args_cli.video_length, + "disable_logger": True, + } + print("[INFO] Recording videos during training.") + print_dict(video_kwargs, nesting=4) + env = gym.wrappers.RecordVideo(env, **video_kwargs) + + # wrap around environment for rsl-rl + env = RslRlVecEnvWrapper(env) + + # create runner from rsl-rl + runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=log_dir, device=agent_cfg.device) + # write git state to logs + runner.add_git_repo_to_log(__file__) + # load the checkpoint + if agent_cfg.resume: + print(f"[INFO]: Loading model checkpoint from: {resume_path}") + # load previously trained model + runner.load(resume_path) + + # dump the configuration into log-directory + dump_yaml(os.path.join(log_dir, "params", "env.yaml"), env_cfg) + dump_yaml(os.path.join(log_dir, "params", "agent.yaml"), agent_cfg) + dump_pickle(os.path.join(log_dir, "params", "env.pkl"), env_cfg) + dump_pickle(os.path.join(log_dir, "params", "agent.pkl"), agent_cfg) + + # run training + runner.learn(num_learning_iterations=agent_cfg.max_iterations, init_at_random_ep_len=True) + + # close the simulator + env.close() + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() + +# From Isaac Lab scripts/reinforcement_learning/rsl_rl/train.py diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/config.yaml b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/config.yaml new file mode 100644 index 0000000..e6be28a --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/config.yaml @@ -0,0 +1,1046 @@ +env: + viewer: + eye: + - 3.5 + - 3.5 + - 3.5 + lookat: + - 0.0 + - 0.0 + - 0.0 + cam_prim_path: /OmniverseKit_Persp + resolution: + - 1280 + - 720 + origin_type: world + env_index: 0 + asset_name: null + body_name: null + sim: + physics_prim_path: /physicsScene + device: cuda:0 + dt: 0.016666666666666666 + render_interval: 2 + gravity: + - 0.0 + - 0.0 + - -9.81 + enable_scene_query_support: false + use_fabric: true + physx: + solver_type: 1 + min_position_iteration_count: 1 + max_position_iteration_count: 255 + min_velocity_iteration_count: 0 + max_velocity_iteration_count: 255 + enable_ccd: false + enable_stabilization: true + enable_enhanced_determinism: false + bounce_threshold_velocity: 0.5 + friction_offset_threshold: 0.04 + friction_correlation_distance: 0.025 + gpu_max_rigid_contact_count: 8388608 + gpu_max_rigid_patch_count: 163840 + gpu_found_lost_pairs_capacity: 2097152 + gpu_found_lost_aggregate_pairs_capacity: 33554432 + gpu_total_aggregate_pairs_capacity: 2097152 + gpu_collision_stack_size: 67108864 + gpu_heap_capacity: 67108864 + gpu_temp_buffer_capacity: 16777216 + gpu_max_num_partitions: 8 + gpu_max_soft_body_contacts: 1048576 + gpu_max_particle_contacts: 1048576 + physics_material: + func: isaaclab.sim.spawners.materials.physics_materials:spawn_rigid_body_material + static_friction: 0.5 + dynamic_friction: 0.5 + restitution: 0.0 + improve_patch_friction: true + friction_combine_mode: average + restitution_combine_mode: average + compliant_contact_stiffness: 0.0 + compliant_contact_damping: 0.0 + render: + enable_translucency: null + enable_reflections: null + enable_global_illumination: null + antialiasing_mode: null + enable_dlssg: null + enable_dl_denoiser: null + dlss_mode: null + enable_direct_lighting: null + samples_per_pixel: null + enable_shadows: null + enable_ambient_occlusion: null + carb_settings: null + rendering_mode: null + ui_window_class_type: isaaclab.envs.ui.manager_based_rl_env_window:ManagerBasedRLEnvWindow + seed: null + decimation: 2 + scene: + num_envs: 4096 + env_spacing: 2.5 + lazy_sensor_update: true + replicate_physics: true + filter_collisions: true + robot: + class_type: isaaclab.assets.articulation.articulation:Articulation + prim_path: '{ENV_REGEX_NS}/Robot' + spawn: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: + rigid_body_enabled: null + kinematic_enabled: null + disable_gravity: false + linear_damping: null + angular_damping: null + max_linear_velocity: null + max_angular_velocity: null + max_depenetration_velocity: 5.0 + max_contact_impulse: null + enable_gyroscopic_forces: null + retain_accelerations: null + solver_position_iteration_count: null + solver_velocity_iteration_count: null + sleep_threshold: null + stabilization_threshold: null + collision_props: null + activate_contact_sensors: false + scale: null + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: /home/hy/Downloads/Humanoid_Wato/ModelAssets/hand.usd + variants: null + init_state: + pos: + - 0.0 + - 0.0 + - 0.0 + rot: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + lin_vel: + - 0.0 + - 0.0 + - 0.0 + ang_vel: + - 0.0 + - 0.0 + - 0.0 + joint_pos: + Revolute_1: 0.0 + Revolute_2: 0.0 + Revolute_3: 0.0 + Revolute_4: 0.0 + Revolute_5: 0.0 + Revolute_6: 0.0 + Revolute_7: 0.0 + Revolute_8: 0.0 + Revolute_9: 0.0 + Revolute_10: 0.0 + Revolute_11: 0.0 + Revolute_12: 0.0 + Revolute_13: 0.0 + Revolute_14: 0.0 + Revolute_15: 0.0 + joint_vel: + .*: 0.0 + collision_group: 0 + debug_vis: false + soft_joint_pos_limit_factor: 1.0 + actuators: + arm: + class_type: isaaclab.actuators.actuator_pd:ImplicitActuator + joint_names_expr: + - .* + effort_limit: null + velocity_limit: null + effort_limit_sim: null + velocity_limit_sim: null + stiffness: 0.5 + damping: 0.5 + armature: null + friction: null + ground: + class_type: null + prim_path: /World/ground + spawn: + func: isaaclab.sim.spawners.from_files.from_files:spawn_ground_plane + visible: true + semantic_tags: null + copy_from_source: true + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Environments/Grid/default_environment.usd + color: + - 0.0 + - 0.0 + - 0.0 + size: + - 100.0 + - 100.0 + physics_material: + func: isaaclab.sim.spawners.materials.physics_materials:spawn_rigid_body_material + static_friction: 0.5 + dynamic_friction: 0.5 + restitution: 0.0 + improve_patch_friction: true + friction_combine_mode: average + restitution_combine_mode: average + compliant_contact_stiffness: 0.0 + compliant_contact_damping: 0.0 + init_state: + pos: + - 0.0 + - 0.0 + - -1.05 + rot: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + collision_group: 0 + debug_vis: false + table: + class_type: null + prim_path: '{ENV_REGEX_NS}/Table' + spawn: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: null + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/Mounts/SeattleLabTable/table_instanceable.usd + variants: null + init_state: + pos: + - 0.55 + - 0.0 + - 0.0 + rot: + - 0.70711 + - 0.0 + - 0.0 + - 0.70711 + collision_group: 0 + debug_vis: false + light: + class_type: null + prim_path: /World/light + spawn: + func: isaaclab.sim.spawners.lights.lights:spawn_light + visible: true + semantic_tags: null + copy_from_source: true + prim_type: DomeLight + color: + - 0.75 + - 0.75 + - 0.75 + enable_color_temperature: false + color_temperature: 6500.0 + normalize: false + exposure: 0.0 + intensity: 2500.0 + texture_file: null + texture_format: automatic + visible_in_primary_ray: true + init_state: + pos: + - 0.0 + - 0.0 + - 0.0 + rot: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + collision_group: 0 + debug_vis: false + recorders: + dataset_file_handler_class_type: isaaclab.utils.datasets.hdf5_dataset_file_handler:HDF5DatasetFileHandler + dataset_export_dir_path: /tmp/isaaclab/logs + dataset_filename: dataset + dataset_export_mode: + _value_: 1 + _name_: EXPORT_ALL + export_in_record_pre_reset: true + observations: + policy: + concatenate_terms: true + enable_corruption: true + history_length: null + flatten_history_dim: true + joint_pos: + func: isaaclab.envs.mdp.observations:joint_pos_rel + params: {} + modifiers: null + noise: + func: isaaclab.utils.noise.noise_model:uniform_noise + operation: add + n_min: -0.01 + n_max: 0.01 + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + joint_vel: + func: isaaclab.envs.mdp.observations:joint_vel_rel + params: {} + modifiers: null + noise: + func: isaaclab.utils.noise.noise_model:uniform_noise + operation: add + n_min: -0.01 + n_max: 0.01 + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + pose_command: + func: isaaclab.envs.mdp.observations:generated_commands + params: + command_name: ee_pose + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + pose_command_2: + func: isaaclab.envs.mdp.observations:generated_commands + params: + command_name: ee_pose_2 + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + pose_command_3: + func: isaaclab.envs.mdp.observations:generated_commands + params: + command_name: ee_pose_3 + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + pose_command_4: + func: isaaclab.envs.mdp.observations:generated_commands + params: + command_name: ee_pose_4 + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + pose_command_5: + func: isaaclab.envs.mdp.observations:generated_commands + params: + command_name: ee_pose_5 + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + actions: + func: isaaclab.envs.mdp.observations:last_action + params: {} + modifiers: null + noise: null + clip: null + scale: null + history_length: 0 + flatten_history_dim: true + actions: + arm_action: + class_type: isaaclab.envs.mdp.actions.joint_actions:JointPositionAction + asset_name: robot + debug_vis: false + clip: null + joint_names: + - .* + scale: 0.5 + offset: 0.0 + preserve_order: false + use_default_offset: true + gripper_action: null + events: + reset_robot_joints: + func: isaaclab.envs.mdp.events:reset_joints_by_scale + params: + position_range: + - 0.75 + - 1.25 + velocity_range: + - 0.0 + - 0.0 + mode: reset + interval_range_s: null + is_global_time: false + min_step_count_between_reset: 0 + rerender_on_reset: false + wait_for_textures: true + xr: null + is_finite_horizon: false + episode_length_s: 12.0 + rewards: + end_effector_position_tracking: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_1 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + command_name: ee_pose + weight: -0.2 + end_effector_2_position_tracking: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_2 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + command_name: ee_pose_2 + weight: -0.2 + end_effector_3_position_tracking: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_3 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + command_name: ee_pose_3 + weight: -0.2 + end_effector_4_position_tracking: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_4 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + command_name: ee_pose_4 + weight: -0.2 + end_effector_5_position_tracking: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_5 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + command_name: ee_pose_5 + weight: -0.2 + end_effector_position_tracking_fine_grained: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error_tanh + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_1 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + std: 0.1 + command_name: ee_pose + weight: 0.1 + end_effector_2_position_tracking_fine_grained: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error_tanh + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_2 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + std: 0.1 + command_name: ee_pose_2 + weight: 0.1 + end_effector_3_position_tracking_fine_grained: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error_tanh + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_3 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + std: 0.1 + command_name: ee_pose_3 + weight: 0.1 + end_effector_4_position_tracking_fine_grained: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error_tanh + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_4 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + std: 0.1 + command_name: ee_pose_4 + weight: 0.1 + end_effector_5_position_tracking_fine_grained: + func: HumanoidRLPackage.HumanoidRLSetup.tasks.manipulation.mdp.rewards:position_command_error_tanh + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: + - TIP_B_5 + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + std: 0.1 + command_name: ee_pose_5 + weight: 0.1 + action_rate: + func: isaaclab.envs.mdp.rewards:action_rate_l2 + params: {} + weight: -0.0001 + joint_vel: + func: isaaclab.envs.mdp.rewards:joint_vel_l2 + params: + asset_cfg: + name: robot + joint_names: null + joint_ids: slice(None,None,None) + fixed_tendon_names: null + fixed_tendon_ids: slice(None,None,None) + body_names: null + body_ids: slice(None,None,None) + object_collection_names: null + object_collection_ids: slice(None,None,None) + preserve_order: false + weight: -0.0001 + terminations: + time_out: + func: isaaclab.envs.mdp.terminations:time_out + params: {} + time_out: true + curriculum: + action_rate: + func: isaaclab.envs.mdp.curriculums:modify_reward_weight + params: + term_name: action_rate + weight: -0.005 + num_steps: 4500 + joint_vel: + func: isaaclab.envs.mdp.curriculums:modify_reward_weight + params: + term_name: joint_vel + weight: -0.001 + num_steps: 4500 + commands: + ee_pose: + class_type: isaaclab.envs.mdp.commands.pose_command:UniformPoseCommand + resampling_time_range: + - 4.0 + - 4.0 + debug_vis: true + asset_name: robot + body_name: TIP_B_1 + make_quat_unique: false + ranges: + pos_x: + - 0.07346 + - 0.07346 + pos_y: + - 0.13 + - 0.1455 + pos_z: + - 0.09 + - 0.1005 + roll: + - 0.0 + - 0.0 + pitch: + - 0.0 + - 0.0 + yaw: + - 0.0 + - 0.0 + goal_pose_visualizer_cfg: + prim_path: /Visuals/Command/goal_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + current_pose_visualizer_cfg: + prim_path: /Visuals/Command/body_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + ee_pose_2: + class_type: isaaclab.envs.mdp.commands.pose_command:UniformPoseCommand + resampling_time_range: + - 4.0 + - 4.0 + debug_vis: true + asset_name: robot + body_name: TIP_B_2 + make_quat_unique: false + ranges: + pos_x: + - 0.04746 + - 0.04746 + pos_y: + - 0.15406 + - 0.15558 + pos_z: + - 0.098 + - 0.10058 + roll: + - 0.0 + - 0.0 + pitch: + - 0.0 + - 0.0 + yaw: + - 0.0 + - 0.0 + goal_pose_visualizer_cfg: + prim_path: /Visuals/Command/goal_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + current_pose_visualizer_cfg: + prim_path: /Visuals/Command/body_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + ee_pose_3: + class_type: isaaclab.envs.mdp.commands.pose_command:UniformPoseCommand + resampling_time_range: + - 4.0 + - 4.0 + debug_vis: true + asset_name: robot + body_name: TIP_B_3 + make_quat_unique: false + ranges: + pos_x: + - 0.01996 + - 0.01996 + pos_y: + - 0.134 + - 0.145 + pos_z: + - 0.09 + - 0.11 + roll: + - 0.0 + - 0.0 + pitch: + - 0.0 + - 0.0 + yaw: + - 0.0 + - 0.0 + goal_pose_visualizer_cfg: + prim_path: /Visuals/Command/goal_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + current_pose_visualizer_cfg: + prim_path: /Visuals/Command/body_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + ee_pose_4: + class_type: isaaclab.envs.mdp.commands.pose_command:UniformPoseCommand + resampling_time_range: + - 4.0 + - 4.0 + debug_vis: true + asset_name: robot + body_name: TIP_B_4 + make_quat_unique: false + ranges: + pos_x: + - -0.00754 + - -0.00754 + pos_y: + - 0.119 + - 0.13 + pos_z: + - 0.09 + - 0.1 + roll: + - 0.0 + - 0.0 + pitch: + - 0.0 + - 0.0 + yaw: + - 0.0 + - 0.0 + goal_pose_visualizer_cfg: + prim_path: /Visuals/Command/goal_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + current_pose_visualizer_cfg: + prim_path: /Visuals/Command/body_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + ee_pose_5: + class_type: isaaclab.envs.mdp.commands.pose_command:UniformPoseCommand + resampling_time_range: + - 4.0 + - 4.0 + debug_vis: true + asset_name: robot + body_name: TIP_B_5 + make_quat_unique: false + ranges: + pos_x: + - 0.0869 + - 0.0869 + pos_y: + - 0.04 + - 0.052 + pos_z: + - 0.17 + - 0.17248 + roll: + - 0.0 + - 0.0 + pitch: + - 0.0 + - 0.0 + yaw: + - 0.0 + - 0.0 + goal_pose_visualizer_cfg: + prim_path: /Visuals/Command/goal_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null + current_pose_visualizer_cfg: + prim_path: /Visuals/Command/body_pose + markers: + frame: + func: isaaclab.sim.spawners.from_files.from_files:spawn_from_usd + visible: true + semantic_tags: null + copy_from_source: true + mass_props: null + deformable_props: null + rigid_props: null + collision_props: null + activate_contact_sensors: false + scale: + - 0.02 + - 0.02 + - 0.02 + articulation_props: null + fixed_tendons_props: null + joint_drive_props: null + visual_material_path: material + visual_material: null + usd_path: http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.5/Isaac/Props/UIElements/frame_prim.usd + variants: null +agent: + seed: 42 + device: cuda:0 + num_steps_per_env: 24 + max_iterations: 1000 + empirical_normalization: false + policy: + class_name: ActorCritic + init_noise_std: 1.0 + noise_std_type: scalar + actor_hidden_dims: + - 64 + - 64 + critic_hidden_dims: + - 64 + - 64 + activation: elu + algorithm: + class_name: PPO + num_learning_epochs: 8 + num_mini_batches: 4 + learning_rate: 0.001 + schedule: adaptive + gamma: 0.99 + lam: 0.95 + entropy_coef: 0.01 + desired_kl: 0.01 + max_grad_norm: 1.0 + value_loss_coef: 1.0 + use_clipped_value_loss: true + clip_param: 0.2 + normalize_advantage_per_mini_batch: false + symmetry_cfg: null + rnd_cfg: null + clip_actions: null + save_interval: 50 + experiment_name: reach_humanoid_hand + run_name: '' + logger: tensorboard + neptune_project: isaaclab + wandb_project: isaaclab + resume: false + load_run: .* + load_checkpoint: model_.*.pt diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/hydra.yaml b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/hydra.yaml new file mode 100644 index 0000000..600800b --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/hydra.yaml @@ -0,0 +1,154 @@ +hydra: + run: + dir: outputs/${now:%Y-%m-%d}/${now:%H-%M-%S} + sweep: + dir: multirun/${now:%Y-%m-%d}/${now:%H-%M-%S} + subdir: ${hydra.job.num} + launcher: + _target_: hydra._internal.core_plugins.basic_launcher.BasicLauncher + sweeper: + _target_: hydra._internal.core_plugins.basic_sweeper.BasicSweeper + max_batch_size: null + params: null + help: + app_name: ${hydra.job.name} + header: '${hydra.help.app_name} is powered by Hydra. + + ' + footer: 'Powered by Hydra (https://hydra.cc) + + Use --hydra-help to view Hydra specific help + + ' + template: '${hydra.help.header} + + == Configuration groups == + + Compose your configuration from those groups (group=option) + + + $APP_CONFIG_GROUPS + + + == Config == + + Override anything in the config (foo.bar=value) + + + $CONFIG + + + ${hydra.help.footer} + + ' + hydra_help: + template: 'Hydra (${hydra.runtime.version}) + + See https://hydra.cc for more info. + + + == Flags == + + $FLAGS_HELP + + + == Configuration groups == + + Compose your configuration from those groups (For example, append hydra/job_logging=disabled + to command line) + + + $HYDRA_CONFIG_GROUPS + + + Use ''--cfg hydra'' to Show the Hydra config. + + ' + hydra_help: ??? + hydra_logging: + version: 1 + formatters: + simple: + format: '[%(asctime)s][HYDRA] %(message)s' + handlers: + console: + class: logging.StreamHandler + formatter: simple + stream: ext://sys.stdout + root: + level: INFO + handlers: + - console + loggers: + logging_example: + level: DEBUG + disable_existing_loggers: false + job_logging: + version: 1 + formatters: + simple: + format: '[%(asctime)s][%(name)s][%(levelname)s] - %(message)s' + handlers: + console: + class: logging.StreamHandler + formatter: simple + stream: ext://sys.stdout + file: + class: logging.FileHandler + formatter: simple + filename: ${hydra.runtime.output_dir}/${hydra.job.name}.log + root: + level: INFO + handlers: + - console + - file + disable_existing_loggers: false + env: {} + mode: RUN + searchpath: [] + callbacks: {} + output_subdir: .hydra + overrides: + hydra: + - hydra.mode=RUN + task: [] + job: + name: hydra + chdir: null + override_dirname: '' + id: ??? + num: ??? + config_name: Isaac-Reach-Humanoid-Hand-v0 + env_set: {} + env_copy: [] + config: + override_dirname: + kv_sep: '=' + item_sep: ',' + exclude_keys: [] + runtime: + version: 1.3.2 + version_base: '1.3' + cwd: /home/hy/Downloads/Humanoid_Wato/HumanoidRL + config_sources: + - path: hydra.conf + schema: pkg + provider: hydra + - path: isaaclab_tasks.utils + schema: pkg + provider: main + - path: '' + schema: structured + provider: schema + output_dir: /home/hy/Downloads/Humanoid_Wato/HumanoidRL/outputs/2025-07-02/23-27-01 + choices: + hydra/env: default + hydra/callbacks: null + hydra/job_logging: default + hydra/hydra_logging: default + hydra/hydra_help: default + hydra/help: default + hydra/sweeper: basic + hydra/launcher: basic + hydra/output: default + verbose: false diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/overrides.yaml b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/overrides.yaml new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/.hydra/overrides.yaml @@ -0,0 +1 @@ +[] diff --git a/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/hydra.log b/autonomy/simulation/Humanoid_RL/HumanoidRL/outputs/2025-07-02/23-27-01/hydra.log new file mode 100644 index 0000000..e69de29 diff --git a/autonomy/simulation/Humanoid_RL/ModelAssets/.thumbs/256x256/Hand.usd.png b/autonomy/simulation/Humanoid_RL/ModelAssets/.thumbs/256x256/Hand.usd.png new file mode 100644 index 0000000..6d01b17 Binary files /dev/null and b/autonomy/simulation/Humanoid_RL/ModelAssets/.thumbs/256x256/Hand.usd.png differ diff --git a/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_base.usd b/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_base.usd new file mode 100644 index 0000000..580fe86 Binary files /dev/null and b/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_base.usd differ diff --git a/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_physics.usd b/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_physics.usd new file mode 100644 index 0000000..96c77c4 Binary files /dev/null and b/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_physics.usd differ diff --git a/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_sensor.usd b/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_sensor.usd new file mode 100644 index 0000000..35a9fe2 Binary files /dev/null and b/autonomy/simulation/Humanoid_RL/ModelAssets/configuration/hand_sensor.usd differ diff --git a/autonomy/simulation/Humanoid_RL/ModelAssets/hand.usd b/autonomy/simulation/Humanoid_RL/ModelAssets/hand.usd new file mode 100644 index 0000000..2bc943c Binary files /dev/null and b/autonomy/simulation/Humanoid_RL/ModelAssets/hand.usd differ diff --git a/autonomy/simulation/Humanoid_RL/hand_test.py b/autonomy/simulation/Humanoid_RL/hand_test.py new file mode 100644 index 0000000..ee5ab78 --- /dev/null +++ b/autonomy/simulation/Humanoid_RL/hand_test.py @@ -0,0 +1,99 @@ +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR +from HumanoidRL.HumanoidRLPackage.HumanoidRLSetup.modelCfg.humanoid import HAND_CFG +from isaaclab.utils import configclass +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.managers import SceneEntityCfg +from isaaclab.assets import AssetBaseCfg +import isaaclab.sim as sim_utils +import torch +import argparse +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Humanoid Hand Testing") +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + + +@configclass +class HandSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg( + prim_path="/World/defaultGroundPlane", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), + ) + + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # table = AssetBaseCfg( + # prim_path="{ENV_REGEX_NS}/Table", + # spawn=sim_utils.UsdFileCfg( + # usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(1.0, 1.0, 1.0) + # ), + # ) + + robot = HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): + + robot = scene["robot"] + + robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=[".*"]) + + robot_entity_cfg.resolve(scene) + + sim_dt = sim.get_physics_dt() + + joint_position = robot.data.default_joint_pos.clone() + joint_vel = robot.data.default_joint_vel.clone() + robot.write_joint_state_to_sim(joint_position, joint_vel) + + while simulation_app.is_running(): + + # joint_position = robot.data.default_joint_pos.clone() + # print(joint_position) + # joint_vel = robot.data.default_joint_vel.clone() + # robot.write_joint_state_to_sim(joint_position, joint_vel) + + joint_position_list = [[1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]] + joint_position = torch.tensor(joint_position_list[0], device=sim.device) + + robot.reset() + joint_pos_des = joint_position.unsqueeze(0)[:, robot_entity_cfg.joint_ids].clone() + robot.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids) + scene.write_data_to_sim() + + sim.step() + + scene.update(sim_dt) + + +def main(): + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + + sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + + scene_cfg = HandSceneCfg(num_envs=1, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + + sim.reset() + + print("[INFO]: Setup complete...") + + run_simulator(sim, scene) + + +if __name__ == "__main__": + main() + simulation_app.close() + + +# (env_isaaclab) hy@hy-LOQ-15IRX9:~/Downloads/Humanoid$ PYTHONPATH=$(pwd) /home/hy/IsaacLab/isaaclab.sh -p hand_test.py diff --git a/autonomy/simulation/Humanoid_RL/models/reach_model.pt b/autonomy/simulation/Humanoid_RL/models/reach_model.pt new file mode 100644 index 0000000..400c9b9 Binary files /dev/null and b/autonomy/simulation/Humanoid_RL/models/reach_model.pt differ diff --git a/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/joint_space_test.py b/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/joint_space_test.py new file mode 100644 index 0000000..06bc1b8 --- /dev/null +++ b/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/joint_space_test.py @@ -0,0 +1,156 @@ +import argparse +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Robot Arm with Joint Space Control") +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +import torch + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg +from isaaclab.managers import SceneEntityCfg +from isaaclab.markers import VisualizationMarkers +from isaaclab.markers.config import FRAME_MARKER_CFG +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.math import subtract_frame_transforms + +## +# Pre-defined configs +## +from isaaclab_assets import UR10_CFG + + +@configclass +class TableTopSceneCfg(InteractiveSceneCfg): + """Configuration for a cart-pole scene.""" + + # ground plane + ground = AssetBaseCfg( + prim_path="/World/defaultGroundPlane", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), + ) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # mount + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(2.0, 2.0, 2.0) + ), + ) + + # articulation + robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): + + robot = scene["robot"] + + # Create controller + diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls") + diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=scene.num_envs, device=sim.device) + + # Markers + frame_marker_cfg = FRAME_MARKER_CFG.copy() + frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current")) + + # Specify robot-specific parameters + robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"]) + + # Resolving the scene entities + robot_entity_cfg.resolve(scene) + + # Obtain the frame index of the end-effector ; For a fixed base robot, the frame index is one less than the body index. This is because ; the root body is not included in the returned Jacobians. + if robot.is_fixed_base: + ee_jacobi_idx = robot_entity_cfg.body_ids[0] - 1 + else: + ee_jacobi_idx = robot_entity_cfg.body_ids[0] + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + count = 0 + joint_position_index = 0 + + while simulation_app.is_running(): + if count % 150 == 0: + # reset time + count = 0 + + # cycle between joint state + joint_pos = robot.data.default_joint_pos.clone() + joint_position_list = [ + [0.0, -0.712, 0.712, 0.0, 0.0, 0.0], + [0.0, -1.712, 1.712, 0.0, 0.0, 0.0], + [1.0, -1.712, 1.712, 0.0, 0.0, 0.0], + [0.0, 1.712, 1.712, 0.0, 0.0, 0.0], + ] + joint_position = torch.tensor(joint_position_list[joint_position_index], device=sim.device) + joint_vel = robot.data.default_joint_vel.clone() + + if joint_position_index >= len(joint_position_list) - 1: + joint_position_index = 0 + else: + joint_position_index += 1 + + robot.reset() + + joint_pos_des = joint_position.unsqueeze(0)[:, robot_entity_cfg.joint_ids].clone() + + # apply actions (Smooth movement) + robot.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids) + scene.write_data_to_sim() + + # perform step + sim.step() + + # update sim-time + count += 1 + + # update buffers + scene.update(sim_dt) + + # obtain quantities from simulation + ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7] + # update marker positions + ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]) + + +def main(): + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + + # Set main camera + sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + + # Design scene + scene_cfg = TableTopSceneCfg(num_envs=1, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + + print("[INFO]: Setup complete...") + + # Run the simulator + run_simulator(sim, scene) + + +if __name__ == "__main__": + main() + simulation_app.close() \ No newline at end of file diff --git a/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py b/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py new file mode 100644 index 0000000..79161cd --- /dev/null +++ b/autonomy/simulation/Wato_additional_scripts/robot_arm_controllers/task_space_test.py @@ -0,0 +1,187 @@ +import argparse +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Robot Arm with Task Space IK Control") +parser.add_argument("--robot", type=str, default="franka_panda", help="Name of the robot.") +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +import torch + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg +from isaaclab.managers import SceneEntityCfg +from isaaclab.markers import VisualizationMarkers +from isaaclab.markers.config import FRAME_MARKER_CFG +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.math import subtract_frame_transforms + +## +# Pre-defined configs +## +from isaaclab_assets import UR10_CFG, FRANKA_PANDA_HIGH_PD_CFG + + +@configclass +class TableTopSceneCfg(InteractiveSceneCfg): + """Configuration for a cart-pole scene.""" + + # ground plane + ground = AssetBaseCfg( + prim_path="/World/defaultGroundPlane", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), + ) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # mount + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(2.0, 2.0, 2.0) + ), + ) + + # Cube + cube = AssetBaseCfg( + prim_path="/World/cube", + spawn=sim_utils.CuboidCfg( + size=[0.1, 0.1, 0.1] + ), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.5, 0.0, 0.5)), + ) + + # articulation + if args_cli.robot == "franka_panda": + robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + elif args_cli.robot == "ur10": + robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + else: + raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10") + # robot = UR10_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): + + robot = scene["robot"] + + # Create controller + diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls") + diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=scene.num_envs, device=sim.device) + + # Markers + frame_marker_cfg = FRAME_MARKER_CFG.copy() + frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current")) + goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal")) + + # Specify robot-specific parameters + if args_cli.robot == "franka_panda": + robot_entity_cfg = SceneEntityCfg("robot", joint_names=["panda_joint.*"], body_names=["panda_hand"]) + elif args_cli.robot == "ur10": + robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"]) + else: + raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10") + # robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"]) + + # Resolving the scene entities + robot_entity_cfg.resolve(scene) + + # Obtain the frame index of the end-effector ; For a fixed base robot, the frame index is one less than the body index. This is because ; the root body is not included in the returned Jacobians. + if robot.is_fixed_base: + ee_jacobi_idx = robot_entity_cfg.body_ids[0] - 1 + else: + ee_jacobi_idx = robot_entity_cfg.body_ids[0] + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + + # May have to set initial position first + if args_cli.robot == "franka_panda": + joint_position = robot.data.default_joint_pos.clone() + joint_vel = robot.data.default_joint_vel.clone() + robot.write_joint_state_to_sim(joint_position, joint_vel) + else: + joint_position_index = 0 + joint_position_list = [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ] + joint_position = torch.tensor(joint_position_list[joint_position_index], device=sim.device) + joint_vel = robot.data.default_joint_vel.clone() + joint_pos_des = joint_position.unsqueeze(0)[:, robot_entity_cfg.joint_ids].clone() + robot.write_joint_state_to_sim(joint_pos_des, joint_vel) + + while simulation_app.is_running(): + # Get cube/target_point coordinates + position, quaternion = scene["cube"].get_world_poses() + ik_commands = torch.cat([position, quaternion], dim=1) # Quaternion is in (w, x, y, z) + + diff_ik_controller.set_command(ik_commands) + + # obtain quantities from simulation + jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, robot_entity_cfg.joint_ids] + ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7] + root_pose_w = robot.data.root_state_w[:, 0:7] + joint_pos = robot.data.joint_pos[:, robot_entity_cfg.joint_ids] + + # compute frame in root frame + ee_pos_b, ee_quat_b = subtract_frame_transforms( + root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] + ) + + # compute the joint commands + joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos) + + # apply actions (Smooth movement) + robot.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids) + scene.write_data_to_sim() + + # perform step + sim.step() + + # update buffers + scene.update(sim_dt) + + # obtain quantities from simulation + ee_pose_w = robot.data.body_state_w[:, robot_entity_cfg.body_ids[0], 0:7] + + # update marker positions + ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]) + goal_marker.visualize(ik_commands[:, 0:3] + scene.env_origins, ik_commands[:, 3:7]) + + +def main(): + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + + # Set main camera + sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + + # Design scene + scene_cfg = TableTopSceneCfg(num_envs=1, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + + print("[INFO]: Setup complete...") + + # Run the simulator + run_simulator(sim, scene) + + +if __name__ == "__main__": + main() + simulation_app.close() \ No newline at end of file diff --git a/autonomy/simulation/Wato_additional_scripts/shadow_hand/shadow_hand_joint_space_test.py b/autonomy/simulation/Wato_additional_scripts/shadow_hand/shadow_hand_joint_space_test.py new file mode 100644 index 0000000..200207c --- /dev/null +++ b/autonomy/simulation/Wato_additional_scripts/shadow_hand/shadow_hand_joint_space_test.py @@ -0,0 +1,139 @@ +import argparse +from isaaclab.app import AppLauncher + +parser = argparse.ArgumentParser(description="Robot Arm with Joint Space Control") +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() + +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +import torch + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg +from isaaclab.managers import SceneEntityCfg +from isaaclab.markers import VisualizationMarkers +from isaaclab.markers.config import FRAME_MARKER_CFG +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.math import subtract_frame_transforms + +from isaaclab_assets import SHADOW_HAND_CFG + +# Shadow hand has 24 DOF - found from print(robot_entity_cfg.joint_ids) +# Print joint position - robot.data.default_joint_pos + + +@configclass +class TableTopSceneCfg(InteractiveSceneCfg): + """Configuration for a cart-pole scene.""" + + # ground plane + ground = AssetBaseCfg( + prim_path="/World/defaultGroundPlane", + spawn=sim_utils.GroundPlaneCfg(), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)), + ) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # mount + # table = AssetBaseCfg( + # prim_path="{ENV_REGEX_NS}/Table", + # spawn=sim_utils.UsdFileCfg( + # usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(2.0, 2.0, 2.0) + # ), + # ) + + # articulation + robot = SHADOW_HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): + + robot = scene["robot"] + + # Specify robot-specific parameters + robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=[".*"]) + + # Resolving the scene entities + robot_entity_cfg.resolve(scene) + + # Obtain the frame index of the end-effector ; For a fixed base robot, the frame index is one less than the body index. This is because ; the root body is not included in the returned Jacobians. + if robot.is_fixed_base: + ee_jacobi_idx = robot_entity_cfg.body_ids[0] - 1 + else: + ee_jacobi_idx = robot_entity_cfg.body_ids[0] + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + count = 0 + joint_position_index = 0 + + while simulation_app.is_running(): + if count % 150 == 0: + # reset time + count = 0 + + # cycle between joint state + joint_position_list = [ + [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], + [1., 1., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], + [0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 1., 0., 0., 0., 0., 0.] + ] + joint_position = torch.tensor(joint_position_list[joint_position_index], device=sim.device) + + if joint_position_index >= len(joint_position_list) - 1: + joint_position_index = 0 + else: + joint_position_index += 1 + + robot.reset() + + joint_pos_des = joint_position.unsqueeze(0)[:, robot_entity_cfg.joint_ids].clone() + + # apply actions (Smooth movement) + robot.set_joint_position_target(joint_pos_des, joint_ids=robot_entity_cfg.joint_ids) + scene.write_data_to_sim() + + # perform step + sim.step() + + # update sim-time + count += 1 + + # update buffers + scene.update(sim_dt) + + +def main(): + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + + # Set main camera + sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + + # Design scene + scene_cfg = TableTopSceneCfg(num_envs=1, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + + print("[INFO]: Setup complete...") + + # Run the simulator + run_simulator(sim, scene) + + +if __name__ == "__main__": + main() + simulation_app.close() \ No newline at end of file diff --git a/autonomy/simulation/package.xml b/autonomy/simulation/package.xml new file mode 100644 index 0000000..39d2bb3 --- /dev/null +++ b/autonomy/simulation/package.xml @@ -0,0 +1,19 @@ + + + simulation + 0.0.0 + Simulation folder for the WATO Humanoid project. + + Wilson Cheng + Apache2.0 + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/autonomy/wato_msgs/common_msgs/CMakeLists.txt b/autonomy/wato_msgs/common_msgs/CMakeLists.txt new file mode 100644 index 0000000..de77874 --- /dev/null +++ b/autonomy/wato_msgs/common_msgs/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.10) +project(common_msgs) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +set(msg_files + msg/ArmPose.msg + msg/HandPose.msg + msg/JointState.msg + ) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + DEPENDENCIES std_msgs geometry_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/autonomy/wato_msgs/common_msgs/msg/ArmPose.msg b/autonomy/wato_msgs/common_msgs/msg/ArmPose.msg new file mode 100644 index 0000000..24a9bfd --- /dev/null +++ b/autonomy/wato_msgs/common_msgs/msg/ArmPose.msg @@ -0,0 +1,11 @@ +std_msgs/Header header + +string name + +common_msgs/JointState shoulder +common_msgs/JointState elbow +common_msgs/JointState wrist + +# Optional Overall Hand Pose +bool include_hand_pose +common_msgs/HandPose hand_pose \ No newline at end of file diff --git a/autonomy/wato_msgs/common_msgs/msg/HandPose.msg b/autonomy/wato_msgs/common_msgs/msg/HandPose.msg new file mode 100644 index 0000000..72d6921 --- /dev/null +++ b/autonomy/wato_msgs/common_msgs/msg/HandPose.msg @@ -0,0 +1,28 @@ +std_msgs/Header header + +string name + +# Thumb +common_msgs/JointState thumb_cmc +common_msgs/JointState thumb_mcp +common_msgs/JointState thumb_ip + +# Index +common_msgs/JointState index_mcp +common_msgs/JointState index_pip +common_msgs/JointState index_dip + +# Middle +common_msgs/JointState middle_mcp +common_msgs/JointState middle_pip +common_msgs/JointState middle_dip + +# Ring +common_msgs/JointState ring_mcp +common_msgs/JointState ring_pip +common_msgs/JointState ring_dip + +# Pinky +common_msgs/JointState pinky_mcp +common_msgs/JointState pinky_pip +common_msgs/JointState pinky_dip \ No newline at end of file diff --git a/autonomy/wato_msgs/common_msgs/msg/JointState.msg b/autonomy/wato_msgs/common_msgs/msg/JointState.msg new file mode 100644 index 0000000..3d141ff --- /dev/null +++ b/autonomy/wato_msgs/common_msgs/msg/JointState.msg @@ -0,0 +1,9 @@ +std_msgs/Header header + +string name + +float64[] position +float64[] velocity +float64[] effort + +geometry_msgs/Quaternion orientation \ No newline at end of file diff --git a/autonomy/wato_msgs/common_msgs/package.xml b/autonomy/wato_msgs/common_msgs/package.xml new file mode 100644 index 0000000..087bd4f --- /dev/null +++ b/autonomy/wato_msgs/common_msgs/package.xml @@ -0,0 +1,21 @@ + + + common_msgs + 0.0.0 + Common ROS2 message definitions for the WATO Humanoid project. + + Gavin Tranquilino + Apache2.0 + + ament_cmake + std_msgs + geometry_msgs + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/wato_msgs/sample_msgs/CMakeLists.txt b/autonomy/wato_msgs/sample_msgs/CMakeLists.txt similarity index 93% rename from src/wato_msgs/sample_msgs/CMakeLists.txt rename to autonomy/wato_msgs/sample_msgs/CMakeLists.txt index 008fc0e..1bd6487 100644 --- a/src/wato_msgs/sample_msgs/CMakeLists.txt +++ b/autonomy/wato_msgs/sample_msgs/CMakeLists.txt @@ -17,7 +17,8 @@ set(msg_files msg/Filtered.msg msg/FilteredArray.msg msg/Unfiltered.msg - msg/Metadata.msg) + msg/Metadata.msg + msg/HandPose.msg) rosidl_generate_interfaces(sample_msgs ${msg_files} DEPENDENCIES std_msgs) diff --git a/src/wato_msgs/sample_msgs/msg/Filtered.msg b/autonomy/wato_msgs/sample_msgs/msg/Filtered.msg similarity index 100% rename from src/wato_msgs/sample_msgs/msg/Filtered.msg rename to autonomy/wato_msgs/sample_msgs/msg/Filtered.msg diff --git a/src/wato_msgs/sample_msgs/msg/FilteredArray.msg b/autonomy/wato_msgs/sample_msgs/msg/FilteredArray.msg similarity index 100% rename from src/wato_msgs/sample_msgs/msg/FilteredArray.msg rename to autonomy/wato_msgs/sample_msgs/msg/FilteredArray.msg diff --git a/autonomy/wato_msgs/sample_msgs/msg/HandPose.msg b/autonomy/wato_msgs/sample_msgs/msg/HandPose.msg new file mode 100644 index 0000000..c5fd6c8 --- /dev/null +++ b/autonomy/wato_msgs/sample_msgs/msg/HandPose.msg @@ -0,0 +1 @@ +float32[] data \ No newline at end of file diff --git a/src/wato_msgs/sample_msgs/msg/Metadata.msg b/autonomy/wato_msgs/sample_msgs/msg/Metadata.msg similarity index 100% rename from src/wato_msgs/sample_msgs/msg/Metadata.msg rename to autonomy/wato_msgs/sample_msgs/msg/Metadata.msg diff --git a/src/wato_msgs/sample_msgs/msg/Unfiltered.msg b/autonomy/wato_msgs/sample_msgs/msg/Unfiltered.msg similarity index 100% rename from src/wato_msgs/sample_msgs/msg/Unfiltered.msg rename to autonomy/wato_msgs/sample_msgs/msg/Unfiltered.msg diff --git a/src/wato_msgs/sample_msgs/package.xml b/autonomy/wato_msgs/sample_msgs/package.xml similarity index 100% rename from src/wato_msgs/sample_msgs/package.xml rename to autonomy/wato_msgs/sample_msgs/package.xml diff --git a/config/wato_asd_training_foxglove_config .json b/config/wato_asd_training_foxglove_config .json index e9ea578..fbfb313 100644 --- a/config/wato_asd_training_foxglove_config .json +++ b/config/wato_asd_training_foxglove_config .json @@ -136,7 +136,7 @@ "instanceId": "9e830917-d6fe-4f25-8e7b-25afa5a47d21", "layerId": "foxglove.Urdf", "sourceType": "url", - "url": "https://raw.githubusercontent.com/WATonomous/wato_asd_training/refs/heads/main/src/gazebo/launch/robot.urdf", + "url": "https://raw.githubusercontent.com/WATonomous/wato_asd_training/refs/heads/main/autonomy/gazebo/launch/robot.urdf", "filePath": "", "parameter": "", "topic": "", @@ -174,7 +174,7 @@ "instanceId": "cadf14d8-072e-4af6-8fd0-60bf81d00399", "layerId": "foxglove.Urdf", "sourceType": "url", - "url": "https://raw.githubusercontent.com/WATonomous/wato_asd_training/refs/heads/main/src/gazebo/launch/env.urdf", + "url": "https://raw.githubusercontent.com/WATonomous/wato_asd_training/refs/heads/main/autonomy/gazebo/launch/env.urdf", "filePath": "", "parameter": "", "topic": "", diff --git a/docker/base/inject_ros2.Dockerfile b/docker/base/inject_ros2.Dockerfile new file mode 100644 index 0000000..b405e5f --- /dev/null +++ b/docker/base/inject_ros2.Dockerfile @@ -0,0 +1,70 @@ +ARG GENERIC_IMAGE + +########################## Install ROS2 Core ########################## +FROM ${GENERIC_IMAGE} as core + +# setup timezone +RUN echo 'Etc/UTC' > /etc/timezone && \ + ln -s /usr/share/zoneinfo/Etc/UTC /etc/localtime && \ + apt-get update && \ + apt-get install -q -y --no-install-recommends tzdata && \ + rm -rf /var/lib/apt/lists/* + +# install packages +RUN apt-get update && apt-get install -q -y --no-install-recommends \ + dirmngr \ + gnupg2 \ + software-properties-common \ + curl \ + && rm -rf /var/lib/apt/lists/* + +# setup keys and sources.list +RUN add-apt-repository universe +RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg +RUN echo "deb [arch=$(dpkg --print-architecture) \ + signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \ + http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) \ + main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null + +# setup environment +ENV LANG C.UTF-8 +ENV LC_ALL C.UTF-8 + +ARG ROS_DISTRO +ENV ROS_DISTRO=${ROS_DISTRO} + +# install ros2 core packages +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-$ROS_DISTRO-ros-core \ + && rm -rf /var/lib/apt/lists/* + +######### Install ROS2 Developer Tools (rosdep, colcon, vcstools) ######### +FROM core as devel + +# install bootstrap tools +RUN apt-get update && apt-get install --no-install-recommends -y \ + build-essential \ + git \ + python3-colcon-common-extensions \ + python3-colcon-mixin \ + python3-rosdep \ + python3-vcstool \ + && rm -rf /var/lib/apt/lists/* + +# bootstrap rosdep +RUN rosdep init && \ + rosdep update --rosdistro $ROS_DISTRO + +# setup colcon mixin and metadata +RUN colcon mixin add default \ + https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml && \ + colcon mixin update && \ + colcon metadata add default \ + https://raw.githubusercontent.com/colcon/colcon-metadata-repository/master/index.yaml && \ + colcon metadata update + +# install ros2 base packages +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-$ROS_DISTRO-ros-base \ + && rm -rf /var/lib/apt/lists/* diff --git a/docker/base/inject_wato_base.Dockerfile b/docker/base/inject_wato_base.Dockerfile new file mode 100644 index 0000000..61c0d89 --- /dev/null +++ b/docker/base/inject_wato_base.Dockerfile @@ -0,0 +1,38 @@ +ARG GENERIC_IMAGE + +########### Setup WATO Tools and ENV (eg. AMENT_WS, apt-fast) ########### +# This stage can be appended on any publicly available base image to make it ready +# for the wato_monorepo. +FROM ${GENERIC_IMAGE} as wato_base + +RUN DEBIAN_FRONTEND=noninteractive apt-get update && \ + apt-get install -y curl sudo && \ + rm -rf /var/lib/apt/lists/* + +ENV USER="bolty" +ENV AMENT_WS=/home/${USER}/ament_ws + +# User Setup +RUN DEBIAN_FRONTEND=noninteractive apt-get update && apt-get install -y curl sudo && \ + rm -rf /var/lib/apt/lists/* + +# Add a user so that created files in the docker container are owned by a non-root user (for prod) +ARG USER_PASSWD +RUN addgroup --gid 1000 ${USER} && \ + useradd -rm -d /home/${USER} -s /bin/bash -g ${USER} -G sudo -u 1000 ${USER} -p "$(openssl passwd -6 $USER_PASSWD)" + +# install apt-fast +RUN DEBIAN_FRONTEND=noninteractive apt-get -qq update && \ + apt-get install -qq -y wget && \ + mkdir -p /etc/apt/keyrings && \ + wget -O - "https://keyserver.ubuntu.com/pks/lookup?op=get&search=0xA2166B8DE8BDC3367D1901C11EE2FF37CA8DA16B" \ + | gpg --dearmor -o /etc/apt/keyrings/apt-fast.gpg && \ + echo "deb [signed-by=/etc/apt/keyrings/apt-fast.gpg] \ + http://ppa.launchpad.net/apt-fast/stable/ubuntu focal main" > /etc/apt/sources.list.d/apt-fast.list && \ + DEBIAN_FRONTEND=noninteractive apt-get update -qq && DEBIAN_FRONTEND=noninteractive apt-get install -qq -y apt-fast && \ + echo debconf apt-fast/maxdownloads string 16 | debconf-set-selections && \ + echo debconf apt-fast/dlflag boolean true | debconf-set-selections && \ + echo debconf apt-fast/aptmanager string apt-get | debconf-set-selections + +# sources ament_ws automatically when we start a terminal inside the container +RUN echo "source /home/$USER/ament_ws/install/setup.bash" >> ~/.bashrc diff --git a/docker/robot/robot.Dockerfile b/docker/controller/controller.Dockerfile similarity index 86% rename from docker/robot/robot.Dockerfile rename to docker/controller/controller.Dockerfile index 1685d40..bd9c425 100644 --- a/docker/robot/robot.Dockerfile +++ b/docker/controller/controller.Dockerfile @@ -6,12 +6,8 @@ FROM ${BASE_IMAGE} AS source WORKDIR ${AMENT_WS}/src # Copy in source code -COPY src/robot/odometry_spoof odometry_spoof -COPY src/robot/costmap costmap -COPY src/robot/map_memory map_memory -COPY src/robot/planner planner -COPY src/robot/control control -COPY src/robot/bringup_robot bringup_robot +COPY autonomy/controller controller +COPY autonomy/wato_msgs wato_msgs # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ @@ -23,8 +19,6 @@ RUN apt-get -qq update && rosdep update && \ ################################# Dependencies ################################ FROM ${BASE_IMAGE} AS dependencies -# ADD MORE DEPENDENCIES HERE - # Install Rosdep requirements COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list) diff --git a/docker/vis_tools/foxglove.Dockerfile b/docker/infrastructure/foxglove/foxglove.Dockerfile similarity index 98% rename from docker/vis_tools/foxglove.Dockerfile rename to docker/infrastructure/foxglove/foxglove.Dockerfile index 45d65c5..657d13a 100644 --- a/docker/vis_tools/foxglove.Dockerfile +++ b/docker/infrastructure/foxglove/foxglove.Dockerfile @@ -6,7 +6,7 @@ FROM ${BASE_IMAGE} AS source WORKDIR ${AMENT_WS}/src # Copy in source code -COPY src/wato_msgs wato_msgs +COPY autonomy/wato_msgs wato_msgs # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ diff --git a/docker/samples/cpp_aggregator.Dockerfile b/docker/samples/cpp_aggregator.Dockerfile index 2f52d0a..42b9339 100644 --- a/docker/samples/cpp_aggregator.Dockerfile +++ b/docker/samples/cpp_aggregator.Dockerfile @@ -6,8 +6,8 @@ FROM ${BASE_IMAGE} AS source WORKDIR ${AMENT_WS}/src # Copy in source code -COPY src/samples/cpp/aggregator aggregator -COPY src/wato_msgs/sample_msgs sample_msgs +COPY autonomy/samples/cpp/aggregator aggregator +COPY autonomy/wato_msgs/sample_msgs sample_msgs # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ diff --git a/docker/samples/cpp_producer.Dockerfile b/docker/samples/cpp_producer.Dockerfile index 3a87d5d..33f3552 100644 --- a/docker/samples/cpp_producer.Dockerfile +++ b/docker/samples/cpp_producer.Dockerfile @@ -6,8 +6,8 @@ FROM ${BASE_IMAGE} AS source WORKDIR ${AMENT_WS}/src # Copy in source code -COPY src/samples/cpp/producer producer -COPY src/wato_msgs/sample_msgs sample_msgs +COPY autonomy/samples/cpp/producer producer +COPY autonomy/wato_msgs/sample_msgs sample_msgs # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ diff --git a/docker/samples/cpp_transformer.Dockerfile b/docker/samples/cpp_transformer.Dockerfile index 3ba7394..9929f2c 100644 --- a/docker/samples/cpp_transformer.Dockerfile +++ b/docker/samples/cpp_transformer.Dockerfile @@ -6,8 +6,8 @@ FROM ${BASE_IMAGE} AS source WORKDIR ${AMENT_WS}/src # Copy in source code -COPY src/samples/cpp/transformer transformer -COPY src/wato_msgs/sample_msgs sample_msgs +COPY autonomy/samples/cpp/transformer transformer +COPY autonomy/wato_msgs/sample_msgs sample_msgs # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ diff --git a/docker/samples/py_aggregator.Dockerfile b/docker/samples/py_aggregator.Dockerfile index a4c2008..bbd4051 100644 --- a/docker/samples/py_aggregator.Dockerfile +++ b/docker/samples/py_aggregator.Dockerfile @@ -6,8 +6,8 @@ FROM ${BASE_IMAGE} AS source WORKDIR ${AMENT_WS}/src # Copy in source code -COPY src/samples/python/aggregator aggregator -COPY src/wato_msgs/sample_msgs sample_msgs +COPY autonomy/samples/python/aggregator aggregator +COPY autonomy/wato_msgs/sample_msgs sample_msgs # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ diff --git a/docker/samples/py_producer.Dockerfile b/docker/samples/py_producer.Dockerfile index 33380b9..ae46da4 100644 --- a/docker/samples/py_producer.Dockerfile +++ b/docker/samples/py_producer.Dockerfile @@ -6,8 +6,8 @@ FROM ${BASE_IMAGE} AS source WORKDIR ${AMENT_WS}/src # Copy in source code -COPY src/samples/python/producer producer -COPY src/wato_msgs/sample_msgs sample_msgs +COPY autonomy/samples/python/producer producer +COPY autonomy/wato_msgs/sample_msgs sample_msgs # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ diff --git a/docker/samples/py_transformer.Dockerfile b/docker/samples/py_transformer.Dockerfile index 50b8f00..51f1810 100644 --- a/docker/samples/py_transformer.Dockerfile +++ b/docker/samples/py_transformer.Dockerfile @@ -6,8 +6,8 @@ FROM ${BASE_IMAGE} AS source WORKDIR ${AMENT_WS}/src # Copy in source code -COPY src/samples/python/transformer transformer -COPY src/wato_msgs/sample_msgs sample_msgs +COPY autonomy/samples/python/transformer transformer +COPY autonomy/wato_msgs/sample_msgs sample_msgs # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ diff --git a/docker/gazebo/gazeboserver.Dockerfile b/docker/simulation/isaac_sim/isaac_sim.Dockerfile similarity index 66% rename from docker/gazebo/gazeboserver.Dockerfile rename to docker/simulation/isaac_sim/isaac_sim.Dockerfile index f445ffa..d391542 100644 --- a/docker/gazebo/gazeboserver.Dockerfile +++ b/docker/simulation/isaac_sim/isaac_sim.Dockerfile @@ -1,4 +1,4 @@ -ARG BASE_IMAGE=ghcr.io/watonomous/robot_base/base:humble-ubuntu22.04 +ARG BASE_IMAGE=nvcr.io/nvidia/isaac-sim:4.2.0 ################################ Source ################################ FROM ${BASE_IMAGE} AS source @@ -6,7 +6,8 @@ FROM ${BASE_IMAGE} AS source WORKDIR ${AMENT_WS}/src # Copy in source code -COPY src/gazebo gazebo +COPY autonomy/samples/cpp/aggregator aggregator +COPY autonomy/wato_msgs/sample_msgs sample_msgs # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ @@ -18,16 +19,6 @@ RUN apt-get -qq update && rosdep update && \ ################################# Dependencies ################################ FROM ${BASE_IMAGE} AS dependencies -# RUN apt-key adv --keyserver keyserver.ubuntu.com --recv-keys A4B469963BF863CC -RUN apt-get update && apt-get install ffmpeg libsm6 libxext6 -y - -RUN apt install -y lsb-release wget gnupg -RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg -RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null -RUN apt-get -y update -RUN apt-get -y install ros-${ROS_DISTRO}-ros-gz ignition-fortress -RUN echo $GAZEBO_PLUGIN_PATH=/opt/ros/humble/lib - # Install Rosdep requirements COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list) diff --git a/docs/Architecture_Map.odg b/docs/Architecture_Map.odg new file mode 100644 index 0000000..a9eadb7 Binary files /dev/null and b/docs/Architecture_Map.odg differ diff --git a/docs/Architecture_Map.pdf b/docs/Architecture_Map.pdf new file mode 100644 index 0000000..4fa3780 Binary files /dev/null and b/docs/Architecture_Map.pdf differ diff --git a/docs/README.md b/docs/README.md new file mode 100644 index 0000000..ff315db --- /dev/null +++ b/docs/README.md @@ -0,0 +1,38 @@ +## Documenting: +Documentation should be used as a blueprint for development, then filled in when specifics are established. + +The documentation of this repository follows a package level scheme. As shown below: +``` +samples +├── README.md ──────────────> overview(component) +├── cpp +│ ├── producer +│ │ ├── README.md ──────> most importantly(package) +│ │ ├── CMakeLists.txt +│ │ ├── config +│ │ ├── include +│ │ ├── launch +│ │ ├── package.xml +│ │ ├── src +│ │ └── test +├── python +├── sample_msgs +└── samples_diagram.svg +``` + +### Component Level +- Overview of component as a whole + +### Package Level +- **Purpose** – What the package does and its role within the component. +- **Inputs & Outputs** – Data flow, including message types, service calls, or file interactions. +- **Key Features** – Key classes, nodes, or scripts, along with their relationships. +- **Usage** – How to build, run, and test the package. +- **Configuration** – Relevant parameters, environment variables, or dependencies. + +### System Architecture +The system architecture can be viewed in this [document](Architecture_Map.pdf), along with the .odg file (use libre draw to edit). + +### Infrastructure Documentation +1. [Project Infrastructure Development Docs](https://github.com/WATonomous/wato_monorepo/tree/main/docs/dev/) +2. [ROS Structure Docs](src/samples/README.md) \ No newline at end of file diff --git a/embedded/README.md b/embedded/README.md new file mode 100644 index 0000000..e69de29 diff --git a/modules/docker-compose.controller.yaml b/modules/docker-compose.controller.yaml new file mode 100644 index 0000000..f2a186a --- /dev/null +++ b/modules/docker-compose.controller.yaml @@ -0,0 +1,16 @@ + +services: + controller: + build: &controller_build + context: .. + dockerfile: docker/controller/controller.Dockerfile + cache_from: + - "${CONTROLLER_IMAGE:?}:${TAG}" + - "${CONTROLLER_IMAGE:?}:main" + args: + BASE_IMAGE: ${BASE_IMAGE_OVERRIDE-} + image: "${CONTROLLER_IMAGE:?}:${TAG}" + profiles: [deploy] + command: /bin/bash -c "ros2 launch controller launch.py" + volumes: + - ${MONO_DIR}/autonomy/controller/:/root/ament_ws/src/controller/ \ No newline at end of file diff --git a/modules/docker-compose.gazebo.yaml b/modules/docker-compose.gazebo.yaml deleted file mode 100644 index 5cbb82d..0000000 --- a/modules/docker-compose.gazebo.yaml +++ /dev/null @@ -1,19 +0,0 @@ -services: - gazeboserver: - build: - context: .. - dockerfile: docker/gazebo/gazeboserver.Dockerfile - cache_from: - - "${GAZEBO_SERVER_IMAGE:?}:${TAG}" - - "${GAZEBO_SERVER_IMAGE:?}:main" - args: - BASE_IMAGE: ${BASE_IMAGE_OVERRIDE-} - image: "${GAZEBO_SERVER_IMAGE:?}:${TAG}" - ipc: host - tty: true - ports: - - "${GAZEBO_PORT:?}:9002" - profiles: [deploy, develop] - command: /bin/bash -c "ros2 launch gazebo sim.launch.py" - volumes: - - ../src/gazebo/launch:/root/src/gazebo/launch diff --git a/modules/docker-compose.vis_tools.yaml b/modules/docker-compose.infrastructure.yaml similarity index 77% rename from modules/docker-compose.vis_tools.yaml rename to modules/docker-compose.infrastructure.yaml index b3d74d5..13573ce 100644 --- a/modules/docker-compose.vis_tools.yaml +++ b/modules/docker-compose.infrastructure.yaml @@ -1,15 +1,18 @@ +version: "3.8" + services: foxglove: build: context: .. - dockerfile: docker/vis_tools/foxglove.Dockerfile + dockerfile: docker/infrastructure/foxglove/foxglove.Dockerfile cache_from: - "${INFRASTRUCTURE_FOXGLOVE_IMAGE:?}:${TAG}" - "${INFRASTRUCTURE_FOXGLOVE_IMAGE:?}:main" + target: deploy args: BASE_IMAGE: ${BASE_IMAGE_OVERRIDE-} image: "${INFRASTRUCTURE_FOXGLOVE_IMAGE:?}:${TAG}" - profiles: [deploy, develop] + # profiles: [deploy, develop] command: ["ros2", "launch", "foxglove_bridge", "foxglove_bridge_launch.xml", "port:=${FOXGLOVE_BRIDGE_PORT:?}"] ports: - "${FOXGLOVE_BRIDGE_PORT:?}:${FOXGLOVE_BRIDGE_PORT:?}" diff --git a/modules/docker-compose.robot.yaml b/modules/docker-compose.robot.yaml deleted file mode 100644 index 1c8eba7..0000000 --- a/modules/docker-compose.robot.yaml +++ /dev/null @@ -1,23 +0,0 @@ -services: - robot: - build: &robot_build - context: .. - dockerfile: docker/robot/robot.Dockerfile - cache_from: - - "${ROBOT_IMAGE:?}:${TAG}" - - "${ROBOT_IMAGE:?}:main" - args: - BASE_IMAGE: ${BASE_IMAGE_OVERRIDE-} - image: "${ROBOT_IMAGE:?}:${TAG}" - profiles: - - deploy - command: /bin/bash -c "ros2 launch bringup_robot robot.launch.py" - - robot_dev: - build: *robot_build - image: "${ROBOT_IMAGE:?}:dev_${TAG}" - command: tail -F anything - profiles: - - develop - volumes: - - ${MONO_DIR}/src/robot:/root/ament_ws/src diff --git a/modules/docker-compose.samples.yaml b/modules/docker-compose.samples.yaml index c3cb810..619bd8b 100644 --- a/modules/docker-compose.samples.yaml +++ b/modules/docker-compose.samples.yaml @@ -10,7 +10,7 @@ services: # profiles: [deploy] # command: /bin/bash -c "ros2 launch aggregator aggregator.launch.py" # volumes: - # - ${MONO_DIR}/src/samples/cpp/aggregator:/root/ament_ws/src/aggregator + # - ${MONO_DIR}/autonomy/samples/cpp/aggregator:/root/ament_ws/src/aggregator aggregator: # PYTHON build: &aggregator_build @@ -25,7 +25,7 @@ services: profiles: [deploy] command: /bin/bash -c "ros2 launch aggregator aggregator.launch.py" volumes: - - ${MONO_DIR}/src/samples/python/aggregator:/root/ament_ws/src/aggregator + - ${MONO_DIR}/autonomy/samples/python/aggregator:/root/ament_ws/src/aggregator producer: # C++ build: &producer_build @@ -40,7 +40,7 @@ services: profiles: [deploy] command: /bin/bash -c "ros2 launch producer producer.launch.py" volumes: - - ${MONO_DIR}/src/samples/cpp/producer:/root/ament_ws/src/producer + - ${MONO_DIR}/autonomy/samples/cpp/producer:/root/ament_ws/src/producer # producer: # PYTHON # build: &producer_build @@ -53,7 +53,7 @@ services: # profiles: [deploy] # command: /bin/bash -c "ros2 launch producer producer.launch.py" # volumes: - # - ${MONO_DIR}/src/samples/python/producer:/root/ament_ws/src/producer + # - ${MONO_DIR}/autonomy/samples/python/producer:/root/ament_ws/src/producer transformer: # C++ build: &transformer_build @@ -68,7 +68,7 @@ services: profiles: [deploy] command: /bin/bash -c "ros2 launch transformer transformer.launch.py" volumes: - - ${MONO_DIR}/src/samples/cpp/transformer:/root/ament_ws/src/transformer + - ${MONO_DIR}/autonomy/samples/cpp/transformer:/root/ament_ws/src/transformer # transformer: # PYTHON # build: &transformer_build @@ -81,7 +81,7 @@ services: # profiles: [deploy] # command: /bin/bash -c "ros2 launch transformer transformer.launch.py" # volumes: - # - ${MONO_DIR}/src/samples/python/transformer:/root/ament_ws/src/transformer + # - ${MONO_DIR}/autonomy/samples/python/transformer:/root/ament_ws/src/transformer aggregator_dev: build: *aggregator_build @@ -89,8 +89,8 @@ services: command: tail -F anything profiles: [develop] volumes: - - ${MONO_DIR}/src/samples/python/aggregator:/root/ament_ws/src/python/aggregator - - ${MONO_DIR}/src/samples/cpp/aggregator:/root/ament_ws/src/cpp/aggregator + - ${MONO_DIR}/autonomy/samples/python/aggregator:/root/ament_ws/src/python/aggregator + - ${MONO_DIR}/autonomy/samples/cpp/aggregator:/root/ament_ws/src/cpp/aggregator producer_dev: build: *producer_build @@ -98,8 +98,8 @@ services: command: tail -F anything profiles: [develop] volumes: - - ${MONO_DIR}/src/samples/python/producer:/root/ament_ws/src/python/producer - - ${MONO_DIR}/src/samples/cpp/producer:/root/ament_ws/src/cpp/producer + - ${MONO_DIR}/autonomy/samples/python/producer:/root/ament_ws/src/python/producer + - ${MONO_DIR}/autonomy/samples/cpp/producer:/root/ament_ws/src/cpp/producer transformer_dev: build: *transformer_build @@ -107,5 +107,5 @@ services: command: tail -F anything profiles: [develop] volumes: - - ${MONO_DIR}/src/samples/python/transformer:/root/ament_ws/src/python/transformer - - ${MONO_DIR}/src/samples/cpp/transformer:/root/ament_ws/src/cpp/transformer + - ${MONO_DIR}/autonomy/samples/python/transformer:/root/ament_ws/src/python/transformer + - ${MONO_DIR}/autonomy/samples/cpp/transformer:/root/ament_ws/src/cpp/transformer diff --git a/modules_future/docker-compose.interfacing.yaml b/modules_future/docker-compose.interfacing.yaml new file mode 100644 index 0000000..e69de29 diff --git a/modules_future/docker-compose.perception.yaml b/modules_future/docker-compose.perception.yaml new file mode 100644 index 0000000..e69de29 diff --git a/modules_future/docker-compose.simulation.yaml b/modules_future/docker-compose.simulation.yaml new file mode 100644 index 0000000..e69de29 diff --git a/src/gazebo/CMakeLists.txt b/src/gazebo/CMakeLists.txt deleted file mode 100644 index 473bca9..0000000 --- a/src/gazebo/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(gazebo) - -find_package(ament_cmake REQUIRED) - -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME}) - -ament_package() diff --git a/src/gazebo/launch/env.urdf b/src/gazebo/launch/env.urdf deleted file mode 100644 index 3e81665..0000000 --- a/src/gazebo/launch/env.urdf +++ /dev/null @@ -1,147 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/gazebo/launch/robot.urdf b/src/gazebo/launch/robot.urdf deleted file mode 100644 index 27a1b1d..0000000 --- a/src/gazebo/launch/robot.urdf +++ /dev/null @@ -1,53 +0,0 @@ - - - - - - - - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - - - - - - - - - - - 1.0 0.0 0.0 1 - 1.0 0.0 0.0 1 - 1.0 0.0 0.0 1 - - - - - - - - - - - 1.0 0.0 0.0 1 - 1.0 0.0 0.0 1 - 1.0 0.0 0.0 1 - - - - - - - - - - - 0.0 1 0.0 1 - 0.0 1 0.0 1 - 0.0 1 0.0 1 - - - - \ No newline at end of file diff --git a/src/gazebo/launch/robot_env.sdf b/src/gazebo/launch/robot_env.sdf deleted file mode 100644 index 8a050ff..0000000 --- a/src/gazebo/launch/robot_env.sdf +++ /dev/null @@ -1,949 +0,0 @@ - - - - - 0.001 - 1.0 - - - - - - - - - - - ogre2 - - - - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - 0.25 - 25000 - - - - - - - floating - 5 - 5 - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - - - - - false - 5 - 5 - floating - false - - - - - false - 5 - 5 - floating - false - - - - - - - - - - false - 5 - 5 - floating - false - - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - true - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - - - - - - false - 0 - 0 - 250 - 50 - floating - false - #666666 - - - - - - - false - 250 - 0 - 150 - 50 - floating - false - #666666 - - - - - - - false - 0 - 50 - 250 - 50 - floating - false - #777777 - - - - - - - false - 250 - 50 - 50 - 50 - floating - false - #777777 - - - - - - - false - 300 - 50 - 50 - 50 - floating - false - #777777 - - - - true - false - 4000000 - - - - - - - docked_collapsed - - - - - - - docked_collapsed - - - - - - - docked_collapsed - - - - - - - - - - true - 0 0 10 0 0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 0.1 -0.9 - - - - - - true - - - - - 0 0 1 - - - - - - - 0 0 1 - 100 100 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - - - - true - 0 -15 0 0 0 0 - - - - - 30 0.5 3.0 - - - - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - - - - - - 30 0.5 3.0 - - - - - - - - true - 0 15 0 0 0 0 - - - - - 30 0.5 3.0 - - - - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - - - - - - 30 0.5 3.0 - - - - - - - - true - 15 0 0 0 0 0 - - - - - 0.5 30 3.0 - - - - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - - - - - - 0.5 30 3.0 - - - - - - - - true - -15 0 0 0 0 0 - - - - - 0.5 30 3.0 - - - - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - - - - - - 0.5 30 3.0 - - - - - - - - true - 7 -6 0 0 0 0 - - - - - 3 3 3.0 - - - - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - - - - - - 3 3 3.0 - - - - - - - - true - -8 6 0 0 0 0 - - - - - 3 3 3.0 - - - - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - - - - - - 3 3 3.0 - - - - - - - - true - 4 9 0 0 0 0 - - - - - 3 3 3.0 - - - - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - - - - - - 3 3 3.0 - - - - - - - - - true - 9 3 0 0 0 0 - - - - - 2 2 3.0 - - - - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - - - - - - 2 2 3.0 - - - - - - - - true - 1 -10 0 0 0 0 - - - - - 2 2 3.0 - - - - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - - - - - - 2 2 3.0 - - - - - - - - true - 0 0 0 0 0 0 - - - - - 3 - 3 - - - - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - - - - - - 3 - 3 - - - - - - - - - true - -7 -7 0 0 0 0 - - - - - 2 - 3 - - - - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - - - - - - 2 - 3 - - - - - - - - - -6 0 1 0 0 0 - - - 0.8 0 0.5 0 0 0 - - - - 0.8 0 0.5 0 0 0 - - - - 0 0 0 -1.57 0 -1.57 - - - - - - 0.5 0 0.4 0 0 0 - - 1.14395 - - 0.126164 - 0 - 0 - 0.416519 - 0 - 0.481014 - - - - - - 2.0 1.0 0.5 - - - - - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - 0.0 0.0 1.0 1 - - - - - - 2.0 1.0 0.5 - - - - - 1 - 1 - true - imu - - " - 0 0 0 0 0 0 - lidar - 10 - - - - 256 - 1 - -3.14159 - 3.14159 - - - 1 - 1 - 0 - 0 - - - - 0.08 - 20.0 - 0.01 - - - 1 - true - - - - 0 0 0 0 0 0 - - 1.047 - - 320 - 240 - - - 0.1 - 100 - - - 1 - 30 - true - camera - - - - - - -0.5 0.6 0 -1.5707 0 0 - - 2 - - 0.145833 - 0 - 0 - 0.145833 - 0 - 0.125 - - - - - - 0.4 - 0.2 - - - - 1.0 0.0 0.0 1 - 1.0 0.0 0.0 1 - 1.0 0.0 0.0 1 - - - - - - 0.4 - 0.2 - - - - - - - - -0.5 -0.6 0 -1.5707 0 0 - - 1 - - 0.145833 - 0 - 0 - 0.145833 - 0 - 0.125 - - - - - - 0.4 - 0.2 - - - - 1.0 0.0 0.0 1 - 1.0 0.0 0.0 1 - 1.0 0.0 0.0 1 - - - - - - 0.4 - 0.2 - - - - - - - 0.8 0 -0.2 0 0 0 - - - - - - - 1 - - 0.1 - 0 - 0 - 0.1 - 0 - 0.1 - - - - - - 0.2 - - - - 0.0 1 0.0 1 - 0.0 1 0.0 1 - 0.0 1 0.0 1 - - - - - - 0.2 - - - - - - - - - chassis - left_wheel - - 0 1 0 - - -1.79769e+308 - 1.79769e+308 - - - - - - - chassis - right_wheel - - 0 1 0 - - -1.79769e+308 - 1.79769e+308 - - - - - - - chassis - caster - - - - - left_wheel_joint - right_wheel_joint - 1.2 - 0.4 - 1 - cmd_vel - - - tf - true - true - false - false - true - true - true - 1000 - - - - - - - 16777235 - - - linear: {x: 0.5}, angular: {z: 0.0} - - - - - - - 16777237 - - - linear: {x: -0.5}, angular: {z: 0.0} - - - - - - - 16777236 - - - linear: {x: 0.0}, angular: {z: -0.5} - - - - - - - 16777234 - - - linear: {x: 0.0}, angular: {z: 0.5} - - - - - - - data: true - - - linear: {x: 0.0}, angular: {z: 0.0} - - - - - \ No newline at end of file diff --git a/src/gazebo/launch/sim.ign b/src/gazebo/launch/sim.ign deleted file mode 100644 index 14eba17..0000000 --- a/src/gazebo/launch/sim.ign +++ /dev/null @@ -1,10 +0,0 @@ - - - - - 30 - 9002 - -1 - - - \ No newline at end of file diff --git a/src/gazebo/launch/sim.launch.py b/src/gazebo/launch/sim.launch.py deleted file mode 100644 index 8cf6276..0000000 --- a/src/gazebo/launch/sim.launch.py +++ /dev/null @@ -1,48 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import ExecuteProcess -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration - -from launch_ros.actions import Node - - -def generate_launch_description(): - gazebo_pkg_prefix = get_package_share_directory('gazebo') - gazebo_sim_ign = os.path.join(gazebo_pkg_prefix, 'launch', 'sim.ign') - sdf_file_path = os.path.join(gazebo_pkg_prefix, 'launch', 'robot_env.sdf') - - gz_sim = ExecuteProcess(cmd=['ign', 'launch', '-v 4', f'{gazebo_sim_ign}']) - gz_sim_server = ExecuteProcess(cmd=['ign', 'gazebo', '-s', '-v 4', '-r', f'{sdf_file_path}']) - - # Bridge - bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=['/model/robot/pose@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V', - '/model/robot/pose_static@tf2_msgs/msg/TFMessage@ignition.msgs.Pose_V', - '/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist', - '/imu@sensor_msgs/msg/Imu@ignition.msgs.IMU', - # '/lidar/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked', - '/lidar@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan', - '/model/robot/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry', - '/camera@sensor_msgs/msg/Image@ignition.msgs.Image', - '/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo'], - parameters=[{'qos_overrides./model/vehicle_blue.subscriber.reliability': 'reliable'}], - output='screen', - remappings=[ - ('/model/robot/pose', '/tf'), - ('/model/robot/pose_static', '/tf') - ] - ) - - return LaunchDescription([ - gz_sim, - gz_sim_server, - bridge - ]) \ No newline at end of file diff --git a/src/gazebo/package.xml b/src/gazebo/package.xml deleted file mode 100644 index ee0cb31..0000000 --- a/src/gazebo/package.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - gazebo - 0.0.0 - Gazebo package for launching asd training sim - Apache2.0 - - Eddy Zhou - - ament_cmake - - - ament_cmake - - \ No newline at end of file diff --git a/src/robot/bringup_robot/CMakeLists.txt b/src/robot/bringup_robot/CMakeLists.txt deleted file mode 100644 index bb4484d..0000000 --- a/src/robot/bringup_robot/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(bringup_robot) - -find_package(ament_cmake REQUIRED) - -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME}) - -ament_package() diff --git a/src/robot/bringup_robot/launch/robot.launch.py b/src/robot/bringup_robot/launch/robot.launch.py deleted file mode 100644 index 355156a..0000000 --- a/src/robot/bringup_robot/launch/robot.launch.py +++ /dev/null @@ -1,96 +0,0 @@ -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -import os - -def generate_launch_description(): - ld = LaunchDescription() # Begin building a launch description - - #################### Costmap Node ##################### - costmap_pkg_prefix = get_package_share_directory('costmap') - costmap_param_file = os.path.join( - costmap_pkg_prefix, 'config', 'params.yaml') - - costmap_param = DeclareLaunchArgument( - 'costmap_param_file', - default_value=costmap_param_file, - description='Path to config file for producer node' - ) - costmap_node = Node( - package='costmap', - name='costmap_node', - executable='costmap_node', - parameters=[LaunchConfiguration('costmap_param_file')], - ) - ld.add_action(costmap_param) - ld.add_action(costmap_node) - - #################### Map Memory Node ##################### - map_memory_pkg_prefix = get_package_share_directory('map_memory') - map_memory_param_file = os.path.join( - map_memory_pkg_prefix, 'config', 'params.yaml') - - map_memory_param = DeclareLaunchArgument( - 'map_memory_param_file', - default_value=map_memory_param_file, - description='Path to config file for producer node' - ) - map_memory_node = Node( - package='map_memory', - name='map_memory_node', - executable='map_memory_node', - parameters=[LaunchConfiguration('map_memory_param_file')], - ) - ld.add_action(map_memory_param) - ld.add_action(map_memory_node) - - ##################### Planner Node ##################### - planner_pkg_prefix = get_package_share_directory('planner') - planner_param_file = os.path.join( - planner_pkg_prefix, 'config', 'params.yaml') - - planner_param = DeclareLaunchArgument( - 'planner_param_file', - default_value=planner_param_file, - description='Path to config file for producer node' - ) - planner_node = Node( - package='planner', - name='planner_node', - executable='planner_node', - parameters=[LaunchConfiguration('planner_param_file')], - ) - ld.add_action(planner_param) - ld.add_action(planner_node) - - ##################### Control Node ##################### - control_pkg_prefix = get_package_share_directory('control') - control_param_file = os.path.join( - control_pkg_prefix, 'config', 'params.yaml') - - control_param = DeclareLaunchArgument( - 'control_param_file', - default_value=control_param_file, - description='Path to config file for producer node' - ) - control_node = Node( - package='control', - name='control_node', - executable='control_node', - parameters=[LaunchConfiguration('control_param_file')], - ) - ld.add_action(control_param) - ld.add_action(control_node) - - #################### Odometry Spoof Node ##################### - odometry_spoof_node = Node( - package='odometry_spoof', - name='odometry_spoof', - executable='odometry_spoof', - ) - ld.add_action(odometry_spoof_node) - - return ld diff --git a/src/robot/bringup_robot/package.xml b/src/robot/bringup_robot/package.xml deleted file mode 100644 index 402e374..0000000 --- a/src/robot/bringup_robot/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - bringup_robot - 0.0.0 - Bringup Robot package for launching the robot - - Eddy Zhou - Apache2.0 - - ament_cmake - - odometry_spoof - costmap - map_memory - planner - control - - - ament_cmake - - \ No newline at end of file diff --git a/src/robot/control/CMakeLists.txt b/src/robot/control/CMakeLists.txt deleted file mode 100644 index 0119109..0000000 --- a/src/robot/control/CMakeLists.txt +++ /dev/null @@ -1,49 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(control) - -# Set compiler to use C++ 17 standard -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# Search for dependencies required for building this package -find_package(ament_cmake REQUIRED) # ROS2 build tool -find_package(rclcpp REQUIRED) # ROS2 C++ package - -# Compiles source files into a library -# A library is not executed, instead other executables can link -# against it to access defined methods and classes. -# We build a library so that the methods defined can be used by -# both the unit test and ROS2 node executables. -add_library(control_lib - src/control_core.cpp) -# Indicate to compiler where to search for header files -target_include_directories(control_lib - PUBLIC - include) - -# Add ROS2 dependencies required by package -ament_target_dependencies(control_lib - rclcpp -) - -# Create ROS2 node executable from source files -add_executable(control_node src/control_node.cpp) -# Link to the previously built library to access Transformer classes and methods -target_link_libraries(control_node control_lib) - -# Copy executable to installation location -install(TARGETS - control_node - DESTINATION lib/${PROJECT_NAME}) - -# Copy launch and config files to installation location -install(DIRECTORY - config - DESTINATION share/${PROJECT_NAME}) - -ament_package() \ No newline at end of file diff --git a/src/robot/control/config/params.yaml b/src/robot/control/config/params.yaml deleted file mode 100644 index 0f334b6..0000000 --- a/src/robot/control/config/params.yaml +++ /dev/null @@ -1,3 +0,0 @@ -# control_node: -# ros__parameters: - diff --git a/src/robot/control/include/control_core.hpp b/src/robot/control/include/control_core.hpp deleted file mode 100644 index ad19ecb..0000000 --- a/src/robot/control/include/control_core.hpp +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef CONTROL_CORE_HPP_ -#define CONTROL_CORE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -namespace robot -{ - -class ControlCore { - public: - // Constructor, we pass in the node's RCLCPP logger to enable logging to terminal - ControlCore(const rclcpp::Logger& logger); - - private: - rclcpp::Logger logger_; -}; - -} - -#endif diff --git a/src/robot/control/include/control_node.hpp b/src/robot/control/include/control_node.hpp deleted file mode 100644 index d47f95b..0000000 --- a/src/robot/control/include/control_node.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef CONTROL_NODE_HPP_ -#define CONTROL_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "control_core.hpp" - -class ControlNode : public rclcpp::Node { - public: - ControlNode(); - - private: - robot::ControlCore control_; -}; - -#endif diff --git a/src/robot/control/package.xml b/src/robot/control/package.xml deleted file mode 100644 index b9b7cd6..0000000 --- a/src/robot/control/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - control - 0.0.0 - A sample ROS package for pubsub communication - - Eddy Zhou - Apache2.0 - - - ament_cmake - rclcpp - - ament_lint_auto - ament_lint_common - ament_cmake_gtest - - - - ament_cmake - - \ No newline at end of file diff --git a/src/robot/control/src/control_core.cpp b/src/robot/control/src/control_core.cpp deleted file mode 100644 index c935803..0000000 --- a/src/robot/control/src/control_core.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include "control_core.hpp" - -namespace robot -{ - -ControlCore::ControlCore(const rclcpp::Logger& logger) - : logger_(logger) {} - -} diff --git a/src/robot/control/src/control_node.cpp b/src/robot/control/src/control_node.cpp deleted file mode 100644 index c99e200..0000000 --- a/src/robot/control/src/control_node.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include "control_node.hpp" - -ControlNode::ControlNode(): Node("control"), control_(robot::ControlCore(this->get_logger())) {} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/robot/costmap/CMakeLists.txt b/src/robot/costmap/CMakeLists.txt deleted file mode 100644 index 130ebb8..0000000 --- a/src/robot/costmap/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(costmap) - -# Set compiler to use C++ 17 standard -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# Search for dependencies required for building this package -find_package(ament_cmake REQUIRED) # ROS2 build tool -find_package(rclcpp REQUIRED) # ROS2 C++ package - -# Compiles source files into a library -# A library is not executed, instead other executables can link -# against it to access defined methods and classes. -# We build a library so that the methods defined can be used by -# both the unit test and ROS2 node executables. -add_library(costmap_lib - src/costmap_core.cpp) -# Indicate to compiler where to search for header files -target_include_directories(costmap_lib - PUBLIC include) -# Add ROS2 dependencies required by package -ament_target_dependencies(costmap_lib rclcpp) - -# Create ROS2 node executable from source files -add_executable(costmap_node src/costmap_node.cpp) -# Link to the previously built library to access costmap classes and methods -target_link_libraries(costmap_node costmap_lib) - -# Copy executable to installation location -install(TARGETS - costmap_node - DESTINATION lib/${PROJECT_NAME}) - -# Copy launch and config files to installation location -install(DIRECTORY - config - DESTINATION share/${PROJECT_NAME}) - -ament_package() \ No newline at end of file diff --git a/src/robot/costmap/config/params.yaml b/src/robot/costmap/config/params.yaml deleted file mode 100644 index 544d49f..0000000 --- a/src/robot/costmap/config/params.yaml +++ /dev/null @@ -1,3 +0,0 @@ -# costmap_node: -# ros__parameters: - diff --git a/src/robot/costmap/include/costmap_core.hpp b/src/robot/costmap/include/costmap_core.hpp deleted file mode 100644 index b8baa59..0000000 --- a/src/robot/costmap/include/costmap_core.hpp +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef COSTMAP_CORE_HPP_ -#define COSTMAP_CORE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -namespace robot -{ - -class CostmapCore { - public: - // Constructor, we pass in the node's RCLCPP logger to enable logging to terminal - explicit CostmapCore(const rclcpp::Logger& logger); - - private: - rclcpp::Logger logger_; - -}; - -} - -#endif \ No newline at end of file diff --git a/src/robot/costmap/include/costmap_node.hpp b/src/robot/costmap/include/costmap_node.hpp deleted file mode 100644 index df7d5f1..0000000 --- a/src/robot/costmap/include/costmap_node.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef COSTMAP_NODE_HPP_ -#define COSTMAP_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "costmap_core.hpp" - -class CostmapNode : public rclcpp::Node { - public: - CostmapNode(); - - private: - robot::CostmapCore costmap_; -}; - -#endif \ No newline at end of file diff --git a/src/robot/costmap/package.xml b/src/robot/costmap/package.xml deleted file mode 100644 index 4651c55..0000000 --- a/src/robot/costmap/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - costmap - 0.0.0 - A sample ROS package for pubsub communication - - Owen Leather - Apache2.0 - - - ament_cmake - rclcpp - - ament_lint_auto - ament_lint_common - ament_cmake_gtest - - - - ament_cmake - - \ No newline at end of file diff --git a/src/robot/costmap/src/costmap_core.cpp b/src/robot/costmap/src/costmap_core.cpp deleted file mode 100644 index 2fd0c92..0000000 --- a/src/robot/costmap/src/costmap_core.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include "costmap_core.hpp" - -namespace robot -{ - -CostmapCore::CostmapCore(const rclcpp::Logger& logger) : logger_(logger) {} - -} \ No newline at end of file diff --git a/src/robot/costmap/src/costmap_node.cpp b/src/robot/costmap/src/costmap_node.cpp deleted file mode 100644 index 33a47a7..0000000 --- a/src/robot/costmap/src/costmap_node.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include -#include - -#include "costmap_node.hpp" - -CostmapNode::CostmapNode() : Node("costmap"), costmap_(robot::CostmapCore(this->get_logger())) {} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/src/robot/map_memory/CMakeLists.txt b/src/robot/map_memory/CMakeLists.txt deleted file mode 100644 index 0445870..0000000 --- a/src/robot/map_memory/CMakeLists.txt +++ /dev/null @@ -1,48 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(map_memory) - -# Set compiler to use C++ 17 standard -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# Search for dependencies required for building this package -find_package(ament_cmake REQUIRED) # ROS2 build tool -find_package(rclcpp REQUIRED) # ROS2 C++ package - -# Compiles source files into a library -# A library is not executed, instead other executables can link -# against it to access defined methods and classes. -# We build a library so that the methods defined can be used by -# both the unit test and ROS2 node executables. -add_library(map_memory_lib - src/map_memory_core.cpp) -# Indicate to compiler where to search for header files -target_include_directories(map_memory_lib - PUBLIC include) -# Add ROS2 dependencies required by package -ament_target_dependencies(map_memory_lib - rclcpp -) - -# Create ROS2 node executable from source files -add_executable(map_memory_node src/map_memory_node.cpp) -# Link to the previously built library to access map_memory classes and methods -target_link_libraries(map_memory_node map_memory_lib) - -# Copy executable to installation location -install(TARGETS - map_memory_node - DESTINATION lib/${PROJECT_NAME}) - -# Copy launch and config files to installation location -install(DIRECTORY - config - DESTINATION share/${PROJECT_NAME}) - - -ament_package() diff --git a/src/robot/map_memory/config/params.yaml b/src/robot/map_memory/config/params.yaml deleted file mode 100644 index 282ca85..0000000 --- a/src/robot/map_memory/config/params.yaml +++ /dev/null @@ -1,3 +0,0 @@ -# map_memory_node: -# ros__parameters: - diff --git a/src/robot/map_memory/include/map_memory_core.hpp b/src/robot/map_memory/include/map_memory_core.hpp deleted file mode 100644 index 2610b8f..0000000 --- a/src/robot/map_memory/include/map_memory_core.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef MAP_MEMORY_CORE_HPP_ -#define MAP_MEMORY_CORE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -namespace robot -{ - -class MapMemoryCore { - public: - explicit MapMemoryCore(const rclcpp::Logger& logger); - - private: - rclcpp::Logger logger_; -}; - -} - -#endif diff --git a/src/robot/map_memory/include/map_memory_node.hpp b/src/robot/map_memory/include/map_memory_node.hpp deleted file mode 100644 index e55a567..0000000 --- a/src/robot/map_memory/include/map_memory_node.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef MAP_MEMORY_NODE_HPP_ -#define MAP_MEMORY_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "map_memory_core.hpp" - -class MapMemoryNode : public rclcpp::Node { - public: - MapMemoryNode(); - - private: - robot::MapMemoryCore map_memory_; -}; - -#endif diff --git a/src/robot/map_memory/package.xml b/src/robot/map_memory/package.xml deleted file mode 100644 index 9cc4b5f..0000000 --- a/src/robot/map_memory/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - map_memory - 0.0.0 - A sample ROS package for pubsub communication - - Eddy Zhou - Apache2.0 - - - ament_cmake - rclcpp - - ament_lint_auto - ament_lint_common - ament_cmake_gtest - - - - ament_cmake - - \ No newline at end of file diff --git a/src/robot/map_memory/src/map_memory_core.cpp b/src/robot/map_memory/src/map_memory_core.cpp deleted file mode 100644 index 8a7dee9..0000000 --- a/src/robot/map_memory/src/map_memory_core.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include "map_memory_core.hpp" - -namespace robot -{ - -MapMemoryCore::MapMemoryCore(const rclcpp::Logger& logger) - : logger_(logger) {} - -} diff --git a/src/robot/map_memory/src/map_memory_node.cpp b/src/robot/map_memory/src/map_memory_node.cpp deleted file mode 100644 index 1ce7774..0000000 --- a/src/robot/map_memory/src/map_memory_node.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include "map_memory_node.hpp" - -MapMemoryNode::MapMemoryNode() : Node("map_memory"), map_memory_(robot::MapMemoryCore(this->get_logger())) {} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/robot/odometry_spoof/CMakeLists.txt b/src/robot/odometry_spoof/CMakeLists.txt deleted file mode 100644 index c236ab1..0000000 --- a/src/robot/odometry_spoof/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(odometry_spoof) - -# Set compiler to use C++ 17 standard -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# Search for dependencies required for building this package -find_package(ament_cmake REQUIRED) # ROS2 build tool -find_package(rclcpp REQUIRED) # ROS2 C++ package -find_package(geometry_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) - -# Create ROS2 node executable from source files -add_executable(odometry_spoof src/odometry_spoof.cpp) -target_include_directories(odometry_spoof - PUBLIC include) -# Add ROS2 dependencies required by package -ament_target_dependencies(odometry_spoof - rclcpp - geometry_msgs - tf2 - tf2_ros - tf2_geometry_msgs - nav_msgs -) - -# Copy executable to installation location -install(TARGETS - odometry_spoof - DESTINATION lib/${PROJECT_NAME}) - -ament_package() diff --git a/src/robot/odometry_spoof/include/odometry_spoof.hpp b/src/robot/odometry_spoof/include/odometry_spoof.hpp deleted file mode 100644 index 88535f7..0000000 --- a/src/robot/odometry_spoof/include/odometry_spoof.hpp +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef ODOMETRY_SPOOF_NODE_HPP_ -#define ODOMETRY_SPOOF_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "nav_msgs/msg/odometry.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" - -#include "tf2/LinearMath/Quaternion.h" -#include "tf2/LinearMath/Vector3.h" -#include "tf2/LinearMath/Matrix3x3.h" - -class OdometrySpoofNode : public rclcpp::Node { - public: - OdometrySpoofNode(); - - private: - void timerCallback(); - - // Odom Publisher - rclcpp::Publisher::SharedPtr odom_pub_; - - // Transform Utilities - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - // Timer to periodically lookup transforms - rclcpp::TimerBase::SharedPtr timer_; - - // Check if last transform was found - bool has_last_transform_; - - // Vars to store previous transform - rclcpp::Time last_time_; - tf2::Vector3 last_position_; - tf2::Quaternion last_orientation_; -}; - -#endif diff --git a/src/robot/odometry_spoof/package.xml b/src/robot/odometry_spoof/package.xml deleted file mode 100644 index 2165219..0000000 --- a/src/robot/odometry_spoof/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - odometry_spoof - 0.0.0 - Spoofs Odometry Signal using Simulation Transforms - - Eddy Zhou - Apache2.0 - - - ament_cmake - rclcpp - geometry_msgs - tf2 - tf2_ros - tf2_geometry_msgs - nav_msgs - - ament_lint_auto - ament_lint_common - ament_cmake_gtest - - - - ament_cmake - - \ No newline at end of file diff --git a/src/robot/odometry_spoof/src/odometry_spoof.cpp b/src/robot/odometry_spoof/src/odometry_spoof.cpp deleted file mode 100644 index 3424fa0..0000000 --- a/src/robot/odometry_spoof/src/odometry_spoof.cpp +++ /dev/null @@ -1,145 +0,0 @@ -#include -#include - -#include "odometry_spoof.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - -OdometrySpoofNode::OdometrySpoofNode() : Node("odometry_spoof") { - // Create publisher for Odometry messages - odom_pub_ = this->create_publisher("odom/filtered", 10); - - // Create a TF buffer and listener - tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - // Create a timer to fetch transform & publish odometry at ~10 Hz - timer_ = this->create_wall_timer( - std::chrono::milliseconds(100), - std::bind(&OdometrySpoofNode::timerCallback, this) - ); -} - -void OdometrySpoofNode::timerCallback() { - // We'll look up the transform from sim_world -> robot/chassis/lidar, - // note robot frame is usually not the lidar sensor, but we do so to make this - // assignment easier - const std::string target_frame = "robot/chassis/lidar"; - const std::string source_frame = "sim_world"; - - geometry_msgs::msg::TransformStamped transform_stamped; - try { - transform_stamped = tf_buffer_->lookupTransform( - source_frame, - target_frame, - tf2::TimePointZero // latest available transform - ); - } catch (const tf2::TransformException &ex) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Could not transform %s to %s: %s", - source_frame.c_str(), target_frame.c_str(), ex.what()); - return; - } - - // Create an Odometry message - nav_msgs::msg::Odometry odom_msg; - - // Fill header - odom_msg.header.stamp = transform_stamped.header.stamp; - odom_msg.header.frame_id = source_frame; // "world" frame - odom_msg.child_frame_id = target_frame; // "robot" frame - - // Pose from TF - odom_msg.pose.pose.position.x = transform_stamped.transform.translation.x; - odom_msg.pose.pose.position.y = transform_stamped.transform.translation.y; - odom_msg.pose.pose.position.z = transform_stamped.transform.translation.z; - odom_msg.pose.pose.orientation = transform_stamped.transform.rotation; - - // --- Compute twist from difference in transforms --- - if (has_last_transform_) - { - // Compute time difference - rclcpp::Time current_time = transform_stamped.header.stamp; - double dt = (current_time - last_time_).seconds(); - - if (dt > 0.0) - { - // Linear velocity - double dx = odom_msg.pose.pose.position.x - last_position_.x(); - double dy = odom_msg.pose.pose.position.y - last_position_.y(); - double dz = odom_msg.pose.pose.position.z - last_position_.z(); - - odom_msg.twist.twist.linear.x = dx / dt; - odom_msg.twist.twist.linear.y = dy / dt; - odom_msg.twist.twist.linear.z = dz / dt; - - // Angular velocity - tf2::Quaternion q_last(last_orientation_.x(), - last_orientation_.y(), - last_orientation_.z(), - last_orientation_.w()); - - tf2::Quaternion q_current(transform_stamped.transform.rotation.x, - transform_stamped.transform.rotation.y, - transform_stamped.transform.rotation.z, - transform_stamped.transform.rotation.w); - - // Orientation difference: q_diff = q_last.inverse() * q_current - tf2::Quaternion q_diff = q_last.inverse() * q_current; - - double roll_diff, pitch_diff, yaw_diff; - tf2::Matrix3x3(q_diff).getRPY(roll_diff, pitch_diff, yaw_diff); - - // Angular velocity (rad/s) - odom_msg.twist.twist.angular.x = roll_diff / dt; - odom_msg.twist.twist.angular.y = pitch_diff / dt; - odom_msg.twist.twist.angular.z = yaw_diff / dt; - } - else - { - // If dt == 0, set velocity to zero - odom_msg.twist.twist.linear.x = 0.0; - odom_msg.twist.twist.linear.y = 0.0; - odom_msg.twist.twist.linear.z = 0.0; - odom_msg.twist.twist.angular.x = 0.0; - odom_msg.twist.twist.angular.y = 0.0; - odom_msg.twist.twist.angular.z = 0.0; - } - } - else - { - // No previous transform yet; set velocity to zero - odom_msg.twist.twist.linear.x = 0.0; - odom_msg.twist.twist.linear.y = 0.0; - odom_msg.twist.twist.linear.z = 0.0; - odom_msg.twist.twist.angular.x = 0.0; - odom_msg.twist.twist.angular.y = 0.0; - odom_msg.twist.twist.angular.z = 0.0; - - // Mark that we've now received at least one transform - has_last_transform_ = true; - } - - // Publish odom - odom_pub_->publish(odom_msg); - - // Store current transform as "last" for next iteration - last_time_ = transform_stamped.header.stamp; - last_position_.setValue( - odom_msg.pose.pose.position.x, - odom_msg.pose.pose.position.y, - odom_msg.pose.pose.position.z - ); - last_orientation_.setValue( - odom_msg.pose.pose.orientation.x, - odom_msg.pose.pose.orientation.y, - odom_msg.pose.pose.orientation.z, - odom_msg.pose.pose.orientation.w - ); -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/robot/planner/CMakeLists.txt b/src/robot/planner/CMakeLists.txt deleted file mode 100644 index af32e49..0000000 --- a/src/robot/planner/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(planner) - -# Set compiler to use C++ 17 standard -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# Search for dependencies required for building this package -find_package(ament_cmake REQUIRED) # ROS2 build tool -find_package(rclcpp REQUIRED) # ROS2 C++ package - -# Compiles source files into a library -# A library is not executed, instead other executables can link -# against it to access defined methods and classes. -# We build a library so that the methods defined can be used by -# both the unit test and ROS2 node executables. -add_library(planner_lib - src/planner_core.cpp) -# Indicate to compiler where to search for header files -target_include_directories(planner_lib - PUBLIC include) -# Add ROS2 dependencies required by package -ament_target_dependencies(planner_lib rclcpp) - -# Create ROS2 node executable from source files -add_executable(planner_node src/planner_node.cpp) -# Link to the previously built library to access planner classes and methods -target_link_libraries(planner_node planner_lib) - -# Copy executable to installation location -install(TARGETS - planner_node - DESTINATION lib/${PROJECT_NAME}) - -# Copy launch and config files to installation location -install(DIRECTORY - config - DESTINATION share/${PROJECT_NAME}) - -ament_package() diff --git a/src/robot/planner/config/params.yaml b/src/robot/planner/config/params.yaml deleted file mode 100644 index cd18c14..0000000 --- a/src/robot/planner/config/params.yaml +++ /dev/null @@ -1,3 +0,0 @@ -# planner_node: -# ros__parameters: - diff --git a/src/robot/planner/include/planner_core.hpp b/src/robot/planner/include/planner_core.hpp deleted file mode 100644 index 9860901..0000000 --- a/src/robot/planner/include/planner_core.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef PLANNER_CORE_HPP_ -#define PLANNER_CORE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -namespace robot -{ - -class PlannerCore { - public: - explicit PlannerCore(const rclcpp::Logger& logger); - - private: - rclcpp::Logger logger_; -}; - -} - -#endif diff --git a/src/robot/planner/include/planner_node.hpp b/src/robot/planner/include/planner_node.hpp deleted file mode 100644 index 95562ed..0000000 --- a/src/robot/planner/include/planner_node.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef PLANNER_NODE_HPP_ -#define PLANNER_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "planner_core.hpp" - -class PlannerNode : public rclcpp::Node { - public: - PlannerNode(); - - private: - robot::PlannerCore planner_; -}; - -#endif diff --git a/src/robot/planner/package.xml b/src/robot/planner/package.xml deleted file mode 100644 index b29cb4e..0000000 --- a/src/robot/planner/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - planner - 0.0.0 - A sample ROS package for pubsub communication - - Eddy Zhou - Apache2.0 - - - ament_cmake - rclcpp - - ament_lint_auto - ament_lint_common - ament_cmake_gtest - - - - ament_cmake - - \ No newline at end of file diff --git a/src/robot/planner/src/planner_core.cpp b/src/robot/planner/src/planner_core.cpp deleted file mode 100644 index a7b56af..0000000 --- a/src/robot/planner/src/planner_core.cpp +++ /dev/null @@ -1,9 +0,0 @@ -#include "planner_core.hpp" - -namespace robot -{ - -PlannerCore::PlannerCore(const rclcpp::Logger& logger) -: logger_(logger) {} - -} diff --git a/src/robot/planner/src/planner_node.cpp b/src/robot/planner/src/planner_node.cpp deleted file mode 100644 index ed5a3b7..0000000 --- a/src/robot/planner/src/planner_node.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include "planner_node.hpp" - -PlannerNode::PlannerNode() : Node("planner"), planner_(robot::PlannerCore(this->get_logger())) {} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/samples/cpp/aggregator/src/aggregator_core.cpp b/src/samples/cpp/aggregator/src/aggregator_core.cpp deleted file mode 100644 index 4cfb8ea..0000000 --- a/src/samples/cpp/aggregator/src/aggregator_core.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include - -#include "aggregator_core.hpp" - -namespace samples -{ - -AggregatorCore::AggregatorCore(int64_t timestamp) -: raw_msg_count_(0), filtered_msg_count_(0), start_(timestamp), - latest_raw_time_(-1), latest_filtered_time_(-1) -{} - -double AggregatorCore::raw_frequency() const -{ - if (latest_raw_time_ <= start_) { - return 0.0; - } - return static_cast(raw_msg_count_) / (latest_raw_time_ - start_); -} - -double AggregatorCore::filtered_frequency() const -{ - if (latest_filtered_time_ <= start_) { - return 0.0; - } - return static_cast(filtered_msg_count_) / (latest_filtered_time_ - start_); -} - -void AggregatorCore::add_raw_msg( - const sample_msgs::msg::Unfiltered::SharedPtr msg) -{ - latest_raw_time_ = std::max( - static_cast(msg->timestamp), latest_raw_time_); - raw_msg_count_++; -} - -void AggregatorCore::add_filtered_msg( - const sample_msgs::msg::FilteredArray::SharedPtr msg) -{ - for (auto filtered_msg : msg->packets) { - latest_filtered_time_ = std::max( - static_cast(filtered_msg.timestamp), latest_filtered_time_); - } - filtered_msg_count_++; -} - -} // namespace samples diff --git a/src/samples/cpp/aggregator/src/aggregator_node.cpp b/src/samples/cpp/aggregator/src/aggregator_node.cpp deleted file mode 100644 index 679e3af..0000000 --- a/src/samples/cpp/aggregator/src/aggregator_node.cpp +++ /dev/null @@ -1,49 +0,0 @@ -#include -#include - -#include "aggregator_node.hpp" - -AggregatorNode::AggregatorNode() -: Node("aggregator"), - aggregator_( - samples::AggregatorCore( - std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()).count())) -{ - raw_sub_ = this->create_subscription( - "/unfiltered_topic", ADVERTISING_FREQ, - std::bind( - &AggregatorNode::unfiltered_callback, this, - std::placeholders::_1)); - filtered_sub_ = this->create_subscription( - "/filtered_topic", ADVERTISING_FREQ, - std::bind( - &AggregatorNode::filtered_callback, this, - std::placeholders::_1)); -} - -void AggregatorNode::unfiltered_callback( - const sample_msgs::msg::Unfiltered::SharedPtr msg) -{ - aggregator_.add_raw_msg(msg); - RCLCPP_INFO( - this->get_logger(), "Raw Frequency(msg/s): %f", - aggregator_.raw_frequency() * 1000); -} - -void AggregatorNode::filtered_callback( - const sample_msgs::msg::FilteredArray::SharedPtr msg) -{ - aggregator_.add_filtered_msg(msg); - RCLCPP_INFO( - this->get_logger(), "Filtered Frequency(msg/s): %f", - aggregator_.filtered_frequency() * 1000); -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/utils/README.md b/utils/README.md new file mode 100644 index 0000000..ec7e9b7 --- /dev/null +++ b/utils/README.md @@ -0,0 +1,32 @@ +# Utils + +## `create-package.bash` + +### Usage +`create-package.bash $package_name $module_name [-p] [-h]` +- $package_name -> required, str: name of package +- $module_name -> required, str: name of parent module +- -p -> optional, flag: build python package. Defaults to a cpp package. +- -h -> optional, flag: help description. + +- ⚠️ Double check for these before running the script: + - The `module_name` directory must exist beforehand for the script to work properly. + - The `modules/docker-compose.module_name.yaml` must exist for the script to run. + +### Description +- Creates a ros2 python or cpp `package_name` in the `module_name` +- Creates missing boilerplate files + - If CPP build, these are: + - launch/`package_name`.launch.py + - config/params.yaml + - src/`package_name`_node.cpp + - include/`package_name`_node.hpp + - src/`package_name`_core.cpp + - include/`package_name`_core.hpp + - test/`package_name`_test.cpp + - If Python build, these are: + - launch/`package_name`.launch.py + - config/params.yaml + - `package_name`/`package_name`_node.py + - `package_name`/`package_name`_core.py +- Modifies module_name.interfacing.Dockerfile to copy in package code into container. \ No newline at end of file diff --git a/utils/boilerplate-files/foo.launch.py b/utils/boilerplate-files/foo.launch.py new file mode 100644 index 0000000..4ea3058 --- /dev/null +++ b/utils/boilerplate-files/foo.launch.py @@ -0,0 +1,87 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def launch_setup(context, *args, **kwargs): + """ + Dynamically determine package name and configuration path. + + This function is called by OpaqueFunction to resolve launch arguments + and create the Node action. + """ + # These values are resolved from the DeclareLaunchArgument definitions below + package_name = LaunchConfiguration('package_name').perform(context) + executable_name = LaunchConfiguration('executable_name').perform(context) + node_name = LaunchConfiguration('node_name').perform(context) + + # Construct the path to the parameter file within the current package + # This assumes your params.yaml is in a 'config' subdirectory within your package's share directory. + param_file_path = os.path.join( + get_package_share_directory(package_name), + 'config', + 'params.yaml' # <-- REPLACE if your parameter file has a different name + ) + + # Check if the params.yaml file actually exists + node_parameters = [] + if os.path.exists(param_file_path): + node_parameters.append(param_file_path) + print(f"INFO: Using parameter file for {node_name}: {param_file_path}") + else: + print( + f"WARNING: No params.yaml found for {node_name} at {param_file_path}. Launching without parameters.") + + # Define the Node action + node = Node( + package=package_name, + executable=executable_name, + name=node_name, + parameters=node_parameters, + output='screen', # Optional: 'screen' or 'log'. 'screen' prints output to the console. + emulate_tty=True, # Optional: Set to True for colored output in the console. + ) + + return [node] + + +def generate_launch_description(): + """ + Generate the launch description for a generic ROS 2 package. + + This template is designed to be placed in any ROS 2 package's + launch directory. It expects the package to have a main executable + and optionally a 'config/params.yaml' file. + """ + return LaunchDescription([ + # Declare the package name argument. + # This argument specifies WHICH ROS 2 package this launch file should target. + # When running, you will set this: + # e.g., ros2 launch generic_package.launch.py package_name:=your_actual_package_name + DeclareLaunchArgument( + 'package_name', + description='Name of the ROS 2 package to launch.' + ), + # Declare the executable name argument. + # This argument specifies WHICH executable within the 'package_name' should be run. + # When running, you will set this: + # e.g., ros2 launch ... executable_name:=your_node_executable_name + DeclareLaunchArgument( + 'executable_name', + description='Name of the executable to run from the package.' + ), + # Declare the node name argument (optional, defaults to executable_name) + # This argument sets the ROS 2 node name. If not provided, it defaults to the executable name. + # e.g., ros2 launch ... node_name:=my_custom_node_name + DeclareLaunchArgument( + 'node_name', + default_value=LaunchConfiguration('executable_name'), # Defaults to the executable name + description='Name to assign to the ROS 2 node.' + ), + # OpaqueFunction defers the creation of the Node action until launch arguments are resolved. + # This is necessary because we need the actual string values of package_name, executable_name, etc. + OpaqueFunction(function=launch_setup) + ]) diff --git a/utils/boilerplate-files/foo_core.cpp b/utils/boilerplate-files/foo_core.cpp new file mode 100644 index 0000000..00232b8 --- /dev/null +++ b/utils/boilerplate-files/foo_core.cpp @@ -0,0 +1,3 @@ +#include "foo_core.hpp" + +FooCore::FooCore() {} \ No newline at end of file diff --git a/utils/boilerplate-files/foo_core.hpp b/utils/boilerplate-files/foo_core.hpp new file mode 100644 index 0000000..f65f37e --- /dev/null +++ b/utils/boilerplate-files/foo_core.hpp @@ -0,0 +1,16 @@ +#ifndef FOO_CORE_HPP_ +#define FOO_CORE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +class FooCore { +public: + /* + * Foo core constructor. + */ + FooCore(); + +private: +}; + +#endif \ No newline at end of file diff --git a/utils/boilerplate-files/foo_core.py b/utils/boilerplate-files/foo_core.py new file mode 100644 index 0000000..c9b74e9 --- /dev/null +++ b/utils/boilerplate-files/foo_core.py @@ -0,0 +1,4 @@ + +class FooCore(): + def __init__(self): + pass diff --git a/utils/boilerplate-files/foo_node.cpp b/utils/boilerplate-files/foo_node.cpp new file mode 100644 index 0000000..ea32b93 --- /dev/null +++ b/utils/boilerplate-files/foo_node.cpp @@ -0,0 +1,3 @@ +#include "foo_node.hpp" + +FooNode::FooNode() {} \ No newline at end of file diff --git a/utils/boilerplate-files/foo_node.hpp b/utils/boilerplate-files/foo_node.hpp new file mode 100644 index 0000000..454f170 --- /dev/null +++ b/utils/boilerplate-files/foo_node.hpp @@ -0,0 +1,18 @@ +#ifndef FOO_NODE_HPP_ +#define FOO_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "foo_core.hpp" + +class FooNode : public rclcpp::Node { +public: + /* + * Foo node constructor. + */ + FooNode(); + +private: +}; + +#endif \ No newline at end of file diff --git a/utils/boilerplate-files/foo_node.py b/utils/boilerplate-files/foo_node.py new file mode 100644 index 0000000..e4ce4c7 --- /dev/null +++ b/utils/boilerplate-files/foo_node.py @@ -0,0 +1,32 @@ +import rclpy +from rclpy.node import Node + +from foo.foo_core import FooCore + + +class Foo(Node): + def __init__(self): + super().__init__('python_foo') + + # Declare and get the parameters + self.declare_parameter('version', 1) + + self.__foo = FooCore() + + +def main(args=None): + rclpy.init(args=args) + + python_foo = Foo() + + rclpy.spin(python_foo) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + python_foo.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/utils/boilerplate-files/foo_test.cpp b/utils/boilerplate-files/foo_test.cpp new file mode 100644 index 0000000..6a9ba66 --- /dev/null +++ b/utils/boilerplate-files/foo_test.cpp @@ -0,0 +1,55 @@ +#include "rclcpp/rclcpp.hpp" +#include "gtest/gtest.h" +// Google testing documentation at +// https://google.github.io/googletest/primer.html + +#include "foo_core.hpp" + +// The fixture for testing class Foo. +class FooTest : public testing::Test { +protected: + // You can remove any or all of the following functions if their bodies would + // be empty. + + FooTest() { + // You can do set-up work for each test here. + } + + ~FooTest() override { + // You can do clean-up work that doesn't throw exceptions here. + } + + // If the constructor and destructor are not enough for setting up + // and cleaning up each test, you can define the following methods: + + void SetUp() override { + // Code here will be called immediately after the constructor (right + // before each test). + } + + void TearDown() override { + // Code here will be called immediately after each test (right + // before the destructor). + } + + // Class members declared here can be used by all tests in the test suite + // for Foo. +}; + +// Tests that the Foo::Bar() method does Abc. +TEST_F(FooTest, MethodBarDoesAbc) { + const std::string input_filepath = "this/package/testdata/myinputfile.dat"; + const std::string output_filepath = "this/package/testdata/myoutputfile.dat"; + Foo f; + EXPECT_EQ(f.Bar(input_filepath, output_filepath), 0); +} + +// Tests that Foo does Xyz. +TEST_F(FooTest, DoesXyz) { + // Exercises the Xyz feature of Foo. +} + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/utils/create_package.bash b/utils/create_package.bash new file mode 100755 index 0000000..a81f03e --- /dev/null +++ b/utils/create_package.bash @@ -0,0 +1,163 @@ +#!/bin/bash + +REPO_ROOT=$(git rev-parse --show-toplevel) +MODULES_DIR="$REPO_ROOT/modules" +DOCKERFILE_DIR="$REPO_ROOT/docker" +build_python=false # build package in cpp or python + + +# Read in keyword arguments +while getopts "ph" opt; do + case $opt in + p) + build_python=true + ;; + h) + echo -e "create-package.bash [-p] [-h] module_name package_name\n" + echo "Executes \"docker compose up\" module_name and creates package_name with \"ros2 pkg create\"." + echo "Arguments module_name and package_name are required." + echo "Use optional -p flag to build a python package. Defaults to a cpp package." + ;; + \?) + echo "Invalid option: -$OPTARG." >&2 + exit 1 + ;; + :) + echo "Option -$OPTARG requires an argument." >&2 + exit 1 + ;; + esac +done + +shift $((OPTIND -1)) + + +# Positional arguments +module_name="$1" +package_name="$2" + +if [ -z "$module_name" ] || [ -z "$package_name" ]; then + echo -e "create-package.bash [-p] [-h] module_name package_name\n" + echo "Arguments module_name and package_name are required." + echo "Use optional -p flag to build a python package. Defaults to a cpp package." + exit 1 +fi + +# Variables & helper function to copy and edit boilerplate files +PACKAGE_DIR="$REPO_ROOT/autonomy/$module_name/$package_name" +replace_foo_variants_in_file() { + local file="$1" + local sed_inplace_flag=(-i) + replacement_str="$2" # Replace foo's with package_name + replacement_str_pascal="$(tr '[:lower:]' '[:upper:]' <<< "${replacement_str:0:1}")${replacement_str:1}" # Capitalize first letter for PascalCase (Foo) + replacement_str_upper="$(tr '[:lower:]' '[:upper:]' <<< "$replacement_str")" # All uppercase (FOO) + + # Detect macOS + if [[ "$OSTYPE" == "darwin"* ]]; then + sed_inplace_flag=(-i "") + fi + + sed "${sed_inplace_flag[@]}" \ + -e "s/Foo/$replacement_str_pascal/g" \ + -e "s/foo/$replacement_str/g" \ + -e "s/FOO/$replacement_str_upper/g" \ + "$file" +} + + +# Shut down container upon exit +cleanup() { + if [[ -n "$module_name" ]]; then + echo "Stopping container for module: $module_name" + docker compose --profile deploy -f "$MODULES_DIR/docker-compose.$module_name.yaml" down || true + fi +} +trap cleanup EXIT + + +# Deploy module_name +docker compose --profile deploy \ + -f $MODULES_DIR/docker-compose.$module_name.yaml \ + up -d $module_name + + +# Run ros2 pkg create +if [[ "$build_python" != "true" ]]; then # cpp package + echo "Creating cpp $package_name package." + + docker compose --profile deploy \ + -f $MODULES_DIR/docker-compose.$module_name.yaml \ + exec interfacing bash -c " + source /opt/watonomous/setup.bash + cd ~/ament_ws/src/$module_name + ros2 pkg create $package_name --build-type ament_cmake --dependencies rclcpp + cd $package_name + mkdir launch config test + touch config/params.yaml + rm -r include/$package_name + cd ../.. + colcon build + " + + # Copy in template files into package directory and modify foo's + cp $REPO_ROOT/utils/boilerplate-files/foo.launch.py $PACKAGE_DIR/launch/$package_name.launch.py + replace_foo_variants_in_file "$PACKAGE_DIR/launch/$package_name.launch.py" "$package_name" + cp $REPO_ROOT/utils/boilerplate-files/foo_test.cpp $PACKAGE_DIR/test/${package_name}_test.cpp + replace_foo_variants_in_file "$PACKAGE_DIR/test/${package_name}_test.cpp" "$package_name" + + cp $REPO_ROOT/utils/boilerplate-files/foo_core.cpp $PACKAGE_DIR/src/${package_name}_core.cpp + replace_foo_variants_in_file "$PACKAGE_DIR/src/${package_name}_core.cpp" "$package_name" + cp $REPO_ROOT/utils/boilerplate-files/foo_core.hpp $PACKAGE_DIR/include/${package_name}_core.hpp + replace_foo_variants_in_file "$PACKAGE_DIR/include/${package_name}_core.hpp" "$package_name" + + cp $REPO_ROOT/utils/boilerplate-files/foo_node.cpp $PACKAGE_DIR/src/${package_name}_node.cpp + replace_foo_variants_in_file "$PACKAGE_DIR/src/${package_name}_node.cpp" "$package_name" + cp $REPO_ROOT/utils/boilerplate-files/foo_node.hpp $PACKAGE_DIR/include/${package_name}_node.hpp + replace_foo_variants_in_file "$PACKAGE_DIR/include/${package_name}_node.hpp" "$package_name" + +else # python package + echo "Creating python $package_name package." + + docker compose --profile deploy \ + -f $MODULES_DIR/docker-compose.$module_name.yaml \ + exec interfacing bash -c " + source /opt/watonomous/setup.bash + cd ~/ament_ws/src/$module_name + ros2 pkg create $package_name --build-type ament_python --dependencies rclpy + cd $package_name + mkdir launch config + touch config/params.yaml + + cd ../.. + colcon build + " + + # Copy in template files into package directory and modify foo's + cp $REPO_ROOT/utils/boilerplate-files/foo.launch.py $PACKAGE_DIR/launch/$package_name.launch.py + replace_foo_variants_in_file "$PACKAGE_DIR/launch/$package_name.launch.py" "$package_name" + cp $REPO_ROOT/utils/boilerplate-files/foo_node.py $PACKAGE_DIR/$package_name/${package_name}_node.py + replace_foo_variants_in_file "$PACKAGE_DIR/$package_name/${package_name}_node.py" "$package_name" + cp $REPO_ROOT/utils/boilerplate-files/foo_core.py $PACKAGE_DIR/$package_name/${package_name}_core.py + replace_foo_variants_in_file "$PACKAGE_DIR/$package_name/${package_name}_core.py" "$package_name" +fi + + +# Update module_names's Dockerfile to copy in package_name into container +line_to_add="COPY autonomy/$module_name/$package_name $package_name" +marker='# Copy in source code' +dockerfile="$DOCKERFILE_DIR/$module_name/$module_name.Dockerfile" + +if ! grep -Fxq "$line_to_add" "$dockerfile"; then # Add only if it does not exist + echo "Adding COPY line to $dockerfile" + + # Check if on macOS + if [[ "$OSTYPE" == "darwin"* ]]; then + # Use `|` as delimiter to avoid issues with `/` + sed -i '' "/^${marker}[[:space:]]*$/a\\ +$line_to_add +" "$dockerfile" + + else + sed -i "/^${marker}[[:space:]]*$/a $line_to_add" "$dockerfile" + fi +fi diff --git a/watod b/watod index 21bc326..e48fc4f 100755 --- a/watod +++ b/watod @@ -4,6 +4,7 @@ set -e programname="$(basename "$(test -L "$0" && readlink "$0" || echo "$0")")" MONO_DIR="$(dirname "$(realpath "$0")")" MODULES_DIR="$MONO_DIR/modules" +modules=$(ls $MODULES_DIR) ANSIBLE_TMP_DIR=/tmp/ansible-tmp-$USER function usage { diff --git a/watod-config.sh b/watod-config.sh index bc5da50..d7ac13e 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -10,12 +10,14 @@ ## List of active modules to run, defined in docker-compose.yaml. ## ## Possible values: -## - vis_tools : starts tools for data visualization (foxglove) -## - gazebo : starts robot simulator (gazebo) -## - robot : starts up robot nodes -## - samples : starts up sample nodes for reference - -# ACTIVE_MODULES="" +## - infrastructure : starts visualization tools +## - interfacing : starts interfacing nodes +## - perception : starts perception nodes +## - controller : starts controller nodes +## - simulation : starts simulation +## - samples : starts sample ROS2 pubsub nodes + +ACTIVE_MODULES="" ################################# MODE OF OPERATION ################################# ## Possible modes of operation when running watod. diff --git a/watod_scripts/watod-setup-docker-env.sh b/watod_scripts/watod-setup-docker-env.sh index 6da895f..0018ad9 100644 --- a/watod_scripts/watod-setup-docker-env.sh +++ b/watod_scripts/watod-setup-docker-env.sh @@ -30,13 +30,23 @@ TAG=${TAG/\//-} # List of active profiles to run, defined in docker-compose.yaml. # Possible values: -# - vis_tools : starts visualization tools (vnc and foxglove) -# - production : configs for all containers required in production -# - samples : starts sample ROS2 pubsub nodes +## - infrastructure : starts visualization tools +## - interfacing : starts interfacing nodes +## - perception : starts perception nodes +## - controller : starts controller nodes +## - simulation : starts simulation +## - samples : starts sample ROS2 pubsub nodes +## - infrastructure : starts visualization tools +## - interfacing : starts interfacing nodes +## - perception : starts perception nodes +## - controller : starts controller nodes +## - simulation : starts simulation +## - samples : starts sample ROS2 pubsub nodes ACTIVE_MODULES=${ACTIVE_MODULES:-""} # Docker Registry to pull/push images -REGISTRY_URL=${REGISTRY_URL:-"ghcr.io/watonomous/wato_asd_training"} +REGISTRY_URL=${REGISTRY_URL:-"ghcr.io/watonomous/humanoid"} +REGISTRY_URL=${REGISTRY_URL:-"ghcr.io/watonomous/humanoid"} REGISTRY=$(echo "$REGISTRY_URL" | sed 's|^\(.*\)/.*$|\1|') REPOSITORY=$(echo "$REGISTRY_URL" | sed 's|^.*/\(.*\)$|\1|') @@ -45,20 +55,18 @@ REPOSITORY=$(echo "$REGISTRY_URL" | sed 's|^.*/\(.*\)$|\1|') # NOTE: ALL IMAGE NAMES MUCH BE IN THE FORMAT OF _ # ROS2 C++ Samples -SAMPLES_AGGREGATOR_IMAGE=${SAMPLES_AGGREGATOR_IMAGE:-"$REGISTRY_URL/samples_aggregator"} -SAMPLES_PRODUCER_IMAGE=${SAMPLES_PRODUCER_IMAGE:-"$REGISTRY_URL/samples_producer"} -SAMPLES_TRANSFORMER_IMAGE=${SAMPLES_TRANSFORMER_IMAGE:-"$REGISTRY_URL/samples_transformer"} +SAMPLES_AGGREGATOR_IMAGE=${SAMPLES_AGGREGATOR_IMAGE:-"$REGISTRY_URL/samples/samples_aggregator"} +SAMPLES_PRODUCER_IMAGE=${SAMPLES_PRODUCER_IMAGE:-"$REGISTRY_URL/samples/samples_producer"} +SAMPLES_TRANSFORMER_IMAGE=${SAMPLES_TRANSFORMER_IMAGE:-"$REGISTRY_URL/samples/samples_transformer"} -# ASD Training Images -GAZEBO_SERVER_IMAGE=${GAZEBO_SERVER_IMAGE:-"$REGISTRY_URL/gazebo_server"} -INFRASTRUCTURE_FOXGLOVE_IMAGE=${INFRASTRUCTURE_FOXGLOVE_IMAGE:-"$REGISTRY_URL/infrastructure_foxglove"} -ROBOT_IMAGE=${ROBOT_IMAGE:-"$REGISTRY_URL/robot"} +# Images +INFRASTRUCTURE_FOXGLOVE_IMAGE=${INFRASTRUCTURE_FOXGLOVE_IMAGE:-"$REGISTRY_URL/infrastructure/foxglove"} +CONTROLLER_IMAGE=${CONTROLLER_IMAGE:-"$REGISTRY_URL/controller/controller"} ## --------------------------- Ports ------------------------------ BASE_PORT=${BASE_PORT:-$(($(id -u)*20))} FOXGLOVE_BRIDGE_PORT=${FOXGLOVE_BRIDGE_PORT:-$((BASE_PORT++))} -GAZEBO_PORT=${GAZEBO_PORT:-$((BASE_PORT++))} ## -------------------- Environment Variables ------------------------- @@ -79,14 +87,13 @@ echo "TAG=$TAG" >> "$MODULES_DIR/.env" # Ports echo "BASE_PORT=$BASE_PORT" >> "$MODULES_DIR/.env" echo "FOXGLOVE_BRIDGE_PORT=$FOXGLOVE_BRIDGE_PORT" >> "$MODULES_DIR/.env" -echo "GAZEBO_PORT=$GAZEBO_PORT" >> "$MODULES_DIR/.env" # ROS2 Samples echo "SAMPLES_AGGREGATOR_IMAGE=$SAMPLES_AGGREGATOR_IMAGE" >> "$MODULES_DIR/.env" echo "SAMPLES_PRODUCER_IMAGE=$SAMPLES_PRODUCER_IMAGE" >> "$MODULES_DIR/.env" echo "SAMPLES_TRANSFORMER_IMAGE=$SAMPLES_TRANSFORMER_IMAGE" >> "$MODULES_DIR/.env" -# ASD Training Images -echo "GAZEBO_SERVER_IMAGE=$GAZEBO_SERVER_IMAGE" >> "$MODULES_DIR/.env" +# Images +# Images echo "INFRASTRUCTURE_FOXGLOVE_IMAGE=$INFRASTRUCTURE_FOXGLOVE_IMAGE" >> "$MODULES_DIR/.env" -echo "ROBOT_IMAGE=$ROBOT_IMAGE" >> "$MODULES_DIR/.env" +echo "CONTROLLER_IMAGE=$CONTROLLER_IMAGE" >> "$MODULES_DIR/.env"