diff --git a/(Tutorial) Micro Autonomy Testing Pipelines.md b/(Tutorial) Micro Autonomy Testing Pipelines.md new file mode 100644 index 0000000..a7df472 --- /dev/null +++ b/(Tutorial) Micro Autonomy Testing Pipelines.md @@ -0,0 +1,100 @@ +## Using and Writing Tests for Micro Autonomy Pipelines + +Some GitHub workflows have been set up in this repo to run tests on any PRs against main. It checks for **code style, if it compiles properly, and any unit tests** you may want to add. These are nearly identical to the ones in the [monorepo](https://github.com/WATonomous/wato_monorepo), so you can also find more examples there. + +After making a PR, check the `Actions` panel in the repo. You should be able to find the test results run on your branch. + +### Using the Auto-Linter +To use the auto-linter on a PR against main, add the green `auto-lint` label on the sidebar. The WATonomous bot will add a commit to reformat your code. + +### Adding Unit Tests + +#### 1. To add a test for a ROS node, make a `test` folder in the node with a testing class. + +Example structure with `gym_vis` node: + +``` +└── gym_vis + ├── assets + │   └── f1tenth_car.stl + ├── CMakeLists.txt + ├── include + │   └── gym_vis.hpp + ├── LICENSE + ├── package.xml + ├── src + │   └── gym_vis.cpp + └── test + └── test_gym_vis.cpp +``` + +#### 2. Write your tests in `test_{node_name}.cpp`. Make sure to import the `#include "gtest/gtest.h"` library for testing. + +It should look something like this. + +```cpp +#include "gtest/gtest.h" +#include +#include "gym_vis.hpp" + +TEST(GymVisTest, MakeNode) { + rclcpp::init(0, nullptr); + auto node = std::make_shared(); + EXPECT_EQ(node->get_name(), std::string("gym_vis")); + rclcpp::shutdown(); +} + +TEST(GymVisTest, NodeExists) { + rclcpp::init(0, nullptr); + auto node = std::make_shared(); + EXPECT_TRUE(node != nullptr); + rclcpp::shutdown(); +} +// you can start by adding this test to make sure it runs the test as expected +TEST(GymVisTest, TestTheTestSuite) { EXPECT_TRUE(true); } +``` + +#### 3. Add any dependencies you need in `CMakeLists.txt` +```cmake +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(${PROJECT_NAME}_test + test/test_gym_vis.cpp + # src/gym_vis.cpp // example + ) + target_include_directories(${PROJECT_NAME}_test PUBLIC include) + # add your dependencies. ex: + # ament_target_dependencies(${PROJECT_NAME}_test rclcpp std_msgs nav_msgs visualization_msgs sensor_msgs ackermann_msgs tf2_ros tf2_geometry_msgs) +endif() +``` + +### 4. Similarly, add dependencies in `package.xml` + +Namely, `ament_cmake_gtest` + +### Last thing, +The build/testing pipeline is especially slow on GitHub so it's a good idea to run it locally first. + +It's something like: +```bash +./watod build +./watod up +``` +Build find, and enter your docker container (looks like: ...robot-dev...) +```bash +docker ps +docker exec -it {container-id} /bin/bash +source /opt/ros/humble/setup.bash +``` +Build your node +```bash +cd {into your node} +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select {your_ros_node} +``` + +Run your tests +```bash +colcon test --packages-select {node_name} +colcon test-result --verbose +``` diff --git a/.github/templates/check_src_changes/action.yml b/.github/templates/check_src_changes/action.yml new file mode 100644 index 0000000..a0dd7fc --- /dev/null +++ b/.github/templates/check_src_changes/action.yml @@ -0,0 +1,77 @@ +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 src directory + id: changed-files-outside-src + uses: tj-actions/changed-files@v42 + with: + files: '!src/**' + + - name: Find changed files inside src/wato_msgs folder + id: changed-files-wato-msgs + uses: tj-actions/changed-files@v42 + with: + files: src/wato_msgs/** + + - name: Find changed files inside src/action folder + id: changed-files-action + uses: tj-actions/changed-files@v42 + with: + files: src/action/** + + - name: Get changed files inside src/interfacing folder + id: changed-files-interfacing + uses: tj-actions/changed-files@v42 + with: + files: src/interfacing/** + + - name: Get changed files inside src/perception folder + id: changed-files-perception + uses: tj-actions/changed-files@v42 + with: + files: src/perception/** + + - name: Get changed files inside src/samples folder + id: changed-files-samples + uses: tj-actions/changed-files@v42 + with: + files: src/samples/** + + - name: Get changed files inside src/simulation folder + id: changed-files-simulation + uses: tj-actions/changed-files@v42 + with: + files: src/simulation/** + + - name: Get changed files inside src/world_modeling folder + id: changed-files-world-modeling + uses: tj-actions/changed-files@v42 + with: + files: src/world_modeling/** + + - 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-src.outputs.any_changed == 'true' }} + ACTION_CHANGED: ${{ steps.changed-files-action.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 }} + WORLD_MODELING_CHANGED: ${{ steps.changed-files-world-modeling.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..5e531b5 --- /dev/null +++ b/.github/templates/check_src_changes/check_src_changes.sh @@ -0,0 +1,53 @@ +#!/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 + +# Action +if [ $ACTION_CHANGED == 'true' ]; then + echo "Detected action changes" + MODIFIED_MODULES+="action " +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 + +# World-modeling +if [ $WORLD_MODELING_CHANGED == 'true' ]; then + echo "Detected world_modeling changes" + MODIFIED_MODULES+="world_modeling" +fi + +# Infrastructure +if [ $INFRASTRUCTURE_CHANGED == 'true' ]; then + echo "::notice:: Detected infrastructure changes" + MODIFIED_MODULES="infrastructure" +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..87bfd0e --- /dev/null +++ b/.github/templates/docker_context/action.yml @@ -0,0 +1,27 @@ +name: Generate Docker Environment + +inputs: + modified_modules: + description: "Space deliminated list of modified modules" + required: true + 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..be37ae5 --- /dev/null +++ b/.github/templates/docker_context/docker_context.sh @@ -0,0 +1,67 @@ +#!/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 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 +fi + +# Loop through each module +while read -r module; do + + # Retrieve docker compose service names + services=$(docker compose -f "$module" config --services) + module_out=$(echo "$module" | sed -n 's/modules\/docker-compose\.\(.*\)\.yaml/\1/p') + + # Skip simulation module + if [[ 'simulation' = $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" ]] || \ + [[ "$service_out" == "lidar_object_detection" ]] || \ + [[ "$service_out" == "semantic_segmentation" ]]; then + continue + fi + # Construct JSON object for each service with module and service name + json_object=$(jq -nc --arg module_out "$module_out" --arg service_out "$service_out" \ + '{module: $module_out, service: $service_out}') + # 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[*]}) +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/wato_monorepo" +# REGISTRY_URL="ghcr.io/watonomous/wato_f1tenth" + +REGISTRY=$(echo "$REGISTRY_URL" | sed 's|^\(.*\)/.*$|\1|') +REPOSITORY=$(echo "$REGISTRY_URL" | sed 's|^.*/\(.*\)$|\1|') + +echo "registry=$REGISTRY" >> $GITHUB_OUTPUT +echo "repository=$REPOSITORY" >> $GITHUB_OUTPUT 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..492961f --- /dev/null +++ b/.github/templates/test/test_image.sh @@ -0,0 +1,6 @@ +#!/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" +docker run $IMAGE:$TAG /bin/bash -c "source /opt/ros/\$ROS_DISTRO/setup.bash; source \$WATONOMOUS_INSTALL/setup.bash; colcon test; colcon test-result --verbose" diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml new file mode 100644 index 0000000..5b17b4e --- /dev/null +++ b/.github/workflows/build_test.yml @@ -0,0 +1,160 @@ +name: Micro Autonomy Build and Test + +on: + push: + branches: + - main + pull_request: + branches: + - main + workflow_dispatch: + +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 (from monorepo) + # id: docker-environment + # uses: "./.github/templates/docker_context" + # with: + # modified_modules: ${{ steps.get-module-changes.outputs.modified_modules }} + - name: Generate Docker Environment + id: docker-environment + run: | + matrix='{"include":[{"service":"robot","module":"","dockerfile":"docker/robot/robot.Dockerfile"}]}' + echo "docker_matrix=$matrix" >> $GITHUB_OUTPUT + echo "registry=ghcr.io" >> $GITHUB_OUTPUT + echo "repository=watonomous/wato_f1tenth" >> $GITHUB_OUTPUT + + + - 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.service }}" \ + >> $GITHUB_OUTPUT + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + id: buildx + + # uses default token right now + - name: Docker Login + uses: docker/login-action@v3 + with: + registry: ${{ env.DOCKER_REGISTRY }} + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Prepare Image Dependencies + if: ${{ matrix.module != 'infrastructure' }} + uses: docker/build-push-action@v5 + with: + context: . + file: docker/${{ matrix.module }}/${{ matrix.service }}/${{ matrix.service }}.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: docker/${{ matrix.module }}/${{ matrix.service }}/${{ matrix.service }}.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 + + - name: Security Sanitation for Deployment + uses: docker/build-push-action@v5 + with: + context: . + file: docker/${{ matrix.module }}/${{ matrix.service }}/${{ matrix.service }}.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: build + + - 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!" \ No newline at end of file diff --git a/.github/workflows/linting_auto.yml b/.github/workflows/linting_auto.yml new file mode 100644 index 0000000..0c2a189 --- /dev/null +++ b/.github/workflows/linting_auto.yml @@ -0,0 +1,55 @@ +name: 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.GITHUB_TOKEN }} + + - name: Set up Python + uses: actions/setup-python@v1 + with: + python-version: 3.9 + + - 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/docker/robot/robot.Dockerfile b/docker/robot/robot.Dockerfile index 99e65cd..f63c354 100644 --- a/docker/robot/robot.Dockerfile +++ b/docker/robot/robot.Dockerfile @@ -5,6 +5,9 @@ FROM ${BASE_IMAGE} AS source WORKDIR ${AMENT_WS}/src +RUN apt-get update && apt-get install -y curl \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + # Clean up and update apt-get, then update rosdep RUN sudo apt-get clean && \ sudo apt-get update && \ @@ -16,6 +19,10 @@ RUN sudo apt-get clean && \ COPY src/robot . # Scan for rosdeps + +RUN apt-get update && apt-get install -y curl \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + RUN apt-get -qq update && rosdep update && \ rosdep install --from-paths . --ignore-src -r -s \ | grep 'apt-get install' \ @@ -26,6 +33,10 @@ RUN apt-get -qq update && rosdep update && \ FROM ${BASE_IMAGE} AS dependencies # Clean up and update apt-get, then update rosdep + +RUN apt-get update && apt-get install -y curl \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + RUN sudo apt-get clean && \ sudo apt-get update && \ sudo rosdep update @@ -50,6 +61,10 @@ RUN apt-get -qq autoremove -y && apt-get -qq autoclean && apt-get -qq clean && \ FROM dependencies AS build # Clean up and update apt-get, then update rosdep + +RUN apt-get update && apt-get install -y curl \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + RUN sudo apt-get clean && \ sudo apt-get update && \ sudo rosdep update @@ -63,6 +78,18 @@ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ # Source and Build Artifact Cleanup RUN rm -rf src/* build/* devel/* install/* log/* +#add the slam toolbox, localizaiton and rviz2 +RUN sudo apt-get update +RUN sudo apt-get install -y ros-humble-rviz2 +RUN sudo apt-get install -y ros-humble-navigation2 +RUN sudo apt-get install -y ros-humble-slam-toolbox + +#add controler support to the container +RUN sudo apt-get install -y ros-humble-joy +RUN sudo apt-get install -y jstest-gtk +RUN mkdir -p /root/.config/jstest-gtk + + # Entrypoint will run before any CMD on launch. Sources ~/opt//setup.bash and ~/ament_ws/install/setup.bash COPY docker/wato_ros_entrypoint.sh ${AMENT_WS}/wato_ros_entrypoint.sh ENTRYPOINT ["./wato_ros_entrypoint.sh"] diff --git a/docker/samples/cpp_aggregator.Dockerfile b/docker/samples/cpp_aggregator.Dockerfile index 2f52d0a..8a0c64f 100644 --- a/docker/samples/cpp_aggregator.Dockerfile +++ b/docker/samples/cpp_aggregator.Dockerfile @@ -9,6 +9,9 @@ WORKDIR ${AMENT_WS}/src COPY src/samples/cpp/aggregator aggregator COPY src/wato_msgs/sample_msgs sample_msgs +RUN apt-get update && apt-get install -y curl \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ rosdep install --from-paths . --ignore-src -r -s \ diff --git a/docker/samples/cpp_producer.Dockerfile b/docker/samples/cpp_producer.Dockerfile index 3a87d5d..5d59844 100644 --- a/docker/samples/cpp_producer.Dockerfile +++ b/docker/samples/cpp_producer.Dockerfile @@ -9,6 +9,9 @@ WORKDIR ${AMENT_WS}/src COPY src/samples/cpp/producer producer COPY src/wato_msgs/sample_msgs sample_msgs +RUN apt-get update && apt-get install -y curl \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ rosdep install --from-paths . --ignore-src -r -s \ diff --git a/docker/samples/cpp_transformer.Dockerfile b/docker/samples/cpp_transformer.Dockerfile index 3ba7394..e9fef32 100644 --- a/docker/samples/cpp_transformer.Dockerfile +++ b/docker/samples/cpp_transformer.Dockerfile @@ -9,6 +9,9 @@ WORKDIR ${AMENT_WS}/src COPY src/samples/cpp/transformer transformer COPY src/wato_msgs/sample_msgs sample_msgs +RUN apt-get update && apt-get install -y curl \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ rosdep install --from-paths . --ignore-src -r -s \ diff --git a/docker/samples/py_aggregator.Dockerfile b/docker/samples/py_aggregator.Dockerfile index a4c2008..0ef51e8 100644 --- a/docker/samples/py_aggregator.Dockerfile +++ b/docker/samples/py_aggregator.Dockerfile @@ -9,6 +9,9 @@ WORKDIR ${AMENT_WS}/src COPY src/samples/python/aggregator aggregator COPY src/wato_msgs/sample_msgs sample_msgs +RUN apt-get update && apt-get install -y curl \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ rosdep install --from-paths . --ignore-src -r -s \ diff --git a/docker/samples/py_producer.Dockerfile b/docker/samples/py_producer.Dockerfile index 33380b9..6dc1af3 100644 --- a/docker/samples/py_producer.Dockerfile +++ b/docker/samples/py_producer.Dockerfile @@ -9,6 +9,9 @@ WORKDIR ${AMENT_WS}/src COPY src/samples/python/producer producer COPY src/wato_msgs/sample_msgs sample_msgs +RUN apt-get update && apt-get install -y curl \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ rosdep install --from-paths . --ignore-src -r -s \ diff --git a/docker/samples/py_transformer.Dockerfile b/docker/samples/py_transformer.Dockerfile index 50b8f00..941bb0f 100644 --- a/docker/samples/py_transformer.Dockerfile +++ b/docker/samples/py_transformer.Dockerfile @@ -9,6 +9,9 @@ WORKDIR ${AMENT_WS}/src COPY src/samples/python/transformer transformer COPY src/wato_msgs/sample_msgs sample_msgs +RUN apt-get update && apt-get install -y curl \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ rosdep install --from-paths . --ignore-src -r -s \ diff --git a/docker/sim/sim.Dockerfile b/docker/sim/sim.Dockerfile index a3d461a..1aee883 100644 --- a/docker/sim/sim.Dockerfile +++ b/docker/sim/sim.Dockerfile @@ -24,6 +24,9 @@ FROM ros:foxy SHELL ["/bin/bash", "-c"] +RUN apt-get update && apt-get install -y curl \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + # dependencies RUN apt-get update --fix-missing && \ apt-get install -y git \ diff --git a/docker/vis_tools/foxglove.Dockerfile b/docker/vis_tools/foxglove.Dockerfile index 45d65c5..e081ca3 100644 --- a/docker/vis_tools/foxglove.Dockerfile +++ b/docker/vis_tools/foxglove.Dockerfile @@ -1,5 +1,7 @@ ARG BASE_IMAGE=ghcr.io/watonomous/robot_base/base:humble-ubuntu22.04 +#ARG BASE_IMAGE=osrf/ros:humble-desktop-ful + ################################ Source ################################ FROM ${BASE_IMAGE} AS source @@ -8,6 +10,9 @@ WORKDIR ${AMENT_WS}/src # Copy in source code COPY src/wato_msgs wato_msgs +RUN apt-get update && apt-get install -y curl \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + # Scan for rosdeps RUN apt-get -qq update && rosdep update && \ rosdep install --from-paths . --ignore-src -r -s \ @@ -18,6 +23,9 @@ RUN apt-get -qq update && rosdep update && \ ################################# Dependencies ################################ FROM ${BASE_IMAGE} AS dependencies +RUN apt-get update && apt-get install -y curl \ + && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + # Install Foxglove Deps RUN apt-get update && apt-get install -y curl ros-humble-ros2bag ros-humble-rosbag2* ros-humble-foxglove-msgs&& \ rm -rf /var/lib/apt/lists/* @@ -61,4 +69,4 @@ RUN rm -rf src/* build/* devel/* install/* log/* # Entrypoint will run before any CMD on launch. Sources ~/opt//setup.bash and ~/ament_ws/install/setup.bash COPY docker/wato_ros_entrypoint.sh ${AMENT_WS}/wato_ros_entrypoint.sh -ENTRYPOINT ["./wato_ros_entrypoint.sh"] +ENTRYPOINT ["./wato_ros_entrypoint.sh"] \ No newline at end of file diff --git a/modules/docker-compose.robot.yaml b/modules/docker-compose.robot.yaml index 7adf39a..6aa303f 100644 --- a/modules/docker-compose.robot.yaml +++ b/modules/docker-compose.robot.yaml @@ -21,3 +21,9 @@ services: - develop volumes: - ${MONO_DIR}/src:/home/bolty/ament_ws/src + - /tmp/.X11-unix:/tmp/.X11-unix + - /dev/input:/dev/input + environment: + - DISPLAY=${DISPLAY} + device_cgroup_rules: + - 'c 13:* rmw' \ No newline at end of file diff --git a/src/robot/bringup_robot/launch/robot.launch.py b/src/robot/bringup_robot/launch/robot.launch.py index 6e75e95..0ccde16 100644 --- a/src/robot/bringup_robot/launch/robot.launch.py +++ b/src/robot/bringup_robot/launch/robot.launch.py @@ -6,9 +6,10 @@ import os + def generate_launch_description(): - - ld = LaunchDescription() # Begin building a launch description + + ld = LaunchDescription() # Begin building a launch description #################### Costmap Node ##################### @@ -34,7 +35,7 @@ def generate_launch_description(): # 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, @@ -53,7 +54,7 @@ def generate_launch_description(): # 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, @@ -67,6 +68,5 @@ def generate_launch_description(): # ) # ld.add_action(map_memory_param) # ld.add_action(map_memory_node) - return ld diff --git a/src/robot/gym_vis/CMakeLists.txt b/src/robot/gym_vis/CMakeLists.txt index 6802967..19bf18a 100644 --- a/src/robot/gym_vis/CMakeLists.txt +++ b/src/robot/gym_vis/CMakeLists.txt @@ -20,17 +20,6 @@ find_package(ackermann_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -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() add_executable(gym_vis_node src/gym_vis.cpp) target_include_directories(gym_vis_node PUBLIC include) @@ -40,13 +29,23 @@ ament_target_dependencies(gym_vis_node rclcpp std_msgs nav_msgs visualization_ms # Install the meshes folder install(DIRECTORY assets/ - DESTINATION share/${PROJECT_NAME}/assets +DESTINATION share/${PROJECT_NAME}/assets ) install(TARGETS - gym_vis_node - DESTINATION lib/${PROJECT_NAME} +gym_vis_node +DESTINATION lib/${PROJECT_NAME} ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(${PROJECT_NAME}_test + test/test_gym_vis.cpp + # src/gym_vis.cpp #TODO: proper test on gymviscpp + ) + target_include_directories(${PROJECT_NAME}_test PUBLIC include) + ament_target_dependencies(${PROJECT_NAME}_test rclcpp std_msgs nav_msgs visualization_msgs sensor_msgs ackermann_msgs tf2_ros tf2_geometry_msgs) +endif() ament_package() diff --git a/src/robot/gym_vis/include/gym_vis.hpp b/src/robot/gym_vis/include/gym_vis.hpp index f401541..81ec205 100644 --- a/src/robot/gym_vis/include/gym_vis.hpp +++ b/src/robot/gym_vis/include/gym_vis.hpp @@ -5,18 +5,16 @@ #include #include #include -#include +#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" #include "std_msgs/msg/string.hpp" -#include "nav_msgs/msg/odometry.hpp" #include "visualization_msgs/msg/marker.hpp" -#include "sensor_msgs/msg/laser_scan.hpp" -#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" -#include -#include #include +#include +#include - -#endif \ No newline at end of file +#endif \ No newline at end of file diff --git a/src/robot/gym_vis/package.xml b/src/robot/gym_vis/package.xml index 8162e55..db7482a 100644 --- a/src/robot/gym_vis/package.xml +++ b/src/robot/gym_vis/package.xml @@ -11,6 +11,7 @@ ament_lint_auto ament_lint_common + ament_cmake_gtest rclcpp std_msgs diff --git a/src/robot/gym_vis/src/gym_vis.cpp b/src/robot/gym_vis/src/gym_vis.cpp index 5cc83c5..096f381 100644 --- a/src/robot/gym_vis/src/gym_vis.cpp +++ b/src/robot/gym_vis/src/gym_vis.cpp @@ -2,27 +2,37 @@ using namespace std::chrono_literals; -class GymVis : public rclcpp::Node -{ +class GymVis : public rclcpp::Node { public: - GymVis(): Node("gym_vis"), count_(0) - { - car_publisher_ = this->create_publisher("f1tenth_car", 10); - lidar_publisher_ = this->create_publisher("f1tenth_lidar_data", 10); - //traj_publisher_ = this->create_publisher("f1tenth_traj", 10); - - gym_car_subscriber_ = this->create_subscription("/ego_racecar/odom", 5, std::bind(&GymVis::car_callback, this, std::placeholders::_1)); - gym_lidar_subscriber_ = this->create_subscription("/scan", 5, std::bind(&GymVis::lidar_callback, this, std::placeholders::_1)); - drive_publisher_ = this->create_publisher("/drive", 10); - - publisher_timer_ = this->create_wall_timer(15ms, std::bind(&GymVis::publisher_timer_callback, this)); + GymVis() : Node("gym_vis"), count_(0) { + car_publisher_ = this->create_publisher( + "f1tenth_car", 10); + lidar_publisher_ = this->create_publisher( + "f1tenth_lidar_data", 10); + // traj_publisher_ = + // this->create_publisher("f1tenth_traj", + // 10); + + gym_car_subscriber_ = this->create_subscription( + "/ego_racecar/odom", 5, + std::bind(&GymVis::car_callback, this, std::placeholders::_1)); + gym_lidar_subscriber_ = + this->create_subscription( + "/scan", 5, + std::bind(&GymVis::lidar_callback, this, std::placeholders::_1)); + drive_publisher_ = + this->create_publisher( + "/drive", 10); + + publisher_timer_ = this->create_wall_timer( + 15ms, std::bind(&GymVis::publisher_timer_callback, this)); // CAR MARKER SETUP float car_scale[3] = {0.60, 0.25, 0.125}; float car_color[4] = {0.0, 0.0, 1.0, 1.0}; car_marker_setup("map", car_scale, car_color); - //LIDAR SCAN SETUP + // LIDAR SCAN SETUP float lidar_scale[3] = {0.1, 0.1, 0.1}; float lidar_color[4] = {0.21, 0.9, 0.0, 1.0}; lidar_marker_setup("map", lidar_scale, lidar_color); @@ -32,17 +42,20 @@ class GymVis : public rclcpp::Node rclcpp::TimerBase::SharedPtr publisher_timer_; rclcpp::Publisher::SharedPtr car_publisher_; - rclcpp::Publisher::SharedPtr lidar_publisher_; + rclcpp::Publisher::SharedPtr + lidar_publisher_; rclcpp::Publisher::SharedPtr traj_publisher_; rclcpp::Subscription::SharedPtr gym_car_subscriber_; - rclcpp::Subscription::SharedPtr gym_lidar_subscriber_; - rclcpp::Publisher::SharedPtr drive_publisher_; + rclcpp::Subscription::SharedPtr + gym_lidar_subscriber_; + rclcpp::Publisher::SharedPtr + drive_publisher_; visualization_msgs::msg::Marker car_marker; visualization_msgs::msg::Marker lidar_scan_markers; - - void car_callback (const nav_msgs::msg::Odometry::SharedPtr msg); + + void car_callback(const nav_msgs::msg::Odometry::SharedPtr msg); void lidar_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg); void publisher_timer_callback(); @@ -50,150 +63,154 @@ class GymVis : public rclcpp::Node void lidar_marker_setup(std::string header, float scale[3], float color[4]); size_t count_; - }; -void GymVis::publisher_timer_callback(){ +void GymVis::publisher_timer_callback() { - car_publisher_->publish(car_marker); + car_publisher_->publish(car_marker); lidar_publisher_->publish(lidar_scan_markers); // SAMPLE CODE TO MOVE THE ROBOT STRAIGHT // auto straight_drive_msg = ackermann_msgs::msg::AckermannDriveStamped(); // // Set driving parameters (slow straight driving) // straight_drive_msg.drive.speed = 0.3; // Slow speed, adjust as needed - // straight_drive_msg.drive.steering_angle = 0.0; + // straight_drive_msg.drive.steering_angle = 0.0; // drive_publisher_->publish(straight_drive_msg); // Publish the marker - } -void GymVis::car_callback (const nav_msgs::msg::Odometry::SharedPtr msg){ - //Set the position and orientation - car_marker.pose.position.x = msg->pose.pose.position.x; - car_marker.pose.position.y = msg->pose.pose.position.y; - car_marker.pose.position.z = msg->pose.pose.position.z + car_marker.scale.z/2; - - car_marker.pose.orientation.x = msg->pose.pose.orientation.x; - car_marker.pose.orientation.y = msg->pose.pose.orientation.y; - car_marker.pose.orientation.z = msg->pose.pose.orientation.z; - car_marker.pose.orientation.w = msg->pose.pose.orientation.w; - - //DEBUG MESSAGES - // RCLCPP_INFO(this->get_logger(), "Position: [%.2f, %.2f, %.2f] Orientation: [%.2f, %.2f, %.2f, %.2f]", - // msg->pose.pose.position.x, - // msg->pose.pose.position.y, - // msg->pose.pose.position.z, - // msg->pose.pose.orientation.x, - // msg->pose.pose.orientation.y, - // msg->pose.pose.orientation.z, - // msg->pose.pose.orientation.w - // ); - +void GymVis::car_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { + // Set the position and orientation + car_marker.pose.position.x = msg->pose.pose.position.x; + car_marker.pose.position.y = msg->pose.pose.position.y; + car_marker.pose.position.z = + msg->pose.pose.position.z + car_marker.scale.z / 2; + + car_marker.pose.orientation.x = msg->pose.pose.orientation.x; + car_marker.pose.orientation.y = msg->pose.pose.orientation.y; + car_marker.pose.orientation.z = msg->pose.pose.orientation.z; + car_marker.pose.orientation.w = msg->pose.pose.orientation.w; + + // DEBUG MESSAGES + // RCLCPP_INFO(this->get_logger(), "Position: [%.2f, %.2f, %.2f] Orientation: + // [%.2f, %.2f, %.2f, %.2f]", + // msg->pose.pose.position.x, + // msg->pose.pose.position.y, + // msg->pose.pose.position.z, + // msg->pose.pose.orientation.x, + // msg->pose.pose.orientation.y, + // msg->pose.pose.orientation.z, + // msg->pose.pose.orientation.w + // ); } -void GymVis::lidar_callback (const sensor_msgs::msg::LaserScan::SharedPtr msg){ - static tf2_ros::Buffer tf_buffer(this->get_clock()); - static tf2_ros::TransformListener tf_listener(tf_buffer); - - // Attempt to lookup the transform from the LiDAR frame to the map frame - geometry_msgs::msg::TransformStamped transform_stamped; - - try { - transform_stamped = tf_buffer.lookupTransform( - "map", // Target frame - msg->header.frame_id, // Source frame (e.g., lidar_frame) - tf2::TimePointZero, // Use the latest available transform - 100ms // Timeout - ); - } catch (const tf2::TransformException &ex) { - RCLCPP_WARN(this->get_logger(), "Transform lookup failed: %s", ex.what()); - return; - } +void GymVis::lidar_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { + static tf2_ros::Buffer tf_buffer(this->get_clock()); + static tf2_ros::TransformListener tf_listener(tf_buffer); + + // Attempt to lookup the transform from the LiDAR frame to the map frame + geometry_msgs::msg::TransformStamped transform_stamped; + + try { + transform_stamped = tf_buffer.lookupTransform( + "map", // Target frame + msg->header.frame_id, // Source frame (e.g., lidar_frame) + tf2::TimePointZero, // Use the latest available transform + 100ms // Timeout + ); + } catch (const tf2::TransformException &ex) { + RCLCPP_WARN(this->get_logger(), "Transform lookup failed: %s", ex.what()); + return; + } - // Iterate through the LaserScan ranges and convert them to points - size_t num_points = msg->ranges.size(); - - lidar_scan_markers.points.clear(); - - for (size_t i = 0; i < num_points; ++i) - { - float range = msg->ranges[i]; - - // Check if the range is valid (non-infinite or NaN) - if (std::isfinite(range) && range >= msg->range_min && range <= msg->range_max) - { - // Calculate the angle for this point - float angle = msg->angle_min + i * msg->angle_increment; - - // Convert polar coordinates (range, angle) to Cartesian coordinates (x, y) - geometry_msgs::msg::PointStamped point_in_source, point_in_map; - point_in_source.header = msg->header; // Use the LaserScan's frame - point_in_source.point.x = range * std::cos(angle); - point_in_source.point.y = range * std::sin(angle); - point_in_source.point.z = 0.0; // Laser scan usually provides 2D data (in the XY plane) - - try { - tf2::doTransform(point_in_source, point_in_map, transform_stamped); - // Add the transformed point to the marker - lidar_scan_markers.points.push_back(point_in_map.point); - } catch (const tf2::TransformException &ex) { - RCLCPP_WARN(this->get_logger(), "Point transformation failed: %s", ex.what()); - } - - // DEBUG MESSAGES - // RCLCPP_INFO(this->get_logger(), "Point: [%.2f, %.2f, %.2f]", - // range * std::cos(angle), - // range * std::sin(angle), - // 0.0 - // ); - } + // Iterate through the LaserScan ranges and convert them to points + size_t num_points = msg->ranges.size(); + + lidar_scan_markers.points.clear(); + + for (size_t i = 0; i < num_points; ++i) { + float range = msg->ranges[i]; + + // Check if the range is valid (non-infinite or NaN) + if (std::isfinite(range) && range >= msg->range_min && + range <= msg->range_max) { + // Calculate the angle for this point + float angle = msg->angle_min + i * msg->angle_increment; + + // Convert polar coordinates (range, angle) to Cartesian coordinates (x, + // y) + geometry_msgs::msg::PointStamped point_in_source, point_in_map; + point_in_source.header = msg->header; // Use the LaserScan's frame + point_in_source.point.x = range * std::cos(angle); + point_in_source.point.y = range * std::sin(angle); + point_in_source.point.z = + 0.0; // Laser scan usually provides 2D data (in the XY plane) + + try { + tf2::doTransform(point_in_source, point_in_map, transform_stamped); + // Add the transformed point to the marker + lidar_scan_markers.points.push_back(point_in_map.point); + } catch (const tf2::TransformException &ex) { + RCLCPP_WARN(this->get_logger(), "Point transformation failed: %s", + ex.what()); + } + + // DEBUG MESSAGES + // RCLCPP_INFO(this->get_logger(), "Point: [%.2f, %.2f, %.2f]", + // range * std::cos(angle), + // range * std::sin(angle), + // 0.0 + // ); } + } } -void GymVis::car_marker_setup(std::string header, float scale[3], float color[4]){ - - // Set the header - car_marker.header.frame_id = header; - // Set the type and action - car_marker.type = visualization_msgs::msg::Marker::CUBE; - car_marker.action = visualization_msgs::msg::Marker::ADD; - - //Define the size of the rectangle (e.g., 2 meters long, 1 meter wide, 0.5 meter high) - car_marker.scale.x = scale[0]; //Length - car_marker.scale.y = scale[1]; //Width - car_marker.scale.z = scale[2]; //Height (thickness for visualization) - - //Set color (RGBA) - car_marker.color.r = color[0]; - car_marker.color.g = color[1]; - car_marker.color.b = color[2]; //Blue rectangle - car_marker.color.a = color[3]; //Fully opaque - //car_marker.mesh_resource = "package://gym_vis/assets/f1tenth_car.stl"; +void GymVis::car_marker_setup(std::string header, float scale[3], + float color[4]) { + + // Set the header + car_marker.header.frame_id = header; + // Set the type and action + car_marker.type = visualization_msgs::msg::Marker::CUBE; + car_marker.action = visualization_msgs::msg::Marker::ADD; + + // Define the size of the rectangle (e.g., 2 meters long, 1 meter wide, 0.5 + // meter high) + car_marker.scale.x = scale[0]; // Length + car_marker.scale.y = scale[1]; // Width + car_marker.scale.z = scale[2]; // Height (thickness for visualization) + + // Set color (RGBA) + car_marker.color.r = color[0]; + car_marker.color.g = color[1]; + car_marker.color.b = color[2]; // Blue rectangle + car_marker.color.a = color[3]; // Fully opaque + // car_marker.mesh_resource = "package://gym_vis/assets/f1tenth_car.stl"; } -void GymVis::lidar_marker_setup(std::string header, float scale[3], float color[4]){ - - lidar_scan_markers.header.frame_id = header; - // Set the type and action - lidar_scan_markers.type = visualization_msgs::msg::Marker::POINTS; - lidar_scan_markers.action = visualization_msgs::msg::Marker::ADD; - - // Set scale (size of points) - lidar_scan_markers.scale.x = scale[0]; // Width of points (smaller is more visible) - lidar_scan_markers.scale.y = scale[1]; // Height of points - lidar_scan_markers.scale.z = scale[2]; // Depth of points - - // Set color (RGBA) - lidar_scan_markers.color.r = color[0]; // Red - lidar_scan_markers.color.g = color[1]; // Green - lidar_scan_markers.color.b = color[2]; // Blue - lidar_scan_markers.color.a = color[3]; // Fully opaque +void GymVis::lidar_marker_setup(std::string header, float scale[3], + float color[4]) { + + lidar_scan_markers.header.frame_id = header; + // Set the type and action + lidar_scan_markers.type = visualization_msgs::msg::Marker::POINTS; + lidar_scan_markers.action = visualization_msgs::msg::Marker::ADD; + + // Set scale (size of points) + lidar_scan_markers.scale.x = + scale[0]; // Width of points (smaller is more visible) + lidar_scan_markers.scale.y = scale[1]; // Height of points + lidar_scan_markers.scale.z = scale[2]; // Depth of points + + // Set color (RGBA) + lidar_scan_markers.color.r = color[0]; // Red + lidar_scan_markers.color.g = color[1]; // Green + lidar_scan_markers.color.b = color[2]; // Blue + lidar_scan_markers.color.a = color[3]; // Fully opaque } -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/robot/gym_vis/test/test_gym_vis.cpp b/src/robot/gym_vis/test/test_gym_vis.cpp new file mode 100644 index 0000000..4d8bcb4 --- /dev/null +++ b/src/robot/gym_vis/test/test_gym_vis.cpp @@ -0,0 +1,20 @@ +#include "gtest/gtest.h" +// #include +// should probably make a header file to access because including the cpp has +// main... #include "../src/gym_vis.cpp" + +// TEST(GymVisTest, MakeNode) { +// rclcpp::init(0, nullptr); +// auto node = std::make_shared(); +// EXPECT_EQ(node->get_name(), std::string("gym_vis")); +// rclcpp::shutdown(); +// } + +// TEST(GymVisTest, NodeExists) { +// rclcpp::init(0, nullptr); +// auto node = std::make_shared(); +// EXPECT_TRUE(node != nullptr); +// rclcpp::shutdown(); +// } + +TEST(GymVisTest, TestTheTestSuite) { EXPECT_TRUE(true); } \ No newline at end of file