diff --git a/.devcontainer/amd64/devcontainer.json b/.devcontainer/amd64/devcontainer.json new file mode 100644 index 00000000..7086eba5 --- /dev/null +++ b/.devcontainer/amd64/devcontainer.json @@ -0,0 +1,39 @@ +{ + "name": "CPRT Dev Container", + + "image": "cprtsoftware/rover:dev-amd64", + + "runArgs": [ + "--network=host", + "--volume=/dev:/dev", + "--volume=/sys:/sys", + "--volume=/etc/timezone:/etc/timezone", + "--ipc=host", + "--shm-size=2g" + ], + "mounts": [ + "source=${env:SSH_AUTH_SOCK},target=/ssh-agent,type=bind", + "source=${env:HOME}/.gitconfig,target=/home/vscode/.gitconfig,type=bind,consistency=cached" + ], + "containerEnv": { + "SSH_AUTH_SOCK": "/ssh-agent", + "NVIDIA_VISIBLE_DEVICES": "all", + "NVIDIA_DRIVER_CAPABILITIES": "all" + }, + "remoteUser": "vscode", + "containerUser": "vscode", + "remoteEnv": { + "USER": "vscode" + }, + + // Usual extras (optional) + "customizations": { + "vscode": { + "settings": { + "terminal.integrated.shell.linux": "/bin/bash" + }, + "extensions": [ "ms-vscode.cpptools", "ms-azuretools.vscode-docker" ] + } + }, +"postCreateCommand": "echo 'source /opt/ros/humble/setup.bash' >> ~/.bashrc" +} \ No newline at end of file diff --git a/.devcontainer/jetson/devcontainer.json b/.devcontainer/jetson/devcontainer.json new file mode 100644 index 00000000..b943ac71 --- /dev/null +++ b/.devcontainer/jetson/devcontainer.json @@ -0,0 +1,41 @@ +{ + "name": "CPRT Dev Container", + + "image": "cprtsoftware/rover:dev-arm64", + + "runArgs": [ + "--network=host", + "--volume=/dev:/dev", + "--volume=/sys:/sys", + "--volume=/etc/timezone:/etc/timezone", + "--runtime=nvidia", + "--gpus=all", + "--ipc=host", + "--shm-size=2g" + ], + "mounts": [ + "source=${env:SSH_AUTH_SOCK},target=/ssh-agent,type=bind", + "source=${env:HOME}/.gitconfig,target=/home/vscode/.gitconfig,type=bind,consistency=cached" + ], + "containerEnv": { + "SSH_AUTH_SOCK": "/ssh-agent", + "NVIDIA_VISIBLE_DEVICES": "all", + "NVIDIA_DRIVER_CAPABILITIES": "all" + }, + "remoteUser": "vscode", + "containerUser": "vscode", + "remoteEnv": { + "USER": "vscode" + }, + + // Usual extras (optional) + "customizations": { + "vscode": { + "settings": { + "terminal.integrated.shell.linux": "/bin/bash" + }, + "extensions": [ "ms-vscode.cpptools", "ms-azuretools.vscode-docker" ] + } + }, + "postCreateCommand": "source /opt/ros/humble/setup.bash" +} \ No newline at end of file diff --git a/.devcontainer/windows/devcontainer.json b/.devcontainer/windows/devcontainer.json new file mode 100644 index 00000000..cff7cb7b --- /dev/null +++ b/.devcontainer/windows/devcontainer.json @@ -0,0 +1,43 @@ +{ + "name": "CPRT Dev Container (Windows Host)", + "image": "cprtsoftware/rover:dev-arm64", + + "runArgs": [ + "--network=host", + "--ipc=host", + "--shm-size=2g", + "--gpus=all" + ], + + "mounts": [ + "source=${env:USERPROFILE}\\.gitconfig,target=/home/vscode/.gitconfig,type=bind,consistency=cached", + "source=${env:USERPROFILE}\\.ccache,target=/home/vscode/.ccache,type=bind", + "source=${env:USERPROFILE}\\.ssh,target=/home/vscode/.ssh,type=bind,consistency=cached" + ], + + "containerEnv": { + "NVIDIA_VISIBLE_DEVICES": "all", + "NVIDIA_DRIVER_CAPABILITIES": "all" + }, + + "remoteUser": "vscode", + "containerUser": "vscode", + "remoteEnv": { + "USER": "vscode" + }, + + "customizations": { + "vscode": { + "settings": { + "terminal.integrated.shell.linux": "/bin/bash" + }, + "extensions": [ + "ms-vscode.cpptools", + "ms-azuretools.vscode-docker", + "ms-iot.vscode-ros" + ] + } + }, + + "postCreateCommand": "source /opt/ros/humble/setup.bash" +} diff --git a/.github/workflows/build-docker.yml b/.github/workflows/build-docker.yml new file mode 100644 index 00000000..9ba6b8f0 --- /dev/null +++ b/.github/workflows/build-docker.yml @@ -0,0 +1,144 @@ +name: Docker CI/CD Multi-Arch + +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + +env: + IMAGE_NAME: cprtsoftware/rover + +jobs: + setup: + runs-on: ubuntu-latest + outputs: + IMAGE_NAME: ${{ env.IMAGE_NAME }} + steps: + - name: Checkout repository + uses: actions/checkout@v4 + with: + submodules: recursive + + build-amd64: + runs-on: ubuntu-latest + needs: setup + env: + IMAGE_NAME: ${{ needs.setup.outputs.IMAGE_NAME }} + outputs: + app_digest: ${{ steps.build_app.outputs.digest }} + dev_digest: ${{ steps.build_dev.outputs.digest }} + steps: + - uses: actions/checkout@v4 + + - uses: docker/setup-buildx-action@v3 + + - name: Docker login + uses: docker/login-action@v3 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + + # --- APP (rover) image, amd64 --- + - name: Build & push amd64 APP (with registry cache) + id: build_app + uses: docker/build-push-action@v6 + with: + context: . + file: ./Dockerfile + target: rover + platforms: linux/amd64 + push: true + build-args: | + BASE_IMAGE=ubuntu:22.04 + TARGETARCH=amd64 + tags: | + ${{ env.IMAGE_NAME }}:amd64 + ${{ env.IMAGE_NAME }}:${{ github.sha }}-amd64 + cache-from: type=registry,ref=${{ env.IMAGE_NAME }}:amd64-cache + cache-to: type=registry,ref=${{ env.IMAGE_NAME }}:amd64-cache,mode=max + provenance: false + + # --- DEV image, amd64 --- + - name: Build & push amd64 DEV (with registry cache) + id: build_dev + uses: docker/build-push-action@v6 + with: + context: . + file: ./Dockerfile + target: dev + platforms: linux/amd64 + push: true + build-args: | + BASE_IMAGE=ubuntu:22.04 + TARGETARCH=amd64 + tags: | + ${{ env.IMAGE_NAME }}:dev-amd64 + ${{ env.IMAGE_NAME }}:dev-${{ github.sha }}-amd64 + cache-from: type=registry,ref=${{ env.IMAGE_NAME }}:amd64-cache + cache-to: type=registry,ref=${{ env.IMAGE_NAME }}:amd64-cache,mode=max + provenance: false + + build-arm64: + runs-on: ubuntu-latest + needs: setup + env: + IMAGE_NAME: ${{ needs.setup.outputs.IMAGE_NAME }} + outputs: + app_digest: ${{ steps.build_app.outputs.digest }} + dev_digest: ${{ steps.build_dev.outputs.digest }} + steps: + - uses: actions/checkout@v4 + + - name: Set up QEMU (for arm emulation) + uses: docker/setup-qemu-action@v3 + with: + platforms: linux/arm64 + + - uses: docker/setup-buildx-action@v3 + + - name: Docker login + uses: docker/login-action@v3 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + + # --- APP (rover) image, arm64 --- + - name: Build & push arm64 APP (with registry cache) + id: build_app + uses: docker/build-push-action@v6 + with: + context: . + file: ./Dockerfile + target: rover + platforms: linux/arm64 + push: true + build-args: | + BASE_IMAGE=nvcr.io/nvidia/l4t-jetpack:r36.4.0 + TARGETARCH=arm64 + tags: | + ${{ env.IMAGE_NAME }}:arm64 + ${{ env.IMAGE_NAME }}:${{ github.sha }}-arm64 + cache-from: type=registry,ref=${{ env.IMAGE_NAME }}:arm64-cache + cache-to: type=registry,ref=${{ env.IMAGE_NAME }}:arm64-cache,mode=max + provenance: false + + # --- DEV image, arm64 --- + - name: Build & push arm64 DEV (with registry cache) + id: build_dev + uses: docker/build-push-action@v6 + with: + context: . + file: ./Dockerfile + target: dev + platforms: linux/arm64 + push: true + build-args: | + BASE_IMAGE=nvcr.io/nvidia/l4t-jetpack:r36.4.0 + TARGETARCH=arm64 + tags: | + ${{ env.IMAGE_NAME }}:dev-arm64 + ${{ env.IMAGE_NAME }}:dev-${{ github.sha }}-arm64 + cache-from: type=registry,ref=${{ env.IMAGE_NAME }}:arm64-cache + cache-to: type=registry,ref=${{ env.IMAGE_NAME }}:arm64-cache,mode=max + provenance: false \ No newline at end of file diff --git a/.github/workflows/docker-build-science.yml b/.github/workflows/docker-build-science.yml deleted file mode 100644 index 4b44594f..00000000 --- a/.github/workflows/docker-build-science.yml +++ /dev/null @@ -1,33 +0,0 @@ -name: Build and Push Docker Image - -on: - push: - branches: [main] - -jobs: - build-and-push: - runs-on: ubuntu-latest - - steps: - - name: Checkout repo - uses: actions/checkout@v3 - - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v3 - - - name: Log in to DockerHub - uses: docker/login-action@v3 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - - name: Build and push Docker image - uses: docker/build-push-action@v5 - with: - context: . - file: ./Dockerfile.science - push: true - platforms: linux/arm64 - tags: | - ${{ secrets.DOCKERHUB_USERNAME }}/cprt_rover_24:science-latest - ${{ secrets.DOCKERHUB_USERNAME }}/cprt_rover_24:science-${{ github.sha }} diff --git a/.github/workflows/lint-and-format.yml b/.github/workflows/lint-and-format.yml index 002df84b..71f1658c 100644 --- a/.github/workflows/lint-and-format.yml +++ b/.github/workflows/lint-and-format.yml @@ -1,9 +1,6 @@ name: Lint and Format Check on: - push: - branches: - - '*' pull_request: branches: - '*' diff --git a/.gitignore b/.gitignore index 5f4b74c7..b965222e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,7 @@ # Directories -install/ +install*/ log/ -build/ +build*/ *.vscode raman/ @@ -13,4 +13,6 @@ __pycache__ camera_setup/images/* # macOS Directory Information -.DS_Store \ No newline at end of file +.DS_Store + +upgrade_pkg.tar.* \ No newline at end of file diff --git a/.gitmodules b/.gitmodules index f4f2007f..c57094fd 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,15 +1,11 @@ -[submodule "src/kindr_ros"] - path = src/kindr_ros - url = https://github.com/SivertHavso/kindr_ros.git - branch = ros2 -[submodule "src/zed-ros2-wrapper"] - path = src/zed-ros2-wrapper - url = https://github.com/stereolabs/zed-ros2-wrapper.git -[submodule "ublox"] - path = src/ublox +[submodule "src/third-party/ublox"] + path = src/third-party/ublox url = https://github.com/KumarRobotics/ublox.git branch = ros2 -[submodule "src/ouster-ros"] - path = src/ouster-ros +[submodule "src/third-party/ouster-ros"] + path = src/third-party/ouster-ros url = https://github.com/ouster-lidar/ouster-ros.git branch = ros2 +[submodule "src/third-party/zed-ros2-wrapper"] + path = src/third-party/zed-ros2-wrapper + url = https://github.com/stereolabs/zed-ros2-wrapper.git diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 00000000..c2f64da1 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,213 @@ +# syntax=docker/dockerfile:1.5 + +############################ +# Stage 0: Base Image per Arch +############################ +ARG BASE_IMAGE=ubuntu:22.04 +ARG TARGETARCH +FROM ${BASE_IMAGE} AS base + +############################ +# Stage 1: Minimal ROS 2 Base +############################ +FROM base AS ros2_humble-base + +ENV DEBIAN_FRONTEND=noninteractive +ENV ROS_DISTRO=humble +ENV LANG=en_US.UTF-8 +ENV LANGUAGE=en_US:en +ENV LC_ALL=en_US.UTF-8 +ENV TZ=UTC + +# Per-arch APT cache (mount to the real apt cache path; keep per-arch id) +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ + --mount=type=cache,target=/var/lib/apt/lists,id=apt-lists-${TARGETARCH},sharing=locked \ + apt-get update && apt-get install -y --no-install-recommends \ + locales apt-utils curl lsb-release gnupg2 \ + software-properties-common build-essential \ + python3-dev tzdata python3-pip git \ + && locale-gen en_US.UTF-8 \ + && update-locale LANG=en_US.UTF-8 LC_ALL=en_US.UTF-8 + +RUN ln -snf /usr/share/zoneinfo/UTC /etc/localtime \ + && echo "UTC" > /etc/timezone + +# ROS 2 repo +RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg && \ + 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" \ + > /etc/apt/sources.list.d/ros2.list + +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ + --mount=type=cache,target=/var/lib/apt/lists,id=apt-lists-${TARGETARCH},sharing=locked \ + apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-ros-base python3-setuptools python3-wheel \ + libeigen3-dev python3-rosdep + +# Python dependencies (per-arch pip cache) +COPY requirements.txt . +RUN --mount=type=cache,target=/root/.cache/pip,id=pip-${TARGETARCH},sharing=locked \ + pip3 install -r requirements.txt + + +############################ +# Stage 2: Rust Setup +############################ +FROM ros2_humble-base AS rust-builder +ENV PATH="/root/.cargo/bin:${PATH}" +ENV CARGO_HOME=/root/.cargo +ENV RUSTUP_HOME=/root/.rustup +SHELL ["/bin/bash", "-c"] + + +RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y && \ + . "$HOME/.cargo/env" && \ + cargo install cargo-c + + +############################ +# Stage 3: GStreamer Build +############################ +FROM rust-builder AS ros2_humble-gstreamer +ARG TARGETARCH + + +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ + --mount=type=cache,target=/var/lib/apt/lists,id=apt-lists-${TARGETARCH},sharing=locked \ + apt-get update && apt-get install -y \ + zlib1g-dev libffi-dev libssl-dev python3-dev python3-pip \ + flex bison libglib2.0-dev libmount-dev libsrt-openssl-dev \ + build-essential git ninja-build curl ccache \ + +# Enable compiler caching +ENV CCACHE_DIR=/root/.cache/ccache +ENV PATH="/usr/lib/ccache:${PATH}" +ENV CC="ccache gcc" +ENV CXX="ccache g++" + + +RUN python3 -m pip install --upgrade --user pip meson + +WORKDIR /gstreamer +RUN --mount=type=cache,target=/root/.cache/gstreamer,id=gst-${TARGETARCH} \ + --mount=type=cache,target=/root/.cache/meson,id=meson-${TARGETARCH} \ + --mount=type=cache,target=/root/.cache/ccache,id=ccache-${TARGETARCH},sharing=locked \ + git clone https://gitlab.freedesktop.org/gstreamer/gstreamer.git . && \ + git checkout 1.24 && \ + ~/.local/bin/meson setup builddir --prefix=/opt/gstreamer --libdir=lib && \ + ~/.local/bin/meson compile -C builddir && \ + ~/.local/bin/meson install -C builddir --destdir /target + +# Build GStreamer Rust plugins +WORKDIR /gst-plugins-rs +ENV LD_LIBRARY_PATH=/target/opt/gstreamer/lib:$LD_LIBRARY_PATH +ENV LIBRARY_PATH=/target/opt/gstreamer/lib:$LIBRARY_PATH +ENV PKG_CONFIG_PATH=/target/opt/gstreamer/lib/pkgconfig:$PKG_CONFIG_PATH +SHELL ["/bin/bash", "-c"] + +RUN git clone https://gitlab.freedesktop.org/gstreamer/gst-plugins-rs.git . && \ + cargo cbuild -p gst-plugin-webrtc --prefix=/opt/gstreamer --release && \ + cargo cbuild -p gst-plugin-rtp --prefix=/opt/gstreamer --release && \ + cargo cinstall -p gst-plugin-webrtc --prefix=/opt/gstreamer --destdir /target --libdir=lib --release && \ + cargo cinstall -p gst-plugin-rtp --prefix=/opt/gstreamer --destdir /target --libdir=lib --release + +############################ +# Stage 6: Collect package.xml files +############################ +FROM alpine AS package_xml_collector +WORKDIR /src +COPY src/ . + +# Create a directory to hold all package.xml files with relative paths +RUN find . -name 'package.xml' -exec mkdir -p /collected/{} \; \ + && find . -name 'package.xml' -exec cp {} /collected/{} \; + +############################ +# Stage 7: Minimal Runtime Base +############################ +FROM ros2_humble-base AS runtime +COPY --from=ros2_humble-gstreamer /target / + +ENV PATH=/opt/gstreamer/bin:$PATH +ENV LD_LIBRARY_PATH=/opt/gstreamer/lib:$LD_LIBRARY_PATH +ENV PKG_CONFIG_PATH=/opt/gstreamer/lib/pkgconfig:$PKG_CONFIG_PATH +SHELL ["/bin/bash", "-o", "pipefail", "-c"] +RUN mkdir -p /temporary +WORKDIR /temporary +COPY --from=package_xml_collector /collected /temporary/src/ + +# rosdep with ROS env loaded (uses cached rosdep/rosdistro) +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ + --mount=type=cache,target=/var/lib/apt/lists,id=apt-lists-${TARGETARCH},sharing=locked \ + source /opt/ros/humble/setup.bash && \ + apt-get update && \ + rosdep init && rosdep update && \ + rosdep install -i -r -y --from-paths src + +RUN git clone https://github.com/ANYbotics/kindr.git && \ + cd kindr && \ + mkdir build && cd build && \ + cmake .. -DUSE_CMAKE=true && \ + make install + +############################ +# Stage 8: Dev Environment +############################ +FROM runtime AS dev +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ + --mount=type=cache,target=/var/lib/apt/lists,id=apt-lists-${TARGETARCH},sharing=locked \ + apt-get update && apt-get install -y --no-install-recommends \ + git x11-apps ros-humble-desktop ros-dev-tools black pylint \ + ros-humble-ament-cmake python3-colcon-common-extensions \ + python3-colcon-ros clang-format cuda-toolkit-12 + +# Enable compiler caching + +RUN useradd -ms /bin/bash -u 1000 vscode && echo "vscode ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers +RUN chown -R vscode:vscode /usr/local/lib/python3.10/dist-packages +WORKDIR / +CMD ["/bin/bash"] + +############################ +# Stage 9: Builder +############################ +FROM runtime AS builder +SHELL ["/bin/bash", "-o", "pipefail", "-c"] +ARG TARGETARCH +ARG DIR=/cprt_rover_24 +ARG CUDA_DIR=/usr/local/cuda-12 +WORKDIR ${DIR} +ENV CMAKE_PREFIX_PATH=/usr/share/eigen3/cmake:$CMAKE_PREFIX_PATH +ENV CMAKE_INCLUDE_PATH=/usr/include/eigen3:$CMAKE_INCLUDE_PATH +ENV PKG_CONFIG_PATH=/usr/lib/x86_64-linux-gnu/pkgconfig:$PKG_CONFIG_PATH +ENV CUDA_TOOLKIT_ROOT_DIR=${CUDA_DIR} +ENV PATH=${CUDA_DIR}/bin:$PATH +ENV LD_LIBRARY_PATH=${CUDA_DIR}/lib64:$LD_LIBRARY_PATH +ENV CCACHE_DIR=/root/.cache/ccache + +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ + --mount=type=cache,target=/var/lib/apt/lists,id=apt-lists-${TARGETARCH},sharing=locked \ + apt-get update && apt-get install -y --no-install-recommends \ + ros-humble-desktop ros-humble-ament-cmake python3-colcon-common-extensions \ + python3-colcon-ros ccache cuda-toolkit-12 + + +COPY src/ ${DIR}/src/ + +# colcon build with ROS env + ccache +RUN --mount=type=cache,target=/root/.cache/ccache,id=ccache-${TARGETARCH},sharing=locked \ + source /opt/ros/humble/setup.bash && \ + colcon build --symlink-install --continue-on-error + +############################ +# Stage 10: Runtime Application +############################ +FROM runtime AS rover +ARG DIR=/cprt_rover_24 +WORKDIR ${DIR} +COPY --from=builder ${DIR}/install ${DIR}/install +RUN echo 'source /opt/ros/humble/setup.bash' >> /etc/profile.d/ros.sh \ + && echo "if [ -f ${DIR}/install/setup.bash ]; then source ${DIR}/install/setup.bash; fi" >> /etc/profile.d/ros.sh + +CMD ["/bin/bash"] diff --git a/Dockerfile.science b/Dockerfile.science deleted file mode 100644 index 6b160501..00000000 --- a/Dockerfile.science +++ /dev/null @@ -1,95 +0,0 @@ -FROM --platform=linux/arm64 ubuntu:22.04 - -ENV DEBIAN_FRONTEND=noninteractive -ENV LANG=en_US.UTF-8 -ENV LC_ALL=en_US.UTF-8 -ENV ROS_DISTRO=humble -ENV ROS_VERSION=2 -ENV ROS_PYTHON_VERSION=3 -ENV ROS_LOCALHOST_ONLY=0 -ENV ROS_DOMAIN_ID=0 - -# Install base packages and ROS keys -RUN apt-get update && apt-get install -y --no-install-recommends \ - git \ - curl \ - lsb-release \ - gnupg2 \ - locales \ - libc6-dev \ - software-properties-common && \ - locale-gen en_US.UTF-8 && \ - apt-get clean && \ - rm -rf /var/lib/apt/lists/* - -# Set up ROS 2 repository -RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \ - echo "deb [arch=arm64 signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" \ - > /etc/apt/sources.list.d/ros2.list - -# Install ROS 2 and development tools -RUN apt-get update && apt-get upgrade -y && \ - apt-get install -y --no-install-recommends \ - ros-humble-ros-base \ - ros-dev-tools \ - python3-dev \ - python3-pip \ - ninja-build \ - python3-rosdep \ - python3-colcon-common-extensions \ - python3-colcon-ros \ - ros-humble-rclcpp && \ - apt-get clean && \ - rm -rf /var/lib/apt/lists/* - -# Install Python packages -RUN pip3 install --no-cache-dir \ - pyserial \ - adafruit-circuitpython-lis3dh \ - adafruit-circuitpython-pca9685 \ - adafruit-circuitpython-motorkit \ - Adafruit-Blinka \ - adafruit-circuitpython-bme280 \ - adafruit-circuitpython-ens160 \ - adafruit-circuitpython-busdevice \ - adafruit-circuitpython-register \ - pyubx2 \ - RPi.GPIO \ - rpi-hardware-pwm - -# Set project directory -ARG DIR=/cprt_rover_24 -WORKDIR ${DIR} - -ARG PACKAGES="camera_controller interfaces science_sensors servo_pkg" - -# Copy workspace into container -COPY src/camera_controller ${DIR}/src/camera_controller -COPY src/interfaces ${DIR}/src/interfaces -COPY src/science_sensors ${DIR}/src/science_sensors -COPY src/servo_pkg ${DIR}/src/servo_pkg - -RUN apt-get update && \ - rosdep init && \ - rosdep update && \ - rosdep install \ - --from-paths $(for pkg in $PACKAGES; do echo src/$pkg; done) \ - -i -r -y - -RUN ldconfig - - -# Build the workspace -RUN bash -c "source /opt/ros/humble/setup.bash && ldconfig && colcon build --packages-select ${PACKAGES} --cmake-args -DCMAKE_BUILD_TYPE=Release" - - -# Create entrypoint script -RUN echo '#!/bin/bash' > /entrypoint.sh && \ - echo 'source /opt/ros/humble/setup.bash' >> /entrypoint.sh && \ - echo 'if [ -f /cprt_rover_24/install/setup.bash ]; then source /cprt_rover_24/install/setup.bash; fi' >> /entrypoint.sh && \ - echo 'exec "$@"' >> /entrypoint.sh && \ - chmod +x /entrypoint.sh - -# Set entrypoint -ENTRYPOINT ["/entrypoint.sh"] -CMD ["/bin/bash"] diff --git a/README.md b/README.md new file mode 100644 index 00000000..9834e342 --- /dev/null +++ b/README.md @@ -0,0 +1,35 @@ +# CPRT Rover 2024 + +This repository contains the workspace and source code for the CPRT Rover 2024 project. It is organized as a ROS 2 (Humble) workspace and includes packages for navigation, science sensors, camera integration, GPS, and more. + +## Workspace Structure + +- **src/**: Main source code for all packages (navigation, sensors, camera, GPS, etc.) +- **setup/**: Setup scripts for installing dependencies and configuring the environment +- **build.sh / build_science.sh**: Build scripts for the workspace and science packages +- **docker/**: Dockerfiles and instructions for containerized development (In Development) +- **.devcontainer/**: VSCode devcontainer configuration +- **.github/**: GitHub Actions and workflows + +## Getting started + +### Host (Only ubuntu 22.04) +See setup/README.md for which setup scripts to run + +### Docker Method (New and still experimental): +Note: Still recommended to use ubuntu 22.04 even with devcontainer + +#### Install Docker: +Window - [Link to download](https://docs.docker.com/desktop/setup/install/windows-install/) +Mac - [Link to download](https://docs.docker.com/desktop/setup/install/mac-install/) +Linux - [Link to download] (https://docs.docker.com/engine/install/) + +#### Install VS code dev containers extension: +Use extension marketplace + +#### Open Dev container in VS code + +#### From inside the container run: +``` +./make.sh +``` diff --git a/build.sh b/build.sh index 730400d5..918747e7 100755 --- a/build.sh +++ b/build.sh @@ -24,4 +24,6 @@ done rosdep install --from-paths src -i -r -y +aarch=$(uname -m) + colcon build --symlink-install --continue-on-error --cmake-args=-DCMAKE_BUILD_TYPE=Release --parallel-workers $(nproc) \ No newline at end of file diff --git a/build_science.sh b/build_science.sh deleted file mode 100755 index dbf2fa97..00000000 --- a/build_science.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -docker build -t cprtsoftware/cprt_rover_24:science-latest -f Dockerfile.science . \ No newline at end of file diff --git a/local_docker_build.sh b/local_docker_build.sh new file mode 100755 index 00000000..cf48f2ab --- /dev/null +++ b/local_docker_build.sh @@ -0,0 +1,51 @@ +#!/usr/bin/env bash +set -euo pipefail + +IMAGE_NAME="cprtsoftware/rover" +GIT_SHA=$(git rev-parse HEAD) + +# Detect local architecture +ARCH=$(uname -m) +case "$ARCH" in + x86_64) + TARGETARCH=amd64 + BASE_IMAGE="stereolabs/zed:4.2-runtime-cuda12.1-ubuntu22.04" + ;; + aarch64) + TARGETARCH=arm64 + BASE_IMAGE="stereolabs/zed:4.2-runtime-l4t-r36.4" + ;; + *) + echo "Unsupported architecture: $ARCH" + exit 1 + ;; +esac + +echo "Building for local architecture: $TARGETARCH (base: $BASE_IMAGE)" + +# Ensure buildx is set up +docker buildx create --use --name roverbuilder 2>/dev/null || true + +# --- DEV image --- +docker buildx build \ + --build-arg BASE_IMAGE=$BASE_IMAGE \ + --build-arg TARGETARCH=$TARGETARCH \ + -f Dockerfile \ + --target dev \ + -t ${IMAGE_NAME}:dev-${TARGETARCH} \ + --cache-from=type=registry,ref=${IMAGE_NAME}:${TARGETARCH}-cache \ + --cache-to=type=registry,ref=${IMAGE_NAME}:${TARGETARCH}-cache,mode=max \ + --load \ + . + +# --- APP (rover) image --- +docker buildx build \ + --build-arg BASE_IMAGE=$BASE_IMAGE \ + --build-arg TARGETARCH=$TARGETARCH \ + -f Dockerfile \ + --target builder \ + -t ${IMAGE_NAME}:${TARGETARCH} \ + --cache-from=type=registry,ref=${IMAGE_NAME}:${TARGETARCH}-cache \ + --cache-to=type=registry,ref=${IMAGE_NAME}:${TARGETARCH}-cache,mode=max \ + --load \ + . diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 00000000..67fe3065 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,15 @@ +pyserial +adafruit-circuitpython-lis3dh +adafruit-circuitpython-pca9685 +adafruit-circuitpython-motorkit +Adafruit-Blinka +adafruit-circuitpython-bme280 +adafruit-circuitpython-ens160 +adafruit-circuitpython-busdevice +adafruit-circuitpython-register +adafruit-circuitpython-neopixel +pyubx2 +RPi.GPIO +rpi-hardware-pwm +Jetson.GPIO +python-can diff --git a/setup/cprt_bash_aliases.sh b/setup/cprt_bash_aliases.sh deleted file mode 100755 index 090dbcf7..00000000 --- a/setup/cprt_bash_aliases.sh +++ /dev/null @@ -1,420 +0,0 @@ -# CPRT Bash Aliases - -if [ "$0" = "$BASH_SOURCE" ]; then - echo "Error: cprt_bash_aliases.sh must be sourced" - exit 1 -fi - -echo "Added CPRT bash aliases" - -debug=0 - -create_alias_with_echo() { - local alias_name="$1" - local command_string="$2" - - if [ "$debug" != 0 ]; then - echo "Adding alias with name: $alias_name and command: $command_string" - fi - - alias "$alias_name"="echo Running: '$command_string'; eval \"$command_string\"" -} - -alias ea='nano $ROVER_DIR/setup/cprt_bash_aliases.sh' -alias eaNano='nano $ROVER_DIR/setup/cprt_bash_aliases.sh' -alias eaVim='vim $ROVER_DIR/setup/cprt_bash_aliases.sh' - -alias home='cd $ROVER_DIR' -alias gohome='cd $ROVER_DIR' -alias goRoverHome='cd $ROVER_DIR' -alias goWebHome='cd $WEB_DIR' -alias homeWeb='cd $WEB_DIR' - -alias roverSource='source $ROVER_DIR/install/setup.bash' -rover_source_name=roverSource # Used below - -create_alias_with_echo goLatLonDir 'cd $ROVER_DIR/src/nav_commanders/known_gps_coords' - -create_alias_with_echo pingBaseAntenna 'ping 192.168.0.2' -create_alias_with_echo pingRoverAntenna 'ping 192.168.0.3' -create_alias_with_echo pingJetson 'ping 192.168.0.55' -create_alias_with_echo pingJetsonWifi 'ping 192.168.1.111' -create_alias_with_echo pingBattleStation 'ping 192.168.0.20' -create_alias_with_echo pingBattleStationWifi 'ping 192.168.1.100' - -create_alias_with_echo sshJetson 'ssh cprt@192.168.0.55' -create_alias_with_echo sshJetsonWifi 'ssh cprt@192.168.1.111' -create_alias_with_echo sshBattleStation 'ssh cprt@192.168.0.20' -create_alias_with_echo sshBattleStationWifi 'ssh cprt@192.168.1.100' - -create_alias_with_echo lsCams 'ls /dev/v4l/by-id' - -create_alias_with_echo openWebUI 'chromium http://localhost:3000' -create_alias_with_echo openRoverCode 'code $ROVER_DIR' -create_alias_with_echo openWebCode 'code $WEB_DIR' - -create_alias_with_echo rvizMoveit 'rviz2 -d $ROVER_DIR/src/arm_srdf/config/moveit.rviz' -create_alias_with_echo rvizArmMoveIt 'rviz2 -d $ROVER_DIR/src/arm_srdf/config/moveit.rviz' -create_alias_with_echo rvizNav 'rviz2 -d $ROVER_DIR/src/navigation/rviz/nav2_cprt.rviz' -create_alias_with_echo rvizElevationMapping 'rviz2 -d $ROVER_DIR/src/navigation/rviz/traversibility_gridmap.rviz' - - -# Launch helpers - -create_alias_with_echo launchBasestation "$rover_source_name && ros2 launch bringup basestation.launch.py" -create_alias_with_echo launchEquipementServicing "$rover_source_name && ros2 launch bringup equipment_servicing.launch.py" -create_alias_with_echo launchNav "$rover_source_name && ros2 launch bringup nav.launch.py" -create_alias_with_echo launchScience "$rover_source_name && ros2 launch bringup science.launch.py" -create_alias_with_echo launchTraversal "$rover_source_name && ros2 launch bringup transversal.launch.py" - - -create_alias_with_echo tmn 'tmux new -s' # Start new session -create_alias_with_echo tma 'tmux attach -t' # Attach to session -create_alias_with_echo tmuxls 'tmux ls' # List sessions -create_alias_with_echo tmks 'tmux kill-session -t' # Kill session - -# # create_alias_with_echo tmuxNewNav 'tmux new -s nav' -alias tmuxNewNav='(tmux has-session -t nav 2>/dev/null && echo "Session nav exists. Running: tmux attach -t nav" && sleep 1 && tmux attach -t nav) || (echo "Starting new nav session. Running: tmux new -s nav" && sleep 1 && tmux new -s nav)' -create_alias_with_echo tmuxAttachNav 'tmux attach -t nav' -create_alias_with_echo tmuxKillNav 'tmux kill-session -t nav' -create_alias_with_echo tmuxIsNavAlive 'tmux ls | grep nav || echo "Session nav not found"' - -create_alias_with_echo tmuxSplitHorizontal 'tmux split-window -h' -create_alias_with_echo tmuxSplitVertical 'tmux split-window -v' - -setDriveCameraExposure() { - local camera_device="" - local exposure_value="$1" - - # Check for the first camera device path - if [[ -e "/dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP_-video-index0" ]]; then - camera_device="/dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP_-video-index0" - # Check for the second camera device path - elif [[ -e "/dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP__Arducam_20231205_0001-video-index0" ]]; then - camera_device="/dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP__Arducam_20231205_0001-video-index0" - else - echo "Error: Drive camera device not found at expected paths." - echo "Please ensure one of the following paths exists:" - echo " /dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP_-video-index0" - echo " /dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP__Arducam_20231205_0001-video-index0" - return 1 - fi - - # Check if an exposure value was provided - if [[ -z "$exposure_value" ]]; then - echo "Usage: setDriveCameraExposure " - echo "Exposure value must be an integer between 5 and 330 (inclusive)." - return 1 - fi - - # Validate the exposure value - if ! [[ "$exposure_value" =~ ^[0-9]+$ ]]; then - echo "Error: Exposure value '$exposure_value' is not a valid integer." - return 1 - fi - - if (( exposure_value < 5 || exposure_value > 330 )); then - echo "Error: Exposure value must be between 5 and 330 (inclusive)." - return 1 - fi - - echo "Setting Drive Camera exposure to $exposure_value using device: $camera_device" - # Execute the v4l2-ctl command - v4l2-ctl -d "$camera_device" --set-ctrl=auto_exposure=1 --set-ctrl=exposure_time_absolute="$exposure_value" - - if [[ $? -eq 0 ]]; then - echo "Exposure set successfully." - else - echo "Failed to set exposure. Check permissions or camera status." - fi -} - -setDriveCameraAutoExposure() { - local camera_device="" - - # Check for the first camera device path - if [[ -e "/dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP_-video-index0" ]]; then - camera_device="/dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP_-video-index0" - # Check for the second camera device path - elif [[ -e "/dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP__Arducam_20231205_0001-video-index0" ]]; then - camera_device="/dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP__Arducam_20231205_0001-video-index0" - else - echo "Error: Gimbal camera device not found at expected paths." - echo "Please ensure one of the following paths exists:" - echo " /dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP_-video-index0" - echo " /dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP__Arducam_20231205_0001-video-index0" - return 1 - fi - - echo "Turning on auto exposure for Gimbal camera using device: $camera_device" - # Execute the v4l2-ctl command to set auto_exposure to 0 (Auto Mode) - v4l2-ctl -d "$camera_device" --set-ctrl=auto_exposure=0 - - if [[ $? -eq 0 ]]; then - echo "Auto exposure turned on successfully." - else - echo "Failed to turn on auto exposure. Check permissions or camera status." - fi -} - -setDriveCameraBrightness() { - local camera_device="" - local brightness_value="$1" - - # Check for the first camera device path - if [[ -e "/dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP_-video-index0" ]]; then - camera_device="/dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP_-video-index0" - # Check for the second camera device path - elif [[ -e "/dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP__Arducam_20231205_0001-video-index0" ]]; then - camera_device="/dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP__Arducam_20231205_0001-video-index0" - else - echo "Error: Drive camera device not found at expected paths." - echo "Please ensure one of the following paths exists:" - echo " /dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP_-video-index0" - echo " /dev/v4l/by-id/usb-Arducam_Arducam_B0495__USB3_2.3MP__Arducam_20231205_0001-video-index0" - return 1 - fi - - # Check if an exposure value was provided - if [[ -z "$brightness_value" ]]; then - echo "Usage: setDriveCameraBrightness " - echo "Exposure value must be an integer between -64 and 64 (inclusive)." - return 1 - fi - - # Validate the exposure value - # if ! [[ "$brightness_value" =~ ^[0-9]+$ ]]; then - # echo "Error: Brightness value '$brightness_value' is not a valid integer." - # return 1 - # fi - - if (( brightness_value < -64 || brightness_value > 64 )); then - echo "Error: Brightness value must be between -64 and 64 (inclusive)." - return 1 - fi - - echo "Setting Drive Camera brightness to $brightness_value using device: $camera_device" - # Execute the v4l2-ctl command - v4l2-ctl -d "$camera_device" --set-ctrl=brightness="$brightness_value" - - if [[ $? -eq 0 ]]; then - echo "Brightness set successfully." - else - echo "Failed to set exposure. Check permissions or camera status." - fi -} - -setBellyCameraExposure() { - local camera_device="/dev/v4l/by-id/usb-HD_Camera_Manufacturer_USB_2.0_Camera-video-index0" - local exposure_value="$1" - - # Check if the camera device path exists - if [[ ! -e "$camera_device" ]]; then - echo "Error: Belly camera not found at: $camera_device" - echo "Please ensure the camera is connected and the device path is correct." - return 1 - fi - - # Check if an exposure value was provided - if [[ -z "$exposure_value" ]]; then - echo "Usage: setBellyCameraExposure " - echo "Exposure value must be an integer between 1 and 5000 (inclusive)." - return 1 - fi - - # Validate the exposure value - if ! [[ "$exposure_value" =~ ^[0-9]+$ ]]; then - echo "Error: Exposure value '$exposure_value' is not a valid integer." - return 1 - fi - - # Based on your list-ctrls, exposure_time_absolute min=1 max=5000 - if (( exposure_value < 1 || exposure_value > 5000 )); then - echo "Error: Exposure value must be between 1 and 5000 (inclusive)." - return 1 - fi - - echo "Setting Belly Camera exposure to $exposure_value using device: $camera_device" - # Execute the v4l2-ctl command - # Set auto_exposure to 1 (Manual Mode) and then set exposure_time_absolute - v4l2-ctl -d "$camera_device" --set-ctrl=auto_exposure=1 --set-ctrl=exposure_time_absolute="$exposure_value" - - if [[ $? -eq 0 ]]; then - echo "Exposure set successfully." - else - echo "Failed to set exposure. Check permissions or camera status." - fi -} - -setBellyCameraAutoExposure() { - local camera_device="/dev/v4l/by-id/usb-HD_Camera_Manufacturer_USB_2.0_Camera-video-index0" - - # Check if the camera device path exists - if [[ ! -e "$camera_device" ]]; then - echo "Error: Belly camera not found at: $camera_device" - echo "Please ensure the camera is connected and the device path is correct." - return 1 - fi - - echo "Turning on auto exposure for Belly Camera using device: $camera_device" - # Execute the v4l2-ctl command to set auto_exposure to 3 (Auto Mode) - v4l2-ctl -d "$camera_device" --set-ctrl=auto_exposure=3 - - if [[ $? -eq 0 ]]; then - echo "Auto exposure turned on successfully." - else - echo "Failed to turn on auto exposure. Check permissions or camera status." - fi -} - - -setBellyCameraBrightness() { - local camera_device="/dev/v4l/by-id/usb-HD_Camera_Manufacturer_USB_2.0_Camera-video-index0" - local brightness_value="$1" - - # Check if the camera device path exists - if [[ ! -e "$camera_device" ]]; then - echo "Error: Belly camera not found at: $camera_device" - echo "Please ensure the camera is connected and the device path is correct." - return 1 - fi - - # Check if an exposure value was provided - if [[ -z "$brightness_value" ]]; then - echo "Usage: setBellyCameraBrightness " - echo "Exposure value must be an integer between -64 and 64 (inclusive)." - return 1 - fi - - # Validate the exposure value - # if ! [[ "$brightness_value" =~ ^[0-9]+$ ]]; then - # echo "Error: Brightness value '$brightness_value' is not a valid integer." - # return 1 - # fi - - # Based on your list-ctrls, exposure_time_absolute min=1 max=5000 - if (( brightness_value < -64 || brightness_value > 64 )); then - echo "Error: Brightness value must be between -64 and 64 (inclusive)." - return 1 - fi - - echo "Setting Belly Camera brightness to $brightness_value using device: $camera_device" - # Execute the v4l2-ctl command - # Set auto_exposure to 1 (Manual Mode) and then set exposure_time_absolute - v4l2-ctl -d "$camera_device" --set-ctrl=brightness="$brightness_value" - - if [[ $? -eq 0 ]]; then - echo "Brightness set successfully." - else - echo "Failed to set brightness. Check permissions or camera status." - fi -} - - -setArmCameraExposure() { - local camera_device="/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB_2.0_Camera_SN5100-video-index0" - local exposure_value="$1" - - # Check if the camera device path exists - if [[ ! -e "$camera_device" ]]; then - echo "Error: Arm camera not found at: $camera_device" - echo "Please ensure the camera is connected and the device path is correct." - return 1 - fi - - # Check if an exposure value was provided - if [[ -z "$exposure_value" ]]; then - echo "Usage: setArmCameraExposure " - echo "Exposure value must be an integer between 1 and 5000 (inclusive)." - return 1 - fi - - # Validate the exposure value - if ! [[ "$exposure_value" =~ ^[0-9]+$ ]]; then - echo "Error: Exposure value '$exposure_value' is not a valid integer." - return 1 - fi - - # Based on your list-ctrls, exposure_time_absolute min=1 max=5000 - if (( exposure_value < 1 || exposure_value > 5000 )); then - echo "Error: Exposure value must be between 1 and 5000 (inclusive)." - return 1 - fi - - echo "Setting Arm Camera exposure to $exposure_value using device: $camera_device" - # Execute the v4l2-ctl command - # Set auto_exposure to 1 (Manual Mode) and then set exposure_time_absolute - v4l2-ctl -d "$camera_device" --set-ctrl=auto_exposure=1 --set-ctrl=exposure_time_absolute="$exposure_value" - - if [[ $? -eq 0 ]]; then - echo "Exposure set successfully." - else - echo "Failed to set exposure. Check permissions or camera status." - fi -} - -setArmCameraAutoExposure() { - local camera_device="/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB_2.0_Camera_SN5100-video-index0" - - # Check if the camera device path exists - if [[ ! -e "$camera_device" ]]; then - echo "Error: Arm camera not found at: $camera_device" - echo "Please ensure the camera is connected and the device path is correct." - return 1 - fi - - echo "Turning on auto exposure for Arm Camera using device: $camera_device" - # Execute the v4l2-ctl command to set auto_exposure to 3 (Auto Mode) - v4l2-ctl -d "$camera_device" --set-ctrl=auto_exposure=3 - - if [[ $? -eq 0 ]]; then - echo "Auto exposure turned on successfully." - else - echo "Failed to turn on auto exposure. Check permissions or camera status." - fi -} - -setArmCameraBrightness() { - local camera_device="/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB_2.0_Camera_SN5100-video-index0" - local brightness_value="$1" - - # Check if the camera device path exists - if [[ ! -e "$camera_device" ]]; then - echo "Error: Arm camera not found at: $camera_device" - echo "Please ensure the camera is connected and the device path is correct." - return 1 - fi - - # Check if an exposure value was provided - if [[ -z "$brightness_value" ]]; then - echo "Usage: setArmCameraBrightness " - echo "Exposure value must be an integer between -64 and 64 (inclusive)." - return 1 - fi - - # Validate the exposure value - # if ! [[ "$brightness_value" =~ ^[0-9]+$ ]]; then - # echo "Error: Brightness value '$brightness_value' is not a valid integer." - # return 1 - # fi - - # Based on your list-ctrls, exposure_time_absolute min=1 max=5000 - if (( brightness_value < -64 || brightness_value > 64 )); then - echo "Error: Brightness value must be between -64 and 64 (inclusive)." - return 1 - fi - - echo "Setting Arm Camera brightness to $brightness_value using device: $camera_device" - # Execute the v4l2-ctl command - # Set auto_exposure to 1 (Manual Mode) and then set exposure_time_absolute - v4l2-ctl -d "$camera_device" --set-ctrl=brightness="$brightness_value" - - if [[ $? -eq 0 ]]; then - echo "Brightness set successfully." - else - echo "Failed to set brightness. Check permissions or camera status." - fi -} - - -create_alias_with_echo setDefaultV4l2CameraSettings 'setDriveCameraBrightness -25; setArmCameraBrightness 0; setBellyCameraBrightness -10' \ No newline at end of file diff --git a/setup/cprt_bash_rc.sh b/setup/cprt_bash_rc.sh deleted file mode 100755 index 237dce2c..00000000 --- a/setup/cprt_bash_rc.sh +++ /dev/null @@ -1,29 +0,0 @@ -# CPRT Bash rc - -if [ "$0" = "$BASH_SOURCE" ]; then - echo "Error: cprt_bash_rc.sh must be sourced" - exit 1 -fi - -if [[ -z "${OVERRIDE_ROVER_DIR}" ]]; then - export ROVER_DIR=$(cd ~/cprt_rover_24 && pwd) - # echo "Set ROVER_DIR env variable to $ROVER_DIR" -else - export ROVER_DIR=$(cd $OVERRIDE_ROVER_DIR && pwd) - # echo "Setting ROVER_DIR to OVERRIDE_ROVER_DIR, so ROVER_DIR set to $ROVER_DIR" -fi - - -if [[ -z "${OVERRIDE_WEB_DIR}" ]]; then - if [ -d "$HOME/cprt_web_ui_24" ]; then - export WEB_DIR=$(cd "$HOME/cprt_web_ui_24" && pwd) - # echo "Set WEB_DIR env variable to $WEB_DIR" - fi -else - export WEB_DIR=$(cd $OVERRIDE_WEB_DIR && pwd) - # echo "Setting WEB_DIR to OVERRIDE_WEB_DIR, so WEB_DIR set to $WEB_DIR" -fi - - -source "$ROVER_DIR/setup/cprt_bash_aliases.sh" - diff --git a/setup/enablecan.sh b/setup/enablecan.sh index f4039121..f1e01589 100755 --- a/setup/enablecan.sh +++ b/setup/enablecan.sh @@ -9,10 +9,6 @@ modprobe can modprobe can_raw modprobe mttcan ip link set can0 type can bitrate 1000000 -#ip link set up can0 - -#ip link set can0 type can bitrate 1000000 dbitrate 1000000 berr-reporting on fd on -#ip link set can1 type can bitrate 1000000 dbitrate 1000000 berr-reporting on fd on ip link set up can0 #ip link set up can1 diff --git a/setup/fastdds_config.xml b/setup/fastdds_config.xml deleted file mode 100644 index f36a89fa..00000000 --- a/setup/fastdds_config.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - fastdds.builtin_transports.udp.max_msg_size - 1024 - - - fastdds.builtin_transports.udp.sockets_size - 65536 - - - - - \ No newline at end of file diff --git a/setup/firstTimeInstall.sh b/setup/firstTimeInstall.sh index b4c997d2..09f77c7c 100755 --- a/setup/firstTimeInstall.sh +++ b/setup/firstTimeInstall.sh @@ -9,14 +9,6 @@ else exit 1 fi - -echo "source $CURRENT_DIR/cprt_bash_rc.sh" >> ~/.bashrc - -./install_ros_humble.sh -source ~/.bashrc -./install_gstreamer.sh -source ~/.bashrc -./install_nav_deps.sh source ~/.bashrc ./setup_service.sh @@ -27,20 +19,12 @@ else echo "Non-Jetson system detected, skipping Jetson specific setup." exit 0 fi + sudo cp enablecan.sh /usr/local/bin || exit 1 echo "$USER ALL=(ALL) NOPASSWD: /usr/local/bin/enablecan.sh" | sudo tee -a /etc/sudoers > /dev/null -sudo cp turnOnRaman.sh /usr/local/bin -sudo chmod 755 /usr/local/bin/turnOnRaman.sh - -echo "${SUDO_USER:-$USER} ALL=(ALL) NOPASSWD: /usr/local/bin/turnOnRaman.sh" \ - | sudo tee /etc/sudoers.d/turnonraman >/dev/null -sudo chmod 0440 /etc/sudoers.d/turnonraman - ./usb_service_setup.sh -cp "$CURRENT_DIR/tmux_config.conf" ~/.tmux.conf - UDEV_RULE='SUBSYSTEM=="video4linux", ATTR{name}=="Arducam B0495 (USB3 2.3MP)", ATTRS{idVendor}=="04b4", ATTR{index}=="0", ATTRS{idProduct}=="0495", SYMLINK+="drive_camera", MODE="0666"' echo "$UDEV_RULE" | sudo tee /etc/udev/rules.d/99-arducam.rules > /dev/null diff --git a/setup/install_gstreamer.sh b/setup/install_gstreamer.sh deleted file mode 100755 index b9ced3d1..00000000 --- a/setup/install_gstreamer.sh +++ /dev/null @@ -1,75 +0,0 @@ -#!/bin/bash - -echo "Starting Gstreamer upgrade..." - -sudo apt-get update -sudo apt-get install -y \ - zlib1g-dev \ - libffi-dev \ - libssl-dev \ - python3-dev \ - python3-pip \ - flex \ - bison \ - libglib2.0-dev \ - libmount-dev \ - libsrt-openssl-dev - -python3 -m pip install --upgrade pip -pip3 install meson -export PATH=$PATH:~/.local/bin -if ! grep -q "export PATH=\$PATH:~/.local/bin" ~/.bashrc; then - echo "export PATH=\$PATH:~/.local/bin" >> ~/.bashrc - echo "meson exported in bashrc" -fi - -cd ~ -git clone https://gitlab.freedesktop.org/gstreamer/gstreamer.git -cd gstreamer -git checkout 1.24 -meson setup builddir -meson compile -C builddir - -export GSTREAMER_DIR=$PWD -./gst-env.py --only-environment > setupGstreamer.sh -sed -i '/PWD/d' setupGstreamer.sh -sudo chmod +x setupGstreamer.sh -source setupGstreamer.sh -if ! grep -q "source $GSTREAMER_DIR/setupGstreamer.sh" ~/.bashrc; then - echo "source $GSTREAMER_DIR/setupGstreamer.sh" >> ~/.bashrc - echo "Gstreamer sourced in bashrc" -fi - -cd /tmp -curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y -. "$HOME/.cargo/env" -git clone https://gitlab.freedesktop.org/gstreamer/gst-plugins-rs.git - -# Webrtc -cd gst-plugins-rs/net/webrtc -cargo build --release --target-dir build -curl -o- https://raw.githubusercontent.com/nvm-sh/nvm/v0.40.0/install.sh | bash -export NVM_DIR="$HOME/.nvm" -[ -s "$NVM_DIR/nvm.sh" ] && \. "$NVM_DIR/nvm.sh" -nvm install 22 -node -v -npm -v -cd gstwebrtc-api/ -npm install -npm run build -cd ../.. -cp -r webrtc $GSTREAMER_DIR -export GST_PLUGIN_PATH=$GST_PLUGIN_PATH:$GSTREAMER_DIR/webrtc/build/release - -# Rtp and congestion controll -cd /tmp/gst-plugins-rs/net/rtp -cargo build --release --target-dir build -cd .. -cp -r rtp $GSTREAMER_DIR -export GST_PLUGIN_PATH=$GST_PLUGIN_PATH:$GSTREAMER_DIR/rtp/build/release - -export GST_PLUGIN_PATH=$GST_PLUGIN_PATH:/usr/lib/aarch64-linux-gnu/gstreamer-1.0/deepstream:/usr/lib/aarch64-linux-gnu/gstreamer-1.0/ - -echo "export GST_PLUGIN_PATH=$GST_PLUGIN_PATH" >> $GSTREAMER_DIR/setupGstreamer.sh - -echo "Finished building GStreamer" \ No newline at end of file diff --git a/setup/install_nav_deps.sh b/setup/install_nav_deps.sh deleted file mode 100755 index a625e320..00000000 --- a/setup/install_nav_deps.sh +++ /dev/null @@ -1,22 +0,0 @@ -#!/bin/bash - -echo "Downloading ZED SDK ..." -cd /tmp -if [ "$(uname -m)" == "aarch64" ]; then -curl -L https://stereolabs.sfo2.cdn.digitaloceanspaces.com/zedsdk/4.2/ZED_SDK_Tegra_L4T36.4_v4.2.2.zstd.run -o zed_sdk_installer.run -else -curl -L https://download.stereolabs.com/zedsdk/4.2/cu12/ubuntu22 -o zed_sdk_installer.run -fi -chmod +x zed_sdk_installer.run -./zed_sdk_installer.run -- silent -echo "Finished downloading ZED SDK ..." - -echo "Building Kindr ..." -cd /tmp -git clone https://github.com/ANYbotics/kindr.git -cd kindr/ -mkdir build -cd build -cmake .. -DUSE_CMAKE=true -sudo make install -echo "Finished building Kindr ..." \ No newline at end of file diff --git a/setup/install_ros_humble.sh b/setup/install_ros_humble.sh deleted file mode 100755 index 924dfe4c..00000000 --- a/setup/install_ros_humble.sh +++ /dev/null @@ -1,64 +0,0 @@ -locale # check for UTF-8 - -set -e - -sudo apt update && sudo apt install -y locales -sudo locale-gen en_US en_US.UTF-8 -sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 -export LANG=en_US.UTF-8 - -locale # verify settings - -sudo apt install -y software-properties-common -sudo add-apt-repository -y universe - -sudo apt update && sudo apt install curl -y -sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg - -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" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null - -sudo apt update - -sudo apt upgrade -y - -sudo apt install -y \ -ros-humble-desktop \ -ros-dev-tools \ -python3-dev \ -python3-pip \ -clang-format \ -clang-tidy \ -ninja-build \ -ros-humble-rmw-cyclonedds-cpp \ -ros-humble-rosbridge-server \ -python3-usb - -source /opt/ros/humble/setup.bash - -if ! grep -q "source /opt/ros/humble/setup.bash" ~/.bashrc; then - echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc - echo "ROS 2 sourced in bashrc" -fi - -pip3 install black -pip3 install pylint -pip3 install pyserial -pip3 install adafruit-circuitpython-lis3dh -pip3 install adafruit-circuitpython-pca9685 -pip3 install adafruit-circuitpython-motorkit -pip3 install Adafruit-Blinka -pip3 install adafruit-circuitpython-bme280 -pip3 install adafruit-circuitpython-ens160 -pip3 install adafruit-circuitpython-busdevice -pip3 install adafruit-circuitpython-register -pip3 install adafruit-circuitpython-neopixel-spi -pip3 install pyubx2 -pip3 install numpy -pip3 install pyusb==1.2.1 - -sudo rosdep init -rosdep update - -sudo usermod -a -G dialout $USER - -echo "ROS2 humble install complete!" \ No newline at end of file diff --git a/setup/science/setup_science.sh b/setup/science/setup_science.sh deleted file mode 100755 index 22a9e0fb..00000000 --- a/setup/science/setup_science.sh +++ /dev/null @@ -1,70 +0,0 @@ -#!/bin/bash -SCRIPT_DIR="$(dirname "$0")" -CURRENT_DIR="$(pwd)" -CURRENT_USER=$(whoami) -VERSION="science-1.0.0" - -# Check if the script is being run from its own directory -if [ "$SCRIPT_DIR" == "$CURRENT_DIR" ] || [ "$SCRIPT_DIR" == "." ]; then - echo "The script is being run from its own directory." -else - echo "The script is NOT being run from its own directory." - exit 1 -fi - -# Install GStreamer dependencies -sudo apt-get install -y \ - libgstreamer1.0-dev \ - libgstreamer-plugins-base1.0-dev \ - libgstreamer-plugins-bad1.0-dev \ - gstreamer1.0-plugins-base \ - gstreamer1.0-plugins-good \ - gstreamer1.0-plugins-bad \ - gstreamer1.0-plugins-ugly \ - gstreamer1.0-libav \ - gstreamer1.0-tools \ - gstreamer1.0-x \ - gstreamer1.0-gl \ - gstreamer1.0-gtk3 \ - gstreamer1.0-qt5 \ - libx264-dev \ - libjpeg-dev \ - gstreamer1.0-libcamera - -if ! grep -q "^dtoverlay=pwm-2chan" /boot/firmware/config.txt; then - sudo bash -c 'echo "dtoverlay=pwm-2chan" >> /boot/firmware/config.txt' -fi - -# Install Docker -if ! command -v docker &> /dev/null; then - echo "Docker not found, installing Docker..." - - # Update apt package list - sudo apt-get update - - sudo apt-get install -y docker.io - - # Verify Docker installation - sudo systemctl enable --now docker - sudo usermod -aG docker $CURRENT_USER - echo "Docker installed and configured." -else - echo "Docker is already installed." -fi - -# Build the Docker image for the project -echo "Pulling Docker image..." -docker pull cprtsoftware/cprt_rover_24:${VERSION} - -# Setup science service -sudo cp start_science.service /etc/systemd/system/start_science.service -sudo sed -i "s|User=%i|User=${CURRENT_USER}|" /etc/systemd/system/start_science.service -sudo chmod 644 /etc/systemd/system/start_science.service -sudo ln -s $PWD/start_science.sh /usr/local/bin/start_science.sh - -# Reload systemd, enable, and start the service -sudo systemctl daemon-reload -sudo systemctl enable start_science.service -sudo systemctl start start_science.service - -echo "Setup completed successfully." diff --git a/setup/science/start_science.service b/setup/science/start_science.service deleted file mode 100644 index bd4304a8..00000000 --- a/setup/science/start_science.service +++ /dev/null @@ -1,11 +0,0 @@ -[Unit] -Description=Start ROS 2 Node on Boot -After=network.target - -[Service] -Type=simple -User=%i -ExecStart=/usr/local/bin/start_science.sh - -[Install] -WantedBy=multi-user.target \ No newline at end of file diff --git a/setup/science/start_science.sh b/setup/science/start_science.sh deleted file mode 100755 index 34e32060..00000000 --- a/setup/science/start_science.sh +++ /dev/null @@ -1,32 +0,0 @@ -#!/bin/bash - -IMAGE=cprtsoftware/cprt_rover_24 -TAG=science-latest -JETSON_IP=192.168.0.55 -PORT=9000 - -# Stop containers on exit -cleanup() { - echo "Stopping containers..." - docker stop servo_container sensor_container - exit 0 -} - -trap cleanup SIGINT SIGTERM SIGHUP EXIT - -# Shared args -DOCKER_ARGS="--network host --privileged -e RMW_IMPLEMENTATION=${RMW_IMPLEMENTATION} -v /dev/gpiomem:/dev/gpiomem" - -# Run containers in detached mode -docker run -d --rm --name servo_container ${DOCKER_ARGS} ${IMAGE}:${TAG} \ - ros2 launch servo_pkg pi_servo_launch.launch.py - -docker run -d --rm --name sensor_container ${DOCKER_ARGS} ${IMAGE}:${TAG} \ - ros2 launch science_sensors gas_sensor.launch.py - -docker run -d --rm --name gpio_container ${DOCKER_ARGS} ${IMAGE}:${TAG} \ - ros2 launch science_sensors gpio.launch.py - -gst-launch-1.0 libcamerasrc ! queue ! jpegenc ! jpegparse ! rtpjpegpay ! queue ! udpsink host=${JETSON_IP} port=${PORT} - -wait diff --git a/setup/start_rover.sh b/setup/start_rover.sh index ceffaed3..bdf37417 100755 --- a/setup/start_rover.sh +++ b/setup/start_rover.sh @@ -2,4 +2,5 @@ source ~/gstreamer/setupGstreamer.sh source /opt/ros/humble/setup.bash source /opt/ros/humble/cprt_setup.bash +sudo enablecan.sh ros2 launch rosbridge_server rosbridge_websocket_launch.xml \ No newline at end of file diff --git a/setup/tmux_config.conf b/setup/tmux_config.conf deleted file mode 100644 index 5877548a..00000000 --- a/setup/tmux_config.conf +++ /dev/null @@ -1,40 +0,0 @@ -# Enable mouse support -set -g mouse on - -# Improve terminal colors -# This tells tmux to use a 256-color terminal. -# You might need to adjust this depending on your actual terminal emulator outside of tmux. -set -g default-terminal "screen-256color" - -# Ensure colors are passed through properly -set-option -ga terminal-overrides ",xterm-256color:Tc" - -# Set a reasonable scrollback buffer size (adjust as needed) -set -g history-limit 10000 - -# Set the prefix key (Ctrl-b is default, Ctrl-a is a common alternative) -# unbind C-b -# set-option -g prefix C-a -# bind-key C-a send-prefix - -# Reload tmux configuration with 'r' key after prefix -bind r source-file ~/.tmux.conf \; display-message "tmux.conf reloaded!" - -# Smart pane switching with awareness of Vim splits. -# See: https://github.com/christoomey/vim-tmux-navigator -# is_vim="ps -o state= -o comm= -t '#{pane_tty}' | grep -iqE '(view|n?vim)'" -# bind-key -n C-h if-shell "$is_vim" "send-keys C-h" "select-pane -L" -# bind-key -n C-j if-shell "$is_vim" "send-keys C-j" "select-pane -D" -# bind-key -n C-k if-shell "$is_vim" "send-keys C-k" "select-pane -U" -# bind-key -n C-l if-shell "$is_vim" "send-keys C-l" "select-pane -R" -# bind-key -n C-\ if-shell "$is_vim" "send-keys C-\\" "select-pane -l" - -# Status bar customization (optional, but nice for hostname/user) -# This is where you can show user@hostname -set -g status-left '#[fg=green][#S] #[fg=blue]#(whoami)@#H' -set -g status-right '#[fg=cyan]%H:%M %d-%b-%y' -set -g status-bg green - -# Automatically set window title -set-option -g set-titles on -set-option -g set-titles-string '#S:#I.#P #W' \ No newline at end of file diff --git a/setup/turnOnRaman.sh b/setup/turnOnRaman.sh deleted file mode 100755 index a29d51ce..00000000 --- a/setup/turnOnRaman.sh +++ /dev/null @@ -1,30 +0,0 @@ -#!/usr/bin/env bash -# turnOnRaman.sh – run the Raman node as root - -# ----- figure out the *real* home of the user who invoked sudo ----- -ME=${SUDO_USER:-$USER} # chris -USER_HOME=$(getent passwd "$ME" | cut -d: -f6) -WS="$USER_HOME/cprt_rover_24" # /home/chris/cprt_rover_24 - -# ----- bring ROS + your workspace into root's environment ----- -source /opt/ros/humble/setup.bash -source "$WS/install/setup.bash" - -# (optional) expose your user-site packages to root as well -export PYTHONPATH="$USER_HOME/.local/lib/python3.10/site-packages:$PYTHONPATH" - -exec ros2 run science_sensors raman "$@" - - -#This is how to properly call the service since it runs as sudo -: ' - -sudo -E bash -c ' - source /opt/ros/humble/setup.bash - source ~/cprt_rover_24/install/setup.bash # if the client needs your workspace - ros2 service call /get_raman_spectrum interfaces/srv/Raman \ - "{inittime: 500, scansavg: 1, smoothing: 1}" -' - - -' \ No newline at end of file diff --git a/src/arm_srdf/.setup_assistant b/src/Arm/arm_srdf/.setup_assistant similarity index 100% rename from src/arm_srdf/.setup_assistant rename to src/Arm/arm_srdf/.setup_assistant diff --git a/src/arm_srdf/CMakeLists.txt b/src/Arm/arm_srdf/CMakeLists.txt similarity index 100% rename from src/arm_srdf/CMakeLists.txt rename to src/Arm/arm_srdf/CMakeLists.txt diff --git a/src/arm_srdf/config/arm_config.yaml b/src/Arm/arm_srdf/config/arm_config.yaml similarity index 100% rename from src/arm_srdf/config/arm_config.yaml rename to src/Arm/arm_srdf/config/arm_config.yaml diff --git a/src/arm_srdf/config/arm_urdf.ros2_control.xacro b/src/Arm/arm_srdf/config/arm_urdf.ros2_control.xacro similarity index 100% rename from src/arm_srdf/config/arm_urdf.ros2_control.xacro rename to src/Arm/arm_srdf/config/arm_urdf.ros2_control.xacro diff --git a/src/arm_srdf/config/arm_urdf.srdf b/src/Arm/arm_srdf/config/arm_urdf.srdf similarity index 100% rename from src/arm_srdf/config/arm_urdf.srdf rename to src/Arm/arm_srdf/config/arm_urdf.srdf diff --git a/src/arm_srdf/config/arm_urdf.urdf.xacro b/src/Arm/arm_srdf/config/arm_urdf.urdf.xacro similarity index 100% rename from src/arm_srdf/config/arm_urdf.urdf.xacro rename to src/Arm/arm_srdf/config/arm_urdf.urdf.xacro diff --git a/src/arm_srdf/config/initial_positions.yaml b/src/Arm/arm_srdf/config/initial_positions.yaml similarity index 100% rename from src/arm_srdf/config/initial_positions.yaml rename to src/Arm/arm_srdf/config/initial_positions.yaml diff --git a/src/arm_srdf/config/joint_limits.yaml b/src/Arm/arm_srdf/config/joint_limits.yaml similarity index 100% rename from src/arm_srdf/config/joint_limits.yaml rename to src/Arm/arm_srdf/config/joint_limits.yaml diff --git a/src/arm_srdf/config/kinematics.yaml b/src/Arm/arm_srdf/config/kinematics.yaml similarity index 100% rename from src/arm_srdf/config/kinematics.yaml rename to src/Arm/arm_srdf/config/kinematics.yaml diff --git a/src/arm_srdf/config/moveit.rviz b/src/Arm/arm_srdf/config/moveit.rviz similarity index 100% rename from src/arm_srdf/config/moveit.rviz rename to src/Arm/arm_srdf/config/moveit.rviz diff --git a/src/arm_srdf/config/moveit_controllers.yaml b/src/Arm/arm_srdf/config/moveit_controllers.yaml similarity index 100% rename from src/arm_srdf/config/moveit_controllers.yaml rename to src/Arm/arm_srdf/config/moveit_controllers.yaml diff --git a/src/arm_srdf/config/pilz_cartesian_limits.yaml b/src/Arm/arm_srdf/config/pilz_cartesian_limits.yaml similarity index 100% rename from src/arm_srdf/config/pilz_cartesian_limits.yaml rename to src/Arm/arm_srdf/config/pilz_cartesian_limits.yaml diff --git a/src/arm_srdf/config/ros2_controllers.yaml b/src/Arm/arm_srdf/config/ros2_controllers.yaml similarity index 100% rename from src/arm_srdf/config/ros2_controllers.yaml rename to src/Arm/arm_srdf/config/ros2_controllers.yaml diff --git a/src/arm_srdf/launch/demo.launch.py b/src/Arm/arm_srdf/launch/demo.launch.py similarity index 100% rename from src/arm_srdf/launch/demo.launch.py rename to src/Arm/arm_srdf/launch/demo.launch.py diff --git a/src/arm_srdf/launch/move_group.launch.py b/src/Arm/arm_srdf/launch/move_group.launch.py similarity index 100% rename from src/arm_srdf/launch/move_group.launch.py rename to src/Arm/arm_srdf/launch/move_group.launch.py diff --git a/src/arm_srdf/launch/moveit_rviz.launch.py b/src/Arm/arm_srdf/launch/moveit_rviz.launch.py similarity index 100% rename from src/arm_srdf/launch/moveit_rviz.launch.py rename to src/Arm/arm_srdf/launch/moveit_rviz.launch.py diff --git a/src/arm_srdf/launch/rsp.launch.py b/src/Arm/arm_srdf/launch/rsp.launch.py similarity index 100% rename from src/arm_srdf/launch/rsp.launch.py rename to src/Arm/arm_srdf/launch/rsp.launch.py diff --git a/src/arm_srdf/launch/servo.launch.py b/src/Arm/arm_srdf/launch/servo.launch.py similarity index 100% rename from src/arm_srdf/launch/servo.launch.py rename to src/Arm/arm_srdf/launch/servo.launch.py diff --git a/src/arm_srdf/launch/setup_assistant.launch.py b/src/Arm/arm_srdf/launch/setup_assistant.launch.py similarity index 100% rename from src/arm_srdf/launch/setup_assistant.launch.py rename to src/Arm/arm_srdf/launch/setup_assistant.launch.py diff --git a/src/arm_srdf/launch/spawn_controllers.launch.py b/src/Arm/arm_srdf/launch/spawn_controllers.launch.py similarity index 100% rename from src/arm_srdf/launch/spawn_controllers.launch.py rename to src/Arm/arm_srdf/launch/spawn_controllers.launch.py diff --git a/src/arm_srdf/launch/static_virtual_joint_tfs.launch.py b/src/Arm/arm_srdf/launch/static_virtual_joint_tfs.launch.py similarity index 100% rename from src/arm_srdf/launch/static_virtual_joint_tfs.launch.py rename to src/Arm/arm_srdf/launch/static_virtual_joint_tfs.launch.py diff --git a/src/arm_srdf/launch/talon.launch.py b/src/Arm/arm_srdf/launch/talon.launch.py similarity index 100% rename from src/arm_srdf/launch/talon.launch.py rename to src/Arm/arm_srdf/launch/talon.launch.py diff --git a/src/arm_srdf/launch/test.launch.py b/src/Arm/arm_srdf/launch/test.launch.py similarity index 100% rename from src/arm_srdf/launch/test.launch.py rename to src/Arm/arm_srdf/launch/test.launch.py diff --git a/src/arm_srdf/launch/warehouse_db.launch.py b/src/Arm/arm_srdf/launch/warehouse_db.launch.py similarity index 100% rename from src/arm_srdf/launch/warehouse_db.launch.py rename to src/Arm/arm_srdf/launch/warehouse_db.launch.py diff --git a/src/arm_srdf/package.xml b/src/Arm/arm_srdf/package.xml similarity index 100% rename from src/arm_srdf/package.xml rename to src/Arm/arm_srdf/package.xml diff --git a/src/arm_urdf/arm_urdf/__init__.py b/src/Arm/arm_urdf/arm_urdf/__init__.py similarity index 100% rename from src/arm_urdf/arm_urdf/__init__.py rename to src/Arm/arm_urdf/arm_urdf/__init__.py diff --git a/src/arm_urdf/launch/launch.py b/src/Arm/arm_urdf/launch/launch.py similarity index 100% rename from src/arm_urdf/launch/launch.py rename to src/Arm/arm_urdf/launch/launch.py diff --git a/src/arm_urdf/meshes/collision/Link_1.STL b/src/Arm/arm_urdf/meshes/collision/Link_1.STL similarity index 100% rename from src/arm_urdf/meshes/collision/Link_1.STL rename to src/Arm/arm_urdf/meshes/collision/Link_1.STL diff --git a/src/arm_urdf/meshes/collision/Link_2.STL b/src/Arm/arm_urdf/meshes/collision/Link_2.STL similarity index 100% rename from src/arm_urdf/meshes/collision/Link_2.STL rename to src/Arm/arm_urdf/meshes/collision/Link_2.STL diff --git a/src/arm_urdf/meshes/collision/Link_3.STL b/src/Arm/arm_urdf/meshes/collision/Link_3.STL similarity index 100% rename from src/arm_urdf/meshes/collision/Link_3.STL rename to src/Arm/arm_urdf/meshes/collision/Link_3.STL diff --git a/src/arm_urdf/meshes/collision/Link_4.STL b/src/Arm/arm_urdf/meshes/collision/Link_4.STL similarity index 100% rename from src/arm_urdf/meshes/collision/Link_4.STL rename to src/Arm/arm_urdf/meshes/collision/Link_4.STL diff --git a/src/arm_urdf/meshes/collision/Link_5.STL b/src/Arm/arm_urdf/meshes/collision/Link_5.STL similarity index 100% rename from src/arm_urdf/meshes/collision/Link_5.STL rename to src/Arm/arm_urdf/meshes/collision/Link_5.STL diff --git a/src/arm_urdf/meshes/collision/Link_6.STL b/src/Arm/arm_urdf/meshes/collision/Link_6.STL similarity index 100% rename from src/arm_urdf/meshes/collision/Link_6.STL rename to src/Arm/arm_urdf/meshes/collision/Link_6.STL diff --git a/src/arm_urdf/meshes/collision/Link_7.STL b/src/Arm/arm_urdf/meshes/collision/Link_7.STL similarity index 100% rename from src/arm_urdf/meshes/collision/Link_7.STL rename to src/Arm/arm_urdf/meshes/collision/Link_7.STL diff --git a/src/arm_urdf/meshes/visual/Link_1.STL b/src/Arm/arm_urdf/meshes/visual/Link_1.STL similarity index 100% rename from src/arm_urdf/meshes/visual/Link_1.STL rename to src/Arm/arm_urdf/meshes/visual/Link_1.STL diff --git a/src/arm_urdf/meshes/visual/Link_2.STL b/src/Arm/arm_urdf/meshes/visual/Link_2.STL similarity index 100% rename from src/arm_urdf/meshes/visual/Link_2.STL rename to src/Arm/arm_urdf/meshes/visual/Link_2.STL diff --git a/src/arm_urdf/meshes/visual/Link_3.STL b/src/Arm/arm_urdf/meshes/visual/Link_3.STL similarity index 100% rename from src/arm_urdf/meshes/visual/Link_3.STL rename to src/Arm/arm_urdf/meshes/visual/Link_3.STL diff --git a/src/arm_urdf/meshes/visual/Link_4.STL b/src/Arm/arm_urdf/meshes/visual/Link_4.STL similarity index 100% rename from src/arm_urdf/meshes/visual/Link_4.STL rename to src/Arm/arm_urdf/meshes/visual/Link_4.STL diff --git a/src/arm_urdf/meshes/visual/Link_5.STL b/src/Arm/arm_urdf/meshes/visual/Link_5.STL similarity index 100% rename from src/arm_urdf/meshes/visual/Link_5.STL rename to src/Arm/arm_urdf/meshes/visual/Link_5.STL diff --git a/src/arm_urdf/meshes/visual/Link_6.STL b/src/Arm/arm_urdf/meshes/visual/Link_6.STL similarity index 100% rename from src/arm_urdf/meshes/visual/Link_6.STL rename to src/Arm/arm_urdf/meshes/visual/Link_6.STL diff --git a/src/arm_urdf/meshes/visual/Link_7.STL b/src/Arm/arm_urdf/meshes/visual/Link_7.STL similarity index 100% rename from src/arm_urdf/meshes/visual/Link_7.STL rename to src/Arm/arm_urdf/meshes/visual/Link_7.STL diff --git a/src/arm_urdf/package.xml b/src/Arm/arm_urdf/package.xml similarity index 100% rename from src/arm_urdf/package.xml rename to src/Arm/arm_urdf/package.xml diff --git a/src/arm_urdf/resource/arm_urdf b/src/Arm/arm_urdf/resource/arm_urdf similarity index 100% rename from src/arm_urdf/resource/arm_urdf rename to src/Arm/arm_urdf/resource/arm_urdf diff --git a/src/arm_urdf/setup.cfg b/src/Arm/arm_urdf/setup.cfg similarity index 100% rename from src/arm_urdf/setup.cfg rename to src/Arm/arm_urdf/setup.cfg diff --git a/src/arm_urdf/setup.py b/src/Arm/arm_urdf/setup.py similarity index 100% rename from src/arm_urdf/setup.py rename to src/Arm/arm_urdf/setup.py diff --git a/src/arm_urdf/test/test_copyright.py b/src/Arm/arm_urdf/test/test_copyright.py similarity index 100% rename from src/arm_urdf/test/test_copyright.py rename to src/Arm/arm_urdf/test/test_copyright.py diff --git a/src/arm_urdf/test/test_flake8.py b/src/Arm/arm_urdf/test/test_flake8.py similarity index 100% rename from src/arm_urdf/test/test_flake8.py rename to src/Arm/arm_urdf/test/test_flake8.py diff --git a/src/arm_urdf/test/test_pep257.py b/src/Arm/arm_urdf/test/test_pep257.py similarity index 100% rename from src/arm_urdf/test/test_pep257.py rename to src/Arm/arm_urdf/test/test_pep257.py diff --git a/src/arm_urdf/urdf/arm_urdf.urdf b/src/Arm/arm_urdf/urdf/arm_urdf.urdf similarity index 100% rename from src/arm_urdf/urdf/arm_urdf.urdf rename to src/Arm/arm_urdf/urdf/arm_urdf.urdf diff --git a/src/bringup/bringup/__init__.py b/src/Bringup/bringup/__init__.py similarity index 100% rename from src/bringup/bringup/__init__.py rename to src/Bringup/bringup/__init__.py diff --git a/src/Bringup/launch/arm.launch.py b/src/Bringup/launch/arm.launch.py new file mode 100644 index 00000000..26ffa88b --- /dev/null +++ b/src/Bringup/launch/arm.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable +import os + + +def get_included_launch_descriptions(launch_files): + included_launches = [] + for pkg, file in launch_files: + pkg_share = FindPackageShare(pkg).find(pkg) + file_path = os.path.join(pkg_share, "launch", file) + included_launches.append( + IncludeLaunchDescription(PythonLaunchDescriptionSource(file_path)) + ) + return included_launches + + +def generate_launch_description(): + SetEnvironmentVariable("ROS_LOG_LEVEL", "WARN") + launch_files = [ + ("arm_srdf", "servo.launch.py"), + ("arm_srdf", "talon.launch.py"), + ] + return LaunchDescription(get_included_launch_descriptions(launch_files)) diff --git a/src/bringup/launch/arm_tasks.launch.py b/src/Bringup/launch/arm_tasks.launch.py similarity index 96% rename from src/bringup/launch/arm_tasks.launch.py rename to src/Bringup/launch/arm_tasks.launch.py index 5653c37b..7cb9d05c 100644 --- a/src/bringup/launch/arm_tasks.launch.py +++ b/src/Bringup/launch/arm_tasks.launch.py @@ -19,7 +19,7 @@ def get_included_launch_descriptions(launch_files): def generate_launch_description(): SetEnvironmentVariable("ROS_LOG_LEVEL", "warn"), launch_files = [ - ("drive", "talon.launch.py"), + ("drive_cpp", "talon.launch.py"), ("arm_srdf", "servo.launch.py"), ("arm_srdf", "talon.launch.py"), ("camera_streaming", "webRTC.launch.py"), diff --git a/src/bringup/launch/basestation.launch.py b/src/Bringup/launch/basestation.launch.py similarity index 100% rename from src/bringup/launch/basestation.launch.py rename to src/Bringup/launch/basestation.launch.py diff --git a/src/bringup/launch/basic_drive.launch.py b/src/Bringup/launch/basic_drive.launch.py similarity index 95% rename from src/bringup/launch/basic_drive.launch.py rename to src/Bringup/launch/basic_drive.launch.py index 19d1ff23..89a256e7 100644 --- a/src/bringup/launch/basic_drive.launch.py +++ b/src/Bringup/launch/basic_drive.launch.py @@ -19,7 +19,7 @@ def get_included_launch_descriptions(launch_files): def generate_launch_description(): SetEnvironmentVariable("ROS_LOG_LEVEL", "warn"), launch_files = [ - ("drive", "talon.launch.py"), + ("drive_cpp", "talon.launch.py"), ("camera_streaming", "webRTC.launch.py"), ("joystick_control", "controller.launch.py"), ("servo_pkg", "usb_servo_launch.launch.py"), diff --git a/src/Bringup/launch/core.launch.py b/src/Bringup/launch/core.launch.py new file mode 100644 index 00000000..993837eb --- /dev/null +++ b/src/Bringup/launch/core.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable +import os + + +def get_included_launch_descriptions(launch_files): + included_launches = [] + for pkg, file in launch_files: + pkg_share = FindPackageShare(pkg).find(pkg) + file_path = os.path.join(pkg_share, "launch", file) + included_launches.append( + IncludeLaunchDescription(PythonLaunchDescriptionSource(file_path)) + ) + return included_launches + + +def generate_launch_description(): + SetEnvironmentVariable("ROS_LOG_LEVEL", "WARN") + launch_files = [ + ("joystick_control", "controller.launch.py"), + ("servo_pkg", "usb_servo_launch.launch.py"), + ] + return LaunchDescription(get_included_launch_descriptions(launch_files)) diff --git a/src/Bringup/launch/drive.launch.py b/src/Bringup/launch/drive.launch.py new file mode 100644 index 00000000..4c4b1191 --- /dev/null +++ b/src/Bringup/launch/drive.launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable +import os + + +def get_included_launch_descriptions(launch_files): + included_launches = [] + for pkg, file in launch_files: + pkg_share = FindPackageShare(pkg).find(pkg) + file_path = os.path.join(pkg_share, "launch", file) + included_launches.append( + IncludeLaunchDescription(PythonLaunchDescriptionSource(file_path)) + ) + return included_launches + + +def generate_launch_description(): + launch_files = [ + ("drive_cpp", "talon.launch.py"), + ] + return LaunchDescription(get_included_launch_descriptions(launch_files)) diff --git a/src/Bringup/launch/localization.launch.py b/src/Bringup/launch/localization.launch.py new file mode 100644 index 00000000..52fb487c --- /dev/null +++ b/src/Bringup/launch/localization.launch.py @@ -0,0 +1,30 @@ +from launch import LaunchDescription +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable +import os + + +def get_included_launch_descriptions(launch_files): + included_launches = [] + for pkg, file in launch_files: + pkg_share = FindPackageShare(pkg).find(pkg) + file_path = os.path.join(pkg_share, "launch", file) + included_launches.append( + IncludeLaunchDescription(PythonLaunchDescriptionSource(file_path)) + ) + return included_launches + + +def generate_launch_description(): + SetEnvironmentVariable("ROS_LOG_LEVEL", "info"), + launch_files = [ + ("localization", "description.launch.py"), + ("localization", "ekf.launch.py"), + ("localization", "navsat.launch.py"), + ("localization", "rtabmap.launch.py"), + ("gps", "gps.launch.py"), + ("navigation", "zed.launch.py"), + ("navigation", "traversability_gridmap.launch.py"), + ] + return LaunchDescription(get_included_launch_descriptions(launch_files)) diff --git a/src/Bringup/launch/navigation.launch.py b/src/Bringup/launch/navigation.launch.py new file mode 100644 index 00000000..9c6bc4b9 --- /dev/null +++ b/src/Bringup/launch/navigation.launch.py @@ -0,0 +1,35 @@ +from launch import LaunchDescription +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable +import os + + +def get_included_launch_descriptions(launch_files): + included_launches = [] + for pkg, file in launch_files: + pkg_share = FindPackageShare(pkg).find(pkg) + file_path = os.path.join(pkg_share, "launch", file) + included_launches.append( + IncludeLaunchDescription(PythonLaunchDescriptionSource(file_path)) + ) + return included_launches + + +def generate_launch_description(): + launch_files = [ + ("navigation", "nav2.launch.py"), + ("navigation", "rtabmap.launch.py"), + ("nav_commanders", "gps_commander.launch.py"), + ] + lights_node = Node( + package="gpio_controller", + executable="lights", + name="lights_controller_node", # Assign a unique name to the node + output="screen", # Show output in the console + emulate_tty=True, # Required for output to show in terminal when using 'screen' + # parameters=[{'some_param': 'some_value'}] # Uncomment and add parameters if needed + ) + return LaunchDescription( + get_included_launch_descriptions(launch_files) + [lights_node] + ) diff --git a/src/bringup/launch/science.launch.py b/src/Bringup/launch/science.launch.py similarity index 96% rename from src/bringup/launch/science.launch.py rename to src/Bringup/launch/science.launch.py index 31a91396..f3c35c22 100644 --- a/src/bringup/launch/science.launch.py +++ b/src/Bringup/launch/science.launch.py @@ -19,7 +19,7 @@ def get_included_launch_descriptions(launch_files): def generate_launch_description(): SetEnvironmentVariable("ROS_LOG_LEVEL", "warn"), launch_files = [ - ("drive", "talon.launch.py"), + ("drive_cpp", "talon.launch.py"), ("camera_streaming", "webRTC.launch.py"), ("gps", "rover.launch.py"), ("joystick_control", "controller.launch.py"), diff --git a/src/Bringup/launch/video_streaming.launch.py b/src/Bringup/launch/video_streaming.launch.py new file mode 100644 index 00000000..1425db75 --- /dev/null +++ b/src/Bringup/launch/video_streaming.launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable +import os + + +def get_included_launch_descriptions(launch_files): + included_launches = [] + for pkg, file in launch_files: + pkg_share = FindPackageShare(pkg).find(pkg) + file_path = os.path.join(pkg_share, "launch", file) + included_launches.append( + IncludeLaunchDescription(PythonLaunchDescriptionSource(file_path)) + ) + return included_launches + + +def generate_launch_description(): + launch_files = [ + ("camera_streaming", "webRTC.launch.py"), + ] + return LaunchDescription(get_included_launch_descriptions(launch_files)) diff --git a/src/bringup/package.xml b/src/Bringup/package.xml similarity index 100% rename from src/bringup/package.xml rename to src/Bringup/package.xml diff --git a/src/bringup/resource/bringup b/src/Bringup/resource/bringup similarity index 100% rename from src/bringup/resource/bringup rename to src/Bringup/resource/bringup diff --git a/src/bringup/setup.cfg b/src/Bringup/setup.cfg similarity index 100% rename from src/bringup/setup.cfg rename to src/Bringup/setup.cfg diff --git a/src/bringup/setup.py b/src/Bringup/setup.py similarity index 100% rename from src/bringup/setup.py rename to src/Bringup/setup.py diff --git a/src/bringup/test/test_copyright.py b/src/Bringup/test/test_copyright.py similarity index 100% rename from src/bringup/test/test_copyright.py rename to src/Bringup/test/test_copyright.py diff --git a/src/bringup/test/test_flake8.py b/src/Bringup/test/test_flake8.py similarity index 100% rename from src/bringup/test/test_flake8.py rename to src/Bringup/test/test_flake8.py diff --git a/src/bringup/test/test_pep257.py b/src/Bringup/test/test_pep257.py similarity index 100% rename from src/bringup/test/test_pep257.py rename to src/Bringup/test/test_pep257.py diff --git a/src/camera_streaming/CMakeLists.txt b/src/Cameras/camera_streaming/CMakeLists.txt similarity index 100% rename from src/camera_streaming/CMakeLists.txt rename to src/Cameras/camera_streaming/CMakeLists.txt diff --git a/src/camera_streaming/config/webrtc.yaml b/src/Cameras/camera_streaming/config/webrtc.yaml similarity index 100% rename from src/camera_streaming/config/webrtc.yaml rename to src/Cameras/camera_streaming/config/webrtc.yaml diff --git a/src/camera_streaming/gst-plugins/CMakeLists.txt b/src/Cameras/camera_streaming/gst-plugins/CMakeLists.txt similarity index 100% rename from src/camera_streaming/gst-plugins/CMakeLists.txt rename to src/Cameras/camera_streaming/gst-plugins/CMakeLists.txt diff --git a/src/camera_streaming/gst-plugins/gstarucomarker.cpp b/src/Cameras/camera_streaming/gst-plugins/gstarucomarker.cpp similarity index 100% rename from src/camera_streaming/gst-plugins/gstarucomarker.cpp rename to src/Cameras/camera_streaming/gst-plugins/gstarucomarker.cpp diff --git a/src/camera_streaming/gst-plugins/gstarucomarker.hpp b/src/Cameras/camera_streaming/gst-plugins/gstarucomarker.hpp similarity index 100% rename from src/camera_streaming/gst-plugins/gstarucomarker.hpp rename to src/Cameras/camera_streaming/gst-plugins/gstarucomarker.hpp diff --git a/src/camera_streaming/hooks/gst_env_hook.sh b/src/Cameras/camera_streaming/hooks/gst_env_hook.sh similarity index 100% rename from src/camera_streaming/hooks/gst_env_hook.sh rename to src/Cameras/camera_streaming/hooks/gst_env_hook.sh diff --git a/src/camera_streaming/include/camera_streaming/webrtc_node.h b/src/Cameras/camera_streaming/include/camera_streaming/webrtc_node.h similarity index 100% rename from src/camera_streaming/include/camera_streaming/webrtc_node.h rename to src/Cameras/camera_streaming/include/camera_streaming/webrtc_node.h diff --git a/src/camera_streaming/launch/webRTC.launch.py b/src/Cameras/camera_streaming/launch/webRTC.launch.py similarity index 100% rename from src/camera_streaming/launch/webRTC.launch.py rename to src/Cameras/camera_streaming/launch/webRTC.launch.py diff --git a/src/camera_streaming/package.xml b/src/Cameras/camera_streaming/package.xml similarity index 100% rename from src/camera_streaming/package.xml rename to src/Cameras/camera_streaming/package.xml diff --git a/src/camera_streaming/src/webrtc_node.cpp b/src/Cameras/camera_streaming/src/webrtc_node.cpp similarity index 100% rename from src/camera_streaming/src/webrtc_node.cpp rename to src/Cameras/camera_streaming/src/webrtc_node.cpp diff --git a/src/camera_streaming_qt/.gitignore b/src/Cameras/camera_streaming_qt/.gitignore similarity index 100% rename from src/camera_streaming_qt/.gitignore rename to src/Cameras/camera_streaming_qt/.gitignore diff --git a/src/camera_streaming_qt/CMakeLists.txt b/src/Cameras/camera_streaming_qt/CMakeLists.txt similarity index 100% rename from src/camera_streaming_qt/CMakeLists.txt rename to src/Cameras/camera_streaming_qt/CMakeLists.txt diff --git a/src/camera_streaming_qt/include/camera_streaming_qt/camera_client.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/camera_client.h similarity index 100% rename from src/camera_streaming_qt/include/camera_streaming_qt/camera_client.h rename to src/Cameras/camera_streaming_qt/include/camera_streaming_qt/camera_client.h diff --git a/src/camera_streaming_qt/include/camera_streaming_qt/mainwindow.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/mainwindow.h similarity index 100% rename from src/camera_streaming_qt/include/camera_streaming_qt/mainwindow.h rename to src/Cameras/camera_streaming_qt/include/camera_streaming_qt/mainwindow.h diff --git a/src/camera_streaming_qt/include/camera_streaming_qt/tests/test_camera_node.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/tests/test_camera_node.h similarity index 100% rename from src/camera_streaming_qt/include/camera_streaming_qt/tests/test_camera_node.h rename to src/Cameras/camera_streaming_qt/include/camera_streaming_qt/tests/test_camera_node.h diff --git a/src/camera_streaming_qt/include/camera_streaming_qt/widgets/capture_image_widget.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/capture_image_widget.h similarity index 100% rename from src/camera_streaming_qt/include/camera_streaming_qt/widgets/capture_image_widget.h rename to src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/capture_image_widget.h diff --git a/src/camera_streaming_qt/include/camera_streaming_qt/widgets/main_widget.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/main_widget.h similarity index 100% rename from src/camera_streaming_qt/include/camera_streaming_qt/widgets/main_widget.h rename to src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/main_widget.h diff --git a/src/camera_streaming_qt/include/camera_streaming_qt/widgets/preset_widget.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/preset_widget.h similarity index 100% rename from src/camera_streaming_qt/include/camera_streaming_qt/widgets/preset_widget.h rename to src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/preset_widget.h diff --git a/src/camera_streaming_qt/include/camera_streaming_qt/widgets/source_widget.h b/src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/source_widget.h similarity index 100% rename from src/camera_streaming_qt/include/camera_streaming_qt/widgets/source_widget.h rename to src/Cameras/camera_streaming_qt/include/camera_streaming_qt/widgets/source_widget.h diff --git a/src/camera_streaming_qt/package.xml b/src/Cameras/camera_streaming_qt/package.xml similarity index 100% rename from src/camera_streaming_qt/package.xml rename to src/Cameras/camera_streaming_qt/package.xml diff --git a/src/camera_streaming_qt/src/camera_client.cpp b/src/Cameras/camera_streaming_qt/src/camera_client.cpp similarity index 100% rename from src/camera_streaming_qt/src/camera_client.cpp rename to src/Cameras/camera_streaming_qt/src/camera_client.cpp diff --git a/src/camera_streaming_qt/src/mainwindow.cpp b/src/Cameras/camera_streaming_qt/src/mainwindow.cpp similarity index 100% rename from src/camera_streaming_qt/src/mainwindow.cpp rename to src/Cameras/camera_streaming_qt/src/mainwindow.cpp diff --git a/src/camera_streaming_qt/src/tests/test_camera_node.cpp b/src/Cameras/camera_streaming_qt/src/tests/test_camera_node.cpp similarity index 100% rename from src/camera_streaming_qt/src/tests/test_camera_node.cpp rename to src/Cameras/camera_streaming_qt/src/tests/test_camera_node.cpp diff --git a/src/camera_streaming_qt/src/widgets/capture_image_widget.cpp b/src/Cameras/camera_streaming_qt/src/widgets/capture_image_widget.cpp similarity index 100% rename from src/camera_streaming_qt/src/widgets/capture_image_widget.cpp rename to src/Cameras/camera_streaming_qt/src/widgets/capture_image_widget.cpp diff --git a/src/camera_streaming_qt/src/widgets/main_widget.cpp b/src/Cameras/camera_streaming_qt/src/widgets/main_widget.cpp similarity index 100% rename from src/camera_streaming_qt/src/widgets/main_widget.cpp rename to src/Cameras/camera_streaming_qt/src/widgets/main_widget.cpp diff --git a/src/camera_streaming_qt/src/widgets/preset_widget.cpp b/src/Cameras/camera_streaming_qt/src/widgets/preset_widget.cpp similarity index 100% rename from src/camera_streaming_qt/src/widgets/preset_widget.cpp rename to src/Cameras/camera_streaming_qt/src/widgets/preset_widget.cpp diff --git a/src/camera_streaming_qt/src/widgets/source_widget.cpp b/src/Cameras/camera_streaming_qt/src/widgets/source_widget.cpp similarity index 100% rename from src/camera_streaming_qt/src/widgets/source_widget.cpp rename to src/Cameras/camera_streaming_qt/src/widgets/source_widget.cpp diff --git a/src/camera_controller/camera_controller/__init__.py b/src/Cameras/computer_vision/computer_vision/__init__.py similarity index 100% rename from src/camera_controller/camera_controller/__init__.py rename to src/Cameras/computer_vision/computer_vision/__init__.py diff --git a/src/computer_vision/computer_vision/zed_aruco_detector.py b/src/Cameras/computer_vision/computer_vision/zed_aruco_detector.py similarity index 100% rename from src/computer_vision/computer_vision/zed_aruco_detector.py rename to src/Cameras/computer_vision/computer_vision/zed_aruco_detector.py diff --git a/src/computer_vision/launch/aruco_detector_zed.launch.py b/src/Cameras/computer_vision/launch/aruco_detector_zed.launch.py similarity index 100% rename from src/computer_vision/launch/aruco_detector_zed.launch.py rename to src/Cameras/computer_vision/launch/aruco_detector_zed.launch.py diff --git a/src/computer_vision/package.xml b/src/Cameras/computer_vision/package.xml similarity index 100% rename from src/computer_vision/package.xml rename to src/Cameras/computer_vision/package.xml diff --git a/src/computer_vision/resource/computer_vision b/src/Cameras/computer_vision/resource/computer_vision similarity index 100% rename from src/computer_vision/resource/computer_vision rename to src/Cameras/computer_vision/resource/computer_vision diff --git a/src/computer_vision/setup.cfg b/src/Cameras/computer_vision/setup.cfg similarity index 100% rename from src/computer_vision/setup.cfg rename to src/Cameras/computer_vision/setup.cfg diff --git a/src/computer_vision/setup.py b/src/Cameras/computer_vision/setup.py similarity index 100% rename from src/computer_vision/setup.py rename to src/Cameras/computer_vision/setup.py diff --git a/src/camera_controller/test/test_copyright.py b/src/Cameras/computer_vision/test/test_copyright.py similarity index 100% rename from src/camera_controller/test/test_copyright.py rename to src/Cameras/computer_vision/test/test_copyright.py diff --git a/src/camera_controller/test/test_flake8.py b/src/Cameras/computer_vision/test/test_flake8.py similarity index 100% rename from src/camera_controller/test/test_flake8.py rename to src/Cameras/computer_vision/test/test_flake8.py diff --git a/src/camera_controller/test/test_pep257.py b/src/Cameras/computer_vision/test/test_pep257.py similarity index 100% rename from src/camera_controller/test/test_pep257.py rename to src/Cameras/computer_vision/test/test_pep257.py diff --git a/src/cprt_gridmap_filters/LICENSE b/src/HW-Devices/gpio_controller/LICENSE similarity index 100% rename from src/cprt_gridmap_filters/LICENSE rename to src/HW-Devices/gpio_controller/LICENSE diff --git a/src/computer_vision/computer_vision/__init__.py b/src/HW-Devices/gpio_controller/gpio_controller/__init__.py similarity index 100% rename from src/computer_vision/computer_vision/__init__.py rename to src/HW-Devices/gpio_controller/gpio_controller/__init__.py diff --git a/src/gpio_controller/gpio_controller/lights.py b/src/HW-Devices/gpio_controller/gpio_controller/lights.py similarity index 100% rename from src/gpio_controller/gpio_controller/lights.py rename to src/HW-Devices/gpio_controller/gpio_controller/lights.py diff --git a/src/gpio_controller/package.xml b/src/HW-Devices/gpio_controller/package.xml similarity index 91% rename from src/gpio_controller/package.xml rename to src/HW-Devices/gpio_controller/package.xml index 7cc44904..46f48483 100644 --- a/src/gpio_controller/package.xml +++ b/src/HW-Devices/gpio_controller/package.xml @@ -6,6 +6,7 @@ TODO: Package description chris Apache-2.0 + rclpy ament_copyright ament_flake8 @@ -13,9 +14,7 @@ python3-pytest - rospy std_msgs - rospy std_msgs ament_python diff --git a/src/gpio_controller/resource/gpio_controller b/src/HW-Devices/gpio_controller/resource/gpio_controller similarity index 100% rename from src/gpio_controller/resource/gpio_controller rename to src/HW-Devices/gpio_controller/resource/gpio_controller diff --git a/src/gpio_controller/setup.cfg b/src/HW-Devices/gpio_controller/setup.cfg similarity index 100% rename from src/gpio_controller/setup.cfg rename to src/HW-Devices/gpio_controller/setup.cfg diff --git a/src/gpio_controller/setup.py b/src/HW-Devices/gpio_controller/setup.py similarity index 100% rename from src/gpio_controller/setup.py rename to src/HW-Devices/gpio_controller/setup.py diff --git a/src/computer_vision/test/test_copyright.py b/src/HW-Devices/gpio_controller/test/test_copyright.py similarity index 100% rename from src/computer_vision/test/test_copyright.py rename to src/HW-Devices/gpio_controller/test/test_copyright.py diff --git a/src/computer_vision/test/test_flake8.py b/src/HW-Devices/gpio_controller/test/test_flake8.py similarity index 100% rename from src/computer_vision/test/test_flake8.py rename to src/HW-Devices/gpio_controller/test/test_flake8.py diff --git a/src/computer_vision/test/test_pep257.py b/src/HW-Devices/gpio_controller/test/test_pep257.py similarity index 100% rename from src/computer_vision/test/test_pep257.py rename to src/HW-Devices/gpio_controller/test/test_pep257.py diff --git a/src/hardware/CMakeLists.txt b/src/HW-Devices/hardware/CMakeLists.txt similarity index 100% rename from src/hardware/CMakeLists.txt rename to src/HW-Devices/hardware/CMakeLists.txt diff --git a/src/hardware/include/TalonSRXWrapper.hpp b/src/HW-Devices/hardware/include/TalonSRXWrapper.hpp similarity index 100% rename from src/hardware/include/TalonSRXWrapper.hpp rename to src/HW-Devices/hardware/include/TalonSRXWrapper.hpp diff --git a/src/hardware/include/rover_arm.hpp b/src/HW-Devices/hardware/include/rover_arm.hpp similarity index 100% rename from src/hardware/include/rover_arm.hpp rename to src/HW-Devices/hardware/include/rover_arm.hpp diff --git a/src/hardware/package.xml b/src/HW-Devices/hardware/package.xml similarity index 96% rename from src/hardware/package.xml rename to src/HW-Devices/hardware/package.xml index e722b579..3d64df89 100644 --- a/src/hardware/package.xml +++ b/src/HW-Devices/hardware/package.xml @@ -31,7 +31,6 @@ joint_state_publisher_gui joint_trajectory_controller robot_state_publisher - ros2_control_demo_description ros2_controllers_test_nodes ros2controlcli ros2launch diff --git a/src/hardware/ros2_control_rover_arm.xml b/src/HW-Devices/hardware/ros2_control_rover_arm.xml similarity index 100% rename from src/hardware/ros2_control_rover_arm.xml rename to src/HW-Devices/hardware/ros2_control_rover_arm.xml diff --git a/src/hardware/src/TalonSRXWrapper.cpp b/src/HW-Devices/hardware/src/TalonSRXWrapper.cpp similarity index 100% rename from src/hardware/src/TalonSRXWrapper.cpp rename to src/HW-Devices/hardware/src/TalonSRXWrapper.cpp diff --git a/src/hardware/src/rover_arm.cpp b/src/HW-Devices/hardware/src/rover_arm.cpp similarity index 100% rename from src/hardware/src/rover_arm.cpp rename to src/HW-Devices/hardware/src/rover_arm.cpp diff --git a/src/ros_phoenix_humble/.gitignore b/src/HW-Devices/ros_phoenix_humble/.gitignore similarity index 100% rename from src/ros_phoenix_humble/.gitignore rename to src/HW-Devices/ros_phoenix_humble/.gitignore diff --git a/src/ros_phoenix_humble/CMakeLists.txt b/src/HW-Devices/ros_phoenix_humble/CMakeLists.txt similarity index 100% rename from src/ros_phoenix_humble/CMakeLists.txt rename to src/HW-Devices/ros_phoenix_humble/CMakeLists.txt diff --git a/src/ros_phoenix_humble/LICENSE b/src/HW-Devices/ros_phoenix_humble/LICENSE similarity index 100% rename from src/ros_phoenix_humble/LICENSE rename to src/HW-Devices/ros_phoenix_humble/LICENSE diff --git a/src/ros_phoenix_humble/README.md b/src/HW-Devices/ros_phoenix_humble/README.md similarity index 100% rename from src/ros_phoenix_humble/README.md rename to src/HW-Devices/ros_phoenix_humble/README.md diff --git a/src/ros_phoenix_humble/include/ros_phoenix/base_node.hpp b/src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/base_node.hpp similarity index 100% rename from src/ros_phoenix_humble/include/ros_phoenix/base_node.hpp rename to src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/base_node.hpp diff --git a/src/ros_phoenix_humble/include/ros_phoenix/phoenix_bridge.hpp b/src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_bridge.hpp similarity index 100% rename from src/ros_phoenix_humble/include/ros_phoenix/phoenix_bridge.hpp rename to src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_bridge.hpp diff --git a/src/ros_phoenix_humble/include/ros_phoenix/phoenix_components.hpp b/src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_components.hpp similarity index 100% rename from src/ros_phoenix_humble/include/ros_phoenix/phoenix_components.hpp rename to src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_components.hpp diff --git a/src/ros_phoenix_humble/include/ros_phoenix/phoenix_manager.hpp b/src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_manager.hpp similarity index 100% rename from src/ros_phoenix_humble/include/ros_phoenix/phoenix_manager.hpp rename to src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_manager.hpp diff --git a/src/ros_phoenix_humble/include/ros_phoenix/phoenix_node.hpp b/src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_node.hpp similarity index 100% rename from src/ros_phoenix_humble/include/ros_phoenix/phoenix_node.hpp rename to src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_node.hpp diff --git a/src/ros_phoenix_humble/include/ros_phoenix/phoenix_nodes.hpp b/src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_nodes.hpp similarity index 100% rename from src/ros_phoenix_humble/include/ros_phoenix/phoenix_nodes.hpp rename to src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_nodes.hpp diff --git a/src/ros_phoenix_humble/include/ros_phoenix/phoenix_system.hpp b/src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_system.hpp similarity index 100% rename from src/ros_phoenix_humble/include/ros_phoenix/phoenix_system.hpp rename to src/HW-Devices/ros_phoenix_humble/include/ros_phoenix/phoenix_system.hpp diff --git a/src/ros_phoenix_humble/launch/demo.launch.py b/src/HW-Devices/ros_phoenix_humble/launch/demo.launch.py similarity index 100% rename from src/ros_phoenix_humble/launch/demo.launch.py rename to src/HW-Devices/ros_phoenix_humble/launch/demo.launch.py diff --git a/src/ros_phoenix_humble/msg/MotorControl.msg b/src/HW-Devices/ros_phoenix_humble/msg/MotorControl.msg similarity index 100% rename from src/ros_phoenix_humble/msg/MotorControl.msg rename to src/HW-Devices/ros_phoenix_humble/msg/MotorControl.msg diff --git a/src/ros_phoenix_humble/msg/MotorStatus.msg b/src/HW-Devices/ros_phoenix_humble/msg/MotorStatus.msg similarity index 100% rename from src/ros_phoenix_humble/msg/MotorStatus.msg rename to src/HW-Devices/ros_phoenix_humble/msg/MotorStatus.msg diff --git a/src/ros_phoenix_humble/package.xml b/src/HW-Devices/ros_phoenix_humble/package.xml similarity index 100% rename from src/ros_phoenix_humble/package.xml rename to src/HW-Devices/ros_phoenix_humble/package.xml diff --git a/src/ros_phoenix_humble/ros2_control/phoenix_bridge_example.ros2_control.xacro b/src/HW-Devices/ros_phoenix_humble/ros2_control/phoenix_bridge_example.ros2_control.xacro similarity index 100% rename from src/ros_phoenix_humble/ros2_control/phoenix_bridge_example.ros2_control.xacro rename to src/HW-Devices/ros_phoenix_humble/ros2_control/phoenix_bridge_example.ros2_control.xacro diff --git a/src/ros_phoenix_humble/ros2_control/phoenix_system_example.ros2_control.xacro b/src/HW-Devices/ros_phoenix_humble/ros2_control/phoenix_system_example.ros2_control.xacro similarity index 100% rename from src/ros_phoenix_humble/ros2_control/phoenix_system_example.ros2_control.xacro rename to src/HW-Devices/ros_phoenix_humble/ros2_control/phoenix_system_example.ros2_control.xacro diff --git a/src/ros_phoenix_humble/ros_phoenix_hardware.xml b/src/HW-Devices/ros_phoenix_humble/ros_phoenix_hardware.xml similarity index 100% rename from src/ros_phoenix_humble/ros_phoenix_hardware.xml rename to src/HW-Devices/ros_phoenix_humble/ros_phoenix_hardware.xml diff --git a/src/ros_phoenix_humble/src/base_node.cpp b/src/HW-Devices/ros_phoenix_humble/src/base_node.cpp similarity index 100% rename from src/ros_phoenix_humble/src/base_node.cpp rename to src/HW-Devices/ros_phoenix_humble/src/base_node.cpp diff --git a/src/ros_phoenix_humble/src/phoenix_bridge.cpp b/src/HW-Devices/ros_phoenix_humble/src/phoenix_bridge.cpp similarity index 100% rename from src/ros_phoenix_humble/src/phoenix_bridge.cpp rename to src/HW-Devices/ros_phoenix_humble/src/phoenix_bridge.cpp diff --git a/src/ros_phoenix_humble/src/phoenix_components.cpp b/src/HW-Devices/ros_phoenix_humble/src/phoenix_components.cpp similarity index 100% rename from src/ros_phoenix_humble/src/phoenix_components.cpp rename to src/HW-Devices/ros_phoenix_humble/src/phoenix_components.cpp diff --git a/src/ros_phoenix_humble/src/phoenix_container.cpp b/src/HW-Devices/ros_phoenix_humble/src/phoenix_container.cpp similarity index 100% rename from src/ros_phoenix_humble/src/phoenix_container.cpp rename to src/HW-Devices/ros_phoenix_humble/src/phoenix_container.cpp diff --git a/src/ros_phoenix_humble/src/phoenix_manager.cpp b/src/HW-Devices/ros_phoenix_humble/src/phoenix_manager.cpp similarity index 100% rename from src/ros_phoenix_humble/src/phoenix_manager.cpp rename to src/HW-Devices/ros_phoenix_humble/src/phoenix_manager.cpp diff --git a/src/ros_phoenix_humble/src/phoenix_nodes.cpp b/src/HW-Devices/ros_phoenix_humble/src/phoenix_nodes.cpp similarity index 100% rename from src/ros_phoenix_humble/src/phoenix_nodes.cpp rename to src/HW-Devices/ros_phoenix_humble/src/phoenix_nodes.cpp diff --git a/src/ros_phoenix_humble/src/phoenix_system.cpp b/src/HW-Devices/ros_phoenix_humble/src/phoenix_system.cpp similarity index 100% rename from src/ros_phoenix_humble/src/phoenix_system.cpp rename to src/HW-Devices/ros_phoenix_humble/src/phoenix_system.cpp diff --git a/src/science_sensors/README.md b/src/HW-Devices/science_sensors/README.md similarity index 100% rename from src/science_sensors/README.md rename to src/HW-Devices/science_sensors/README.md diff --git a/src/science_sensors/launch/can_module_reader.launch.py b/src/HW-Devices/science_sensors/launch/can_module_reader.launch.py similarity index 100% rename from src/science_sensors/launch/can_module_reader.launch.py rename to src/HW-Devices/science_sensors/launch/can_module_reader.launch.py diff --git a/src/science_sensors/launch/gas_sensor.launch.py b/src/HW-Devices/science_sensors/launch/gas_sensor.launch.py similarity index 100% rename from src/science_sensors/launch/gas_sensor.launch.py rename to src/HW-Devices/science_sensors/launch/gas_sensor.launch.py diff --git a/src/science_sensors/launch/gpio.launch.py b/src/HW-Devices/science_sensors/launch/gpio.launch.py similarity index 100% rename from src/science_sensors/launch/gpio.launch.py rename to src/HW-Devices/science_sensors/launch/gpio.launch.py diff --git a/src/science_sensors/launch/panoramic.launch.py b/src/HW-Devices/science_sensors/launch/panoramic.launch.py similarity index 100% rename from src/science_sensors/launch/panoramic.launch.py rename to src/HW-Devices/science_sensors/launch/panoramic.launch.py diff --git a/src/science_sensors/launch/talon.launch.py b/src/HW-Devices/science_sensors/launch/talon.launch.py similarity index 100% rename from src/science_sensors/launch/talon.launch.py rename to src/HW-Devices/science_sensors/launch/talon.launch.py diff --git a/src/science_sensors/package.xml b/src/HW-Devices/science_sensors/package.xml similarity index 100% rename from src/science_sensors/package.xml rename to src/HW-Devices/science_sensors/package.xml diff --git a/src/science_sensors/resource/science_sensors b/src/HW-Devices/science_sensors/resource/science_sensors similarity index 100% rename from src/science_sensors/resource/science_sensors rename to src/HW-Devices/science_sensors/resource/science_sensors diff --git a/src/drive/drive/__init__.py b/src/HW-Devices/science_sensors/science_sensors/__init__.py similarity index 100% rename from src/drive/drive/__init__.py rename to src/HW-Devices/science_sensors/science_sensors/__init__.py diff --git a/src/science_sensors/science_sensors/can_module_sensor.py b/src/HW-Devices/science_sensors/science_sensors/can_module_sensor.py similarity index 100% rename from src/science_sensors/science_sensors/can_module_sensor.py rename to src/HW-Devices/science_sensors/science_sensors/can_module_sensor.py diff --git a/src/science_sensors/science_sensors/gas_sensor.py b/src/HW-Devices/science_sensors/science_sensors/gas_sensor.py similarity index 100% rename from src/science_sensors/science_sensors/gas_sensor.py rename to src/HW-Devices/science_sensors/science_sensors/gas_sensor.py diff --git a/src/science_sensors/science_sensors/microscope_control.py b/src/HW-Devices/science_sensors/science_sensors/microscope_control.py similarity index 100% rename from src/science_sensors/science_sensors/microscope_control.py rename to src/HW-Devices/science_sensors/science_sensors/microscope_control.py diff --git a/src/science_sensors/science_sensors/panoramic.py b/src/HW-Devices/science_sensors/science_sensors/panoramic.py similarity index 100% rename from src/science_sensors/science_sensors/panoramic.py rename to src/HW-Devices/science_sensors/science_sensors/panoramic.py diff --git a/src/science_sensors/science_sensors/pi_gpio_controller.py b/src/HW-Devices/science_sensors/science_sensors/pi_gpio_controller.py similarity index 100% rename from src/science_sensors/science_sensors/pi_gpio_controller.py rename to src/HW-Devices/science_sensors/science_sensors/pi_gpio_controller.py diff --git a/src/science_sensors/science_sensors/pi_gpio_reader.py b/src/HW-Devices/science_sensors/science_sensors/pi_gpio_reader.py similarity index 100% rename from src/science_sensors/science_sensors/pi_gpio_reader.py rename to src/HW-Devices/science_sensors/science_sensors/pi_gpio_reader.py diff --git a/src/science_sensors/science_sensors/plot.py b/src/HW-Devices/science_sensors/science_sensors/plot.py similarity index 100% rename from src/science_sensors/science_sensors/plot.py rename to src/HW-Devices/science_sensors/science_sensors/plot.py diff --git a/src/science_sensors/science_sensors/raman.py b/src/HW-Devices/science_sensors/science_sensors/raman.py similarity index 100% rename from src/science_sensors/science_sensors/raman.py rename to src/HW-Devices/science_sensors/science_sensors/raman.py diff --git a/src/science_sensors/science_sensors/stellarnet_demo.py b/src/HW-Devices/science_sensors/science_sensors/stellarnet_demo.py similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_demo.py rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_demo.py diff --git a/src/gpio_controller/gpio_controller/__init__.py b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/__init__.py similarity index 100% rename from src/gpio_controller/gpio_controller/__init__.py rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/__init__.py diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win32.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win32.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win32.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win_amd64.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win_amd64.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win_amd64.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win32.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win32.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win32.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win_amd64.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win_amd64.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win_amd64.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp311-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp311-win_amd64.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp311-win_amd64.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp311-win_amd64.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp312-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp312-win_amd64.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp312-win_amd64.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp312-win_amd64.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win32.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win32.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win32.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win_amd64.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win_amd64.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win_amd64.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win32.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win32.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win32.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win_amd64.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win_amd64.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win_amd64.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win32.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win32.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win32.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win_amd64.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win_amd64.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win_amd64.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win32.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win32.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win32.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win_amd64.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win_amd64.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win_amd64.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win32.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win32.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win32.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win32.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win_amd64.pyd b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win_amd64.pyd similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win_amd64.pyd rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win_amd64.pyd diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-aarch64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-aarch64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-aarch64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-arm-linux-gnueabihf.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-arm-linux-gnueabihf.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-arm-linux-gnueabihf.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-arm-linux-gnueabihf.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-darwin.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-darwin.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-darwin.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-darwin.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-x86_64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-x86_64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-x86_64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-aarch64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-aarch64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-aarch64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-arm-linux-gnueabihf.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-arm-linux-gnueabihf.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-arm-linux-gnueabihf.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-arm-linux-gnueabihf.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-darwin.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-darwin.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-darwin.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-darwin.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-x86_64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-x86_64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-x86_64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-aarch64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-aarch64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-aarch64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-darwin.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-darwin.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-darwin.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-darwin.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-x86_64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-x86_64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-x86_64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-35m-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-35m-x86_64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-35m-x86_64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-35m-x86_64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-aarch64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-aarch64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-aarch64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-x86_64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-x86_64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-x86_64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-aarch64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-aarch64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-aarch64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-arm-linux-gnueabihf.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-arm-linux-gnueabihf.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-arm-linux-gnueabihf.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-arm-linux-gnueabihf.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-darwin.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-darwin.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-darwin.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-darwin.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-x86_64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-x86_64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-x86_64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-aarch64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-aarch64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-aarch64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-arm-linux-gnueabihf.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-arm-linux-gnueabihf.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-arm-linux-gnueabihf.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-arm-linux-gnueabihf.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-darwin.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-darwin.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-darwin.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-darwin.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-x86_64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-x86_64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-x86_64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-aarch64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-aarch64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-aarch64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-aarch64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-darwin.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-darwin.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-darwin.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-darwin.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-x86_64-linux-gnu.so b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-x86_64-linux-gnu.so similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-x86_64-linux-gnu.so rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-x86_64-linux-gnu.so diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/InstallDriver.exe b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/InstallDriver.exe similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/InstallDriver.exe rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/InstallDriver.exe diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.cat b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.cat similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.cat rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.cat diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.inf b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.inf similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.inf rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.inf diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst32.exe b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst32.exe similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst32.exe rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst32.exe diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst64.exe b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst64.exe similarity index 100% rename from src/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst64.exe rename to src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst64.exe diff --git a/src/science_sensors/setup.cfg b/src/HW-Devices/science_sensors/setup.cfg similarity index 100% rename from src/science_sensors/setup.cfg rename to src/HW-Devices/science_sensors/setup.cfg diff --git a/src/science_sensors/setup.py b/src/HW-Devices/science_sensors/setup.py similarity index 100% rename from src/science_sensors/setup.py rename to src/HW-Devices/science_sensors/setup.py diff --git a/src/gpio_controller/test/test_copyright.py b/src/HW-Devices/science_sensors/test/test_copyright.py similarity index 100% rename from src/gpio_controller/test/test_copyright.py rename to src/HW-Devices/science_sensors/test/test_copyright.py diff --git a/src/drive/test/test_flake8.py b/src/HW-Devices/science_sensors/test/test_flake8.py similarity index 100% rename from src/drive/test/test_flake8.py rename to src/HW-Devices/science_sensors/test/test_flake8.py diff --git a/src/drive/test/test_pep257.py b/src/HW-Devices/science_sensors/test/test_pep257.py similarity index 100% rename from src/drive/test/test_pep257.py rename to src/HW-Devices/science_sensors/test/test_pep257.py diff --git a/src/servo_pkg/README.md b/src/HW-Devices/servo_pkg/README.md similarity index 100% rename from src/servo_pkg/README.md rename to src/HW-Devices/servo_pkg/README.md diff --git a/src/servo_pkg/config/pi_controller.yaml b/src/HW-Devices/servo_pkg/config/pi_controller.yaml similarity index 100% rename from src/servo_pkg/config/pi_controller.yaml rename to src/HW-Devices/servo_pkg/config/pi_controller.yaml diff --git a/src/servo_pkg/config/usb_controller.yaml b/src/HW-Devices/servo_pkg/config/usb_controller.yaml similarity index 100% rename from src/servo_pkg/config/usb_controller.yaml rename to src/HW-Devices/servo_pkg/config/usb_controller.yaml diff --git a/src/servo_pkg/launch/i2c_servo_launch.launch.py b/src/HW-Devices/servo_pkg/launch/i2c_servo_launch.launch.py similarity index 100% rename from src/servo_pkg/launch/i2c_servo_launch.launch.py rename to src/HW-Devices/servo_pkg/launch/i2c_servo_launch.launch.py diff --git a/src/servo_pkg/launch/pi_servo_launch.launch.py b/src/HW-Devices/servo_pkg/launch/pi_servo_launch.launch.py similarity index 100% rename from src/servo_pkg/launch/pi_servo_launch.launch.py rename to src/HW-Devices/servo_pkg/launch/pi_servo_launch.launch.py diff --git a/src/servo_pkg/launch/usb_servo_launch.launch.py b/src/HW-Devices/servo_pkg/launch/usb_servo_launch.launch.py similarity index 100% rename from src/servo_pkg/launch/usb_servo_launch.launch.py rename to src/HW-Devices/servo_pkg/launch/usb_servo_launch.launch.py diff --git a/src/servo_pkg/package.xml b/src/HW-Devices/servo_pkg/package.xml similarity index 100% rename from src/servo_pkg/package.xml rename to src/HW-Devices/servo_pkg/package.xml diff --git a/src/servo_pkg/resource/servo_pkg b/src/HW-Devices/servo_pkg/resource/servo_pkg similarity index 100% rename from src/servo_pkg/resource/servo_pkg rename to src/HW-Devices/servo_pkg/resource/servo_pkg diff --git a/src/servo_pkg/servo_pkg/USB_Servo.py b/src/HW-Devices/servo_pkg/servo_pkg/USB_Servo.py similarity index 100% rename from src/servo_pkg/servo_pkg/USB_Servo.py rename to src/HW-Devices/servo_pkg/servo_pkg/USB_Servo.py diff --git a/src/gps/gps/__init__.py b/src/HW-Devices/servo_pkg/servo_pkg/__init__.py similarity index 100% rename from src/gps/gps/__init__.py rename to src/HW-Devices/servo_pkg/servo_pkg/__init__.py diff --git a/src/servo_pkg/servo_pkg/i2c_Servo.py b/src/HW-Devices/servo_pkg/servo_pkg/i2c_Servo.py similarity index 100% rename from src/servo_pkg/servo_pkg/i2c_Servo.py rename to src/HW-Devices/servo_pkg/servo_pkg/i2c_Servo.py diff --git a/src/servo_pkg/servo_pkg/maestro.py b/src/HW-Devices/servo_pkg/servo_pkg/maestro.py similarity index 100% rename from src/servo_pkg/servo_pkg/maestro.py rename to src/HW-Devices/servo_pkg/servo_pkg/maestro.py diff --git a/src/servo_pkg/servo_pkg/pi_Servo.py b/src/HW-Devices/servo_pkg/servo_pkg/pi_Servo.py similarity index 100% rename from src/servo_pkg/servo_pkg/pi_Servo.py rename to src/HW-Devices/servo_pkg/servo_pkg/pi_Servo.py diff --git a/src/servo_pkg/servo_pkg/servo_client.py b/src/HW-Devices/servo_pkg/servo_pkg/servo_client.py similarity index 100% rename from src/servo_pkg/servo_pkg/servo_client.py rename to src/HW-Devices/servo_pkg/servo_pkg/servo_client.py diff --git a/src/servo_pkg/setup.cfg b/src/HW-Devices/servo_pkg/setup.cfg similarity index 100% rename from src/servo_pkg/setup.cfg rename to src/HW-Devices/servo_pkg/setup.cfg diff --git a/src/servo_pkg/setup.py b/src/HW-Devices/servo_pkg/setup.py similarity index 100% rename from src/servo_pkg/setup.py rename to src/HW-Devices/servo_pkg/setup.py diff --git a/src/gps/test/test_copyright.py b/src/HW-Devices/servo_pkg/test/test_copyright.py similarity index 100% rename from src/gps/test/test_copyright.py rename to src/HW-Devices/servo_pkg/test/test_copyright.py diff --git a/src/gpio_controller/test/test_flake8.py b/src/HW-Devices/servo_pkg/test/test_flake8.py similarity index 100% rename from src/gpio_controller/test/test_flake8.py rename to src/HW-Devices/servo_pkg/test/test_flake8.py diff --git a/src/gpio_controller/test/test_pep257.py b/src/HW-Devices/servo_pkg/test/test_pep257.py similarity index 100% rename from src/gpio_controller/test/test_pep257.py rename to src/HW-Devices/servo_pkg/test/test_pep257.py diff --git a/src/cprt_costmap_plugins/CMakeLists.txt b/src/Nav/cprt_costmap_plugins/CMakeLists.txt similarity index 100% rename from src/cprt_costmap_plugins/CMakeLists.txt rename to src/Nav/cprt_costmap_plugins/CMakeLists.txt diff --git a/src/cprt_costmap_plugins/costmap_plugins.xml b/src/Nav/cprt_costmap_plugins/costmap_plugins.xml similarity index 100% rename from src/cprt_costmap_plugins/costmap_plugins.xml rename to src/Nav/cprt_costmap_plugins/costmap_plugins.xml diff --git a/src/cprt_costmap_plugins/include/cprt_costmap_plugins/gridmap_layer.hpp b/src/Nav/cprt_costmap_plugins/include/cprt_costmap_plugins/gridmap_layer.hpp similarity index 100% rename from src/cprt_costmap_plugins/include/cprt_costmap_plugins/gridmap_layer.hpp rename to src/Nav/cprt_costmap_plugins/include/cprt_costmap_plugins/gridmap_layer.hpp diff --git a/src/cprt_costmap_plugins/package.xml b/src/Nav/cprt_costmap_plugins/package.xml similarity index 100% rename from src/cprt_costmap_plugins/package.xml rename to src/Nav/cprt_costmap_plugins/package.xml diff --git a/src/cprt_costmap_plugins/src/gridmap_layer.cpp b/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp similarity index 100% rename from src/cprt_costmap_plugins/src/gridmap_layer.cpp rename to src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp diff --git a/src/cprt_gridmap_filters/CMakeLists.txt b/src/Nav/cprt_gridmap_filters/CMakeLists.txt similarity index 100% rename from src/cprt_gridmap_filters/CMakeLists.txt rename to src/Nav/cprt_gridmap_filters/CMakeLists.txt diff --git a/src/cprt_planner_plugins/LICENSE b/src/Nav/cprt_gridmap_filters/LICENSE similarity index 100% rename from src/cprt_planner_plugins/LICENSE rename to src/Nav/cprt_gridmap_filters/LICENSE diff --git a/src/cprt_gridmap_filters/filter_plugins.xml b/src/Nav/cprt_gridmap_filters/filter_plugins.xml similarity index 100% rename from src/cprt_gridmap_filters/filter_plugins.xml rename to src/Nav/cprt_gridmap_filters/filter_plugins.xml diff --git a/src/cprt_gridmap_filters/include/PreserveCostInflationFilter.hpp b/src/Nav/cprt_gridmap_filters/include/PreserveCostInflationFilter.hpp similarity index 100% rename from src/cprt_gridmap_filters/include/PreserveCostInflationFilter.hpp rename to src/Nav/cprt_gridmap_filters/include/PreserveCostInflationFilter.hpp diff --git a/src/cprt_gridmap_filters/package.xml b/src/Nav/cprt_gridmap_filters/package.xml similarity index 100% rename from src/cprt_gridmap_filters/package.xml rename to src/Nav/cprt_gridmap_filters/package.xml diff --git a/src/cprt_gridmap_filters/scripts/calculate_decay_rate.py b/src/Nav/cprt_gridmap_filters/scripts/calculate_decay_rate.py similarity index 100% rename from src/cprt_gridmap_filters/scripts/calculate_decay_rate.py rename to src/Nav/cprt_gridmap_filters/scripts/calculate_decay_rate.py diff --git a/src/cprt_gridmap_filters/scripts/visualize_decay_rate.py b/src/Nav/cprt_gridmap_filters/scripts/visualize_decay_rate.py similarity index 100% rename from src/cprt_gridmap_filters/scripts/visualize_decay_rate.py rename to src/Nav/cprt_gridmap_filters/scripts/visualize_decay_rate.py diff --git a/src/cprt_gridmap_filters/src/PreserveCostInflationFilter.cpp b/src/Nav/cprt_gridmap_filters/src/PreserveCostInflationFilter.cpp similarity index 100% rename from src/cprt_gridmap_filters/src/PreserveCostInflationFilter.cpp rename to src/Nav/cprt_gridmap_filters/src/PreserveCostInflationFilter.cpp diff --git a/src/cprt_planner_plugins/CMakeLists.txt b/src/Nav/cprt_planner_plugins/CMakeLists.txt similarity index 100% rename from src/cprt_planner_plugins/CMakeLists.txt rename to src/Nav/cprt_planner_plugins/CMakeLists.txt diff --git a/src/gpio_controller/LICENSE b/src/Nav/cprt_planner_plugins/LICENSE similarity index 100% rename from src/gpio_controller/LICENSE rename to src/Nav/cprt_planner_plugins/LICENSE diff --git a/src/cprt_planner_plugins/global_planner_plugin.xml b/src/Nav/cprt_planner_plugins/global_planner_plugin.xml similarity index 100% rename from src/cprt_planner_plugins/global_planner_plugin.xml rename to src/Nav/cprt_planner_plugins/global_planner_plugin.xml diff --git a/src/cprt_planner_plugins/include/cprt_planner_plugins/event_horizon_planner.hpp b/src/Nav/cprt_planner_plugins/include/cprt_planner_plugins/event_horizon_planner.hpp similarity index 100% rename from src/cprt_planner_plugins/include/cprt_planner_plugins/event_horizon_planner.hpp rename to src/Nav/cprt_planner_plugins/include/cprt_planner_plugins/event_horizon_planner.hpp diff --git a/src/cprt_planner_plugins/package.xml b/src/Nav/cprt_planner_plugins/package.xml similarity index 100% rename from src/cprt_planner_plugins/package.xml rename to src/Nav/cprt_planner_plugins/package.xml diff --git a/src/cprt_planner_plugins/src/event_horizon_planner.cpp b/src/Nav/cprt_planner_plugins/src/event_horizon_planner.cpp similarity index 100% rename from src/cprt_planner_plugins/src/event_horizon_planner.cpp rename to src/Nav/cprt_planner_plugins/src/event_horizon_planner.cpp diff --git a/src/elevation_mapping/CMakeLists.txt b/src/Nav/elevation_mapping/CMakeLists.txt similarity index 100% rename from src/elevation_mapping/CMakeLists.txt rename to src/Nav/elevation_mapping/CMakeLists.txt diff --git a/src/elevation_mapping/README.md b/src/Nav/elevation_mapping/README.md similarity index 100% rename from src/elevation_mapping/README.md rename to src/Nav/elevation_mapping/README.md diff --git a/src/elevation_mapping/config/elevation_maps/long_range.yaml b/src/Nav/elevation_mapping/config/elevation_maps/long_range.yaml similarity index 100% rename from src/elevation_mapping/config/elevation_maps/long_range.yaml rename to src/Nav/elevation_mapping/config/elevation_maps/long_range.yaml diff --git a/src/elevation_mapping/config/elevation_maps/remove_object.yaml b/src/Nav/elevation_mapping/config/elevation_maps/remove_object.yaml similarity index 100% rename from src/elevation_mapping/config/elevation_maps/remove_object.yaml rename to src/Nav/elevation_mapping/config/elevation_maps/remove_object.yaml diff --git a/src/elevation_mapping/config/elevation_maps/simple_demo_map.yaml b/src/Nav/elevation_mapping/config/elevation_maps/simple_demo_map.yaml similarity index 100% rename from src/elevation_mapping/config/elevation_maps/simple_demo_map.yaml rename to src/Nav/elevation_mapping/config/elevation_maps/simple_demo_map.yaml diff --git a/src/elevation_mapping/config/elevation_maps/starleth_map.yaml b/src/Nav/elevation_mapping/config/elevation_maps/starleth_map.yaml similarity index 100% rename from src/elevation_mapping/config/elevation_maps/starleth_map.yaml rename to src/Nav/elevation_mapping/config/elevation_maps/starleth_map.yaml diff --git a/src/elevation_mapping/config/elevation_maps/zed2i_map.yaml b/src/Nav/elevation_mapping/config/elevation_maps/zed2i_map.yaml similarity index 100% rename from src/elevation_mapping/config/elevation_maps/zed2i_map.yaml rename to src/Nav/elevation_mapping/config/elevation_maps/zed2i_map.yaml diff --git a/src/elevation_mapping/config/file_path.yaml b/src/Nav/elevation_mapping/config/file_path.yaml similarity index 100% rename from src/elevation_mapping/config/file_path.yaml rename to src/Nav/elevation_mapping/config/file_path.yaml diff --git a/src/elevation_mapping/config/postprocessing/postprocessor_pipeline.yaml b/src/Nav/elevation_mapping/config/postprocessing/postprocessor_pipeline.yaml similarity index 100% rename from src/elevation_mapping/config/postprocessing/postprocessor_pipeline.yaml rename to src/Nav/elevation_mapping/config/postprocessing/postprocessor_pipeline.yaml diff --git a/src/elevation_mapping/config/postprocessing/traversability_pipeline.yaml b/src/Nav/elevation_mapping/config/postprocessing/traversability_pipeline.yaml similarity index 100% rename from src/elevation_mapping/config/postprocessing/traversability_pipeline.yaml rename to src/Nav/elevation_mapping/config/postprocessing/traversability_pipeline.yaml diff --git a/src/elevation_mapping/config/robots/ground_truth_demo.yaml b/src/Nav/elevation_mapping/config/robots/ground_truth_demo.yaml similarity index 100% rename from src/elevation_mapping/config/robots/ground_truth_demo.yaml rename to src/Nav/elevation_mapping/config/robots/ground_truth_demo.yaml diff --git a/src/elevation_mapping/config/robots/remove_object.yaml b/src/Nav/elevation_mapping/config/robots/remove_object.yaml similarity index 100% rename from src/elevation_mapping/config/robots/remove_object.yaml rename to src/Nav/elevation_mapping/config/robots/remove_object.yaml diff --git a/src/elevation_mapping/config/robots/simple_demo_robot.yaml b/src/Nav/elevation_mapping/config/robots/simple_demo_robot.yaml similarity index 100% rename from src/elevation_mapping/config/robots/simple_demo_robot.yaml rename to src/Nav/elevation_mapping/config/robots/simple_demo_robot.yaml diff --git a/src/elevation_mapping/config/robots/starleth.yaml b/src/Nav/elevation_mapping/config/robots/starleth.yaml similarity index 100% rename from src/elevation_mapping/config/robots/starleth.yaml rename to src/Nav/elevation_mapping/config/robots/starleth.yaml diff --git a/src/elevation_mapping/config/robots/waffle_robot.yaml b/src/Nav/elevation_mapping/config/robots/waffle_robot.yaml similarity index 100% rename from src/elevation_mapping/config/robots/waffle_robot.yaml rename to src/Nav/elevation_mapping/config/robots/waffle_robot.yaml diff --git a/src/elevation_mapping/config/robots/zed2i_robot.yaml b/src/Nav/elevation_mapping/config/robots/zed2i_robot.yaml similarity index 100% rename from src/elevation_mapping/config/robots/zed2i_robot.yaml rename to src/Nav/elevation_mapping/config/robots/zed2i_robot.yaml diff --git a/src/elevation_mapping/config/rosconsole.conf b/src/Nav/elevation_mapping/config/rosconsole.conf similarity index 100% rename from src/elevation_mapping/config/rosconsole.conf rename to src/Nav/elevation_mapping/config/rosconsole.conf diff --git a/src/elevation_mapping/config/sensor_processors/RSLidar-32.yaml b/src/Nav/elevation_mapping/config/sensor_processors/RSLidar-32.yaml similarity index 100% rename from src/elevation_mapping/config/sensor_processors/RSLidar-32.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/RSLidar-32.yaml diff --git a/src/elevation_mapping/config/sensor_processors/aslam.yaml b/src/Nav/elevation_mapping/config/sensor_processors/aslam.yaml similarity index 100% rename from src/elevation_mapping/config/sensor_processors/aslam.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/aslam.yaml diff --git a/src/elevation_mapping/config/sensor_processors/fotonic_g45.yaml b/src/Nav/elevation_mapping/config/sensor_processors/fotonic_g45.yaml similarity index 100% rename from src/elevation_mapping/config/sensor_processors/fotonic_g45.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/fotonic_g45.yaml diff --git a/src/elevation_mapping/config/sensor_processors/hokuyo_utm-30lx.yaml b/src/Nav/elevation_mapping/config/sensor_processors/hokuyo_utm-30lx.yaml similarity index 100% rename from src/elevation_mapping/config/sensor_processors/hokuyo_utm-30lx.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/hokuyo_utm-30lx.yaml diff --git a/src/elevation_mapping/config/sensor_processors/kinect_nguyen_et_al.yaml b/src/Nav/elevation_mapping/config/sensor_processors/kinect_nguyen_et_al.yaml similarity index 100% rename from src/elevation_mapping/config/sensor_processors/kinect_nguyen_et_al.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/kinect_nguyen_et_al.yaml diff --git a/src/elevation_mapping/config/sensor_processors/perfect.yaml b/src/Nav/elevation_mapping/config/sensor_processors/perfect.yaml similarity index 100% rename from src/elevation_mapping/config/sensor_processors/perfect.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/perfect.yaml diff --git a/src/elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_datasheet.yaml b/src/Nav/elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_datasheet.yaml similarity index 100% rename from src/elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_datasheet.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_datasheet.yaml diff --git a/src/elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_uncalibrated.yaml b/src/Nav/elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_uncalibrated.yaml similarity index 100% rename from src/elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_uncalibrated.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_uncalibrated.yaml diff --git a/src/elevation_mapping/config/sensor_processors/realsense_ZR300.yaml b/src/Nav/elevation_mapping/config/sensor_processors/realsense_ZR300.yaml similarity index 100% rename from src/elevation_mapping/config/sensor_processors/realsense_ZR300.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/realsense_ZR300.yaml diff --git a/src/elevation_mapping/config/sensor_processors/realsense_d435.yaml b/src/Nav/elevation_mapping/config/sensor_processors/realsense_d435.yaml similarity index 100% rename from src/elevation_mapping/config/sensor_processors/realsense_d435.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/realsense_d435.yaml diff --git a/src/elevation_mapping/config/sensor_processors/velodyne_HDL-32E.yaml b/src/Nav/elevation_mapping/config/sensor_processors/velodyne_HDL-32E.yaml similarity index 100% rename from src/elevation_mapping/config/sensor_processors/velodyne_HDL-32E.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/velodyne_HDL-32E.yaml diff --git a/src/elevation_mapping/config/sensor_processors/zed2i_processor.yaml b/src/Nav/elevation_mapping/config/sensor_processors/zed2i_processor.yaml similarity index 100% rename from src/elevation_mapping/config/sensor_processors/zed2i_processor.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/zed2i_processor.yaml diff --git a/src/elevation_mapping/config/visualization/fused.yaml b/src/Nav/elevation_mapping/config/visualization/fused.yaml similarity index 100% rename from src/elevation_mapping/config/visualization/fused.yaml rename to src/Nav/elevation_mapping/config/visualization/fused.yaml diff --git a/src/elevation_mapping/config/visualization/raw.yaml b/src/Nav/elevation_mapping/config/visualization/raw.yaml similarity index 100% rename from src/elevation_mapping/config/visualization/raw.yaml rename to src/Nav/elevation_mapping/config/visualization/raw.yaml diff --git a/src/elevation_mapping/config/zed_configs/common_stereo.yaml b/src/Nav/elevation_mapping/config/zed_configs/common_stereo.yaml similarity index 100% rename from src/elevation_mapping/config/zed_configs/common_stereo.yaml rename to src/Nav/elevation_mapping/config/zed_configs/common_stereo.yaml diff --git a/src/elevation_mapping/config/zed_configs/zed2i.yaml b/src/Nav/elevation_mapping/config/zed_configs/zed2i.yaml similarity index 100% rename from src/elevation_mapping/config/zed_configs/zed2i.yaml rename to src/Nav/elevation_mapping/config/zed_configs/zed2i.yaml diff --git a/src/elevation_mapping/docker/.gitignore b/src/Nav/elevation_mapping/docker/.gitignore similarity index 100% rename from src/elevation_mapping/docker/.gitignore rename to src/Nav/elevation_mapping/docker/.gitignore diff --git a/src/elevation_mapping/docker/Dockerfile.elevation_mapping b/src/Nav/elevation_mapping/docker/Dockerfile.elevation_mapping similarity index 100% rename from src/elevation_mapping/docker/Dockerfile.elevation_mapping rename to src/Nav/elevation_mapping/docker/Dockerfile.elevation_mapping diff --git a/src/elevation_mapping/docker/Dockerfile.ros2_humble b/src/Nav/elevation_mapping/docker/Dockerfile.ros2_humble similarity index 100% rename from src/elevation_mapping/docker/Dockerfile.ros2_humble rename to src/Nav/elevation_mapping/docker/Dockerfile.ros2_humble diff --git a/src/elevation_mapping/docker/Dockerfile.user b/src/Nav/elevation_mapping/docker/Dockerfile.user similarity index 100% rename from src/elevation_mapping/docker/Dockerfile.user rename to src/Nav/elevation_mapping/docker/Dockerfile.user diff --git a/src/elevation_mapping/docker/build_docker.sh b/src/Nav/elevation_mapping/docker/build_docker.sh similarity index 100% rename from src/elevation_mapping/docker/build_docker.sh rename to src/Nav/elevation_mapping/docker/build_docker.sh diff --git a/src/elevation_mapping/docker/container_scripts/.dockerignore b/src/Nav/elevation_mapping/docker/container_scripts/.dockerignore similarity index 100% rename from src/elevation_mapping/docker/container_scripts/.dockerignore rename to src/Nav/elevation_mapping/docker/container_scripts/.dockerignore diff --git a/src/elevation_mapping/docker/container_scripts/workspace-entrypoint.sh b/src/Nav/elevation_mapping/docker/container_scripts/workspace-entrypoint.sh similarity index 100% rename from src/elevation_mapping/docker/container_scripts/workspace-entrypoint.sh rename to src/Nav/elevation_mapping/docker/container_scripts/workspace-entrypoint.sh diff --git a/src/elevation_mapping/docker/middleware_profiles/rtps_udp_profile.xml b/src/Nav/elevation_mapping/docker/middleware_profiles/rtps_udp_profile.xml similarity index 100% rename from src/elevation_mapping/docker/middleware_profiles/rtps_udp_profile.xml rename to src/Nav/elevation_mapping/docker/middleware_profiles/rtps_udp_profile.xml diff --git a/src/elevation_mapping/docker/rosdep/extra_rosdeps.yaml b/src/Nav/elevation_mapping/docker/rosdep/extra_rosdeps.yaml similarity index 100% rename from src/elevation_mapping/docker/rosdep/extra_rosdeps.yaml rename to src/Nav/elevation_mapping/docker/rosdep/extra_rosdeps.yaml diff --git a/src/elevation_mapping/docker/run_docker.sh b/src/Nav/elevation_mapping/docker/run_docker.sh similarity index 100% rename from src/elevation_mapping/docker/run_docker.sh rename to src/Nav/elevation_mapping/docker/run_docker.sh diff --git a/src/elevation_mapping/docker/sourced_scripts/bash_functions.sh b/src/Nav/elevation_mapping/docker/sourced_scripts/bash_functions.sh similarity index 100% rename from src/elevation_mapping/docker/sourced_scripts/bash_functions.sh rename to src/Nav/elevation_mapping/docker/sourced_scripts/bash_functions.sh diff --git a/src/elevation_mapping/docker_instructions.md b/src/Nav/elevation_mapping/docker_instructions.md similarity index 100% rename from src/elevation_mapping/docker_instructions.md rename to src/Nav/elevation_mapping/docker_instructions.md diff --git a/src/elevation_mapping/include/elevation_mapping/ElevationMap.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/ElevationMap.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/ElevationMap.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/ElevationMap.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/ElevationMapFunctors.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/ElevationMapFunctors.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/ElevationMapFunctors.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/ElevationMapFunctors.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/PointXYZRGBConfidenceRatio.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/PointXYZRGBConfidenceRatio.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/PointXYZRGBConfidenceRatio.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/PointXYZRGBConfidenceRatio.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/RobotMotionMapUpdater.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/RobotMotionMapUpdater.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/RobotMotionMapUpdater.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/RobotMotionMapUpdater.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/ThreadSafeDataWrapper.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/ThreadSafeDataWrapper.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/ThreadSafeDataWrapper.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/ThreadSafeDataWrapper.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/elevation_mapping_node.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/elevation_mapping_node.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/elevation_mapping_node.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/elevation_mapping_node.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/input_sources/Input.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/input_sources/Input.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/input_sources/Input.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/input_sources/Input.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/input_sources/InputSourceManager.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/input_sources/InputSourceManager.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/input_sources/InputSourceManager.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/input_sources/InputSourceManager.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingPipelineFunctor.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingPipelineFunctor.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingPipelineFunctor.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingPipelineFunctor.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingWorker.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingWorker.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingWorker.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingWorker.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessorPool.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessorPool.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessorPool.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessorPool.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/sensor_processors/LaserSensorProcessor.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/sensor_processors/LaserSensorProcessor.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/sensor_processors/LaserSensorProcessor.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/sensor_processors/LaserSensorProcessor.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/sensor_processors/StereoSensorProcessor.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/sensor_processors/StereoSensorProcessor.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/sensor_processors/StereoSensorProcessor.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/sensor_processors/StereoSensorProcessor.hpp diff --git a/src/elevation_mapping/include/elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp similarity index 100% rename from src/elevation_mapping/include/elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp diff --git a/src/elevation_mapping/launch/elevationMapping_launch.py b/src/Nav/elevation_mapping/launch/elevationMapping_launch.py similarity index 100% rename from src/elevation_mapping/launch/elevationMapping_launch.py rename to src/Nav/elevation_mapping/launch/elevationMapping_launch.py diff --git a/src/elevation_mapping/launch/rviz.launch.py b/src/Nav/elevation_mapping/launch/rviz.launch.py similarity index 100% rename from src/elevation_mapping/launch/rviz.launch.py rename to src/Nav/elevation_mapping/launch/rviz.launch.py diff --git a/src/elevation_mapping/launch/visualization.launch.py b/src/Nav/elevation_mapping/launch/visualization.launch.py similarity index 100% rename from src/elevation_mapping/launch/visualization.launch.py rename to src/Nav/elevation_mapping/launch/visualization.launch.py diff --git a/src/elevation_mapping/launch/zed2i.launch.py b/src/Nav/elevation_mapping/launch/zed2i.launch.py similarity index 100% rename from src/elevation_mapping/launch/zed2i.launch.py rename to src/Nav/elevation_mapping/launch/zed2i.launch.py diff --git a/src/elevation_mapping/package.xml b/src/Nav/elevation_mapping/package.xml similarity index 100% rename from src/elevation_mapping/package.xml rename to src/Nav/elevation_mapping/package.xml diff --git a/src/elevation_mapping/rviz2/custom_rviz2.rviz b/src/Nav/elevation_mapping/rviz2/custom_rviz2.rviz similarity index 100% rename from src/elevation_mapping/rviz2/custom_rviz2.rviz rename to src/Nav/elevation_mapping/rviz2/custom_rviz2.rviz diff --git a/src/elevation_mapping/rviz2/map.rviz b/src/Nav/elevation_mapping/rviz2/map.rviz similarity index 100% rename from src/elevation_mapping/rviz2/map.rviz rename to src/Nav/elevation_mapping/rviz2/map.rviz diff --git a/src/elevation_mapping/rviz2/viz.rviz b/src/Nav/elevation_mapping/rviz2/viz.rviz similarity index 100% rename from src/elevation_mapping/rviz2/viz.rviz rename to src/Nav/elevation_mapping/rviz2/viz.rviz diff --git a/src/elevation_mapping/rviz2/zed2i.rviz b/src/Nav/elevation_mapping/rviz2/zed2i.rviz similarity index 100% rename from src/elevation_mapping/rviz2/zed2i.rviz rename to src/Nav/elevation_mapping/rviz2/zed2i.rviz diff --git a/src/elevation_mapping/sample_data/maze.ply b/src/Nav/elevation_mapping/sample_data/maze.ply similarity index 100% rename from src/elevation_mapping/sample_data/maze.ply rename to src/Nav/elevation_mapping/sample_data/maze.ply diff --git a/src/elevation_mapping/src/ElevationMap.cpp b/src/Nav/elevation_mapping/src/ElevationMap.cpp similarity index 100% rename from src/elevation_mapping/src/ElevationMap.cpp rename to src/Nav/elevation_mapping/src/ElevationMap.cpp diff --git a/src/elevation_mapping/src/ElevationMapping.cpp b/src/Nav/elevation_mapping/src/ElevationMapping.cpp similarity index 100% rename from src/elevation_mapping/src/ElevationMapping.cpp rename to src/Nav/elevation_mapping/src/ElevationMapping.cpp diff --git a/src/elevation_mapping/src/PointXYZRGBConfidenceRatio.cpp b/src/Nav/elevation_mapping/src/PointXYZRGBConfidenceRatio.cpp similarity index 100% rename from src/elevation_mapping/src/PointXYZRGBConfidenceRatio.cpp rename to src/Nav/elevation_mapping/src/PointXYZRGBConfidenceRatio.cpp diff --git a/src/elevation_mapping/src/RobotMotionMapUpdater.cpp b/src/Nav/elevation_mapping/src/RobotMotionMapUpdater.cpp similarity index 100% rename from src/elevation_mapping/src/RobotMotionMapUpdater.cpp rename to src/Nav/elevation_mapping/src/RobotMotionMapUpdater.cpp diff --git a/src/elevation_mapping/src/elevation_mapping_node.cpp b/src/Nav/elevation_mapping/src/elevation_mapping_node.cpp similarity index 100% rename from src/elevation_mapping/src/elevation_mapping_node.cpp rename to src/Nav/elevation_mapping/src/elevation_mapping_node.cpp diff --git a/src/elevation_mapping/src/input_sources/Input.cpp b/src/Nav/elevation_mapping/src/input_sources/Input.cpp similarity index 100% rename from src/elevation_mapping/src/input_sources/Input.cpp rename to src/Nav/elevation_mapping/src/input_sources/Input.cpp diff --git a/src/elevation_mapping/src/input_sources/InputSourceManager.cpp b/src/Nav/elevation_mapping/src/input_sources/InputSourceManager.cpp similarity index 100% rename from src/elevation_mapping/src/input_sources/InputSourceManager.cpp rename to src/Nav/elevation_mapping/src/input_sources/InputSourceManager.cpp diff --git a/src/elevation_mapping/src/postprocessing/PostprocessingPipelineFunctor.cpp b/src/Nav/elevation_mapping/src/postprocessing/PostprocessingPipelineFunctor.cpp similarity index 100% rename from src/elevation_mapping/src/postprocessing/PostprocessingPipelineFunctor.cpp rename to src/Nav/elevation_mapping/src/postprocessing/PostprocessingPipelineFunctor.cpp diff --git a/src/elevation_mapping/src/postprocessing/PostprocessingWorker.cpp b/src/Nav/elevation_mapping/src/postprocessing/PostprocessingWorker.cpp similarity index 100% rename from src/elevation_mapping/src/postprocessing/PostprocessingWorker.cpp rename to src/Nav/elevation_mapping/src/postprocessing/PostprocessingWorker.cpp diff --git a/src/elevation_mapping/src/postprocessing/PostprocessorPool.cpp b/src/Nav/elevation_mapping/src/postprocessing/PostprocessorPool.cpp similarity index 100% rename from src/elevation_mapping/src/postprocessing/PostprocessorPool.cpp rename to src/Nav/elevation_mapping/src/postprocessing/PostprocessorPool.cpp diff --git a/src/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp b/src/Nav/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp similarity index 100% rename from src/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp rename to src/Nav/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp diff --git a/src/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp b/src/Nav/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp similarity index 100% rename from src/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp rename to src/Nav/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp diff --git a/src/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp b/src/Nav/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp similarity index 100% rename from src/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp rename to src/Nav/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp diff --git a/src/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp b/src/Nav/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp similarity index 100% rename from src/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp rename to src/Nav/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp diff --git a/src/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp b/src/Nav/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp similarity index 100% rename from src/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp rename to src/Nav/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp diff --git a/src/elevation_mapping/src/test.cpp b/src/Nav/elevation_mapping/src/test.cpp similarity index 100% rename from src/elevation_mapping/src/test.cpp rename to src/Nav/elevation_mapping/src/test.cpp diff --git a/src/elevation_mapping/src/test_service.cpp b/src/Nav/elevation_mapping/src/test_service.cpp similarity index 100% rename from src/elevation_mapping/src/test_service.cpp rename to src/Nav/elevation_mapping/src/test_service.cpp diff --git a/src/elevation_mapping/test/ElevationMapTest.cpp b/src/Nav/elevation_mapping/test/ElevationMapTest.cpp similarity index 100% rename from src/elevation_mapping/test/ElevationMapTest.cpp rename to src/Nav/elevation_mapping/test/ElevationMapTest.cpp diff --git a/src/elevation_mapping/test/WeightedEmpiricalCumulativeDistributionFunctionTest.cpp b/src/Nav/elevation_mapping/test/WeightedEmpiricalCumulativeDistributionFunctionTest.cpp similarity index 100% rename from src/elevation_mapping/test/WeightedEmpiricalCumulativeDistributionFunctionTest.cpp rename to src/Nav/elevation_mapping/test/WeightedEmpiricalCumulativeDistributionFunctionTest.cpp diff --git a/src/elevation_mapping/test/elevation_mapping.test b/src/Nav/elevation_mapping/test/elevation_mapping.test similarity index 100% rename from src/elevation_mapping/test/elevation_mapping.test rename to src/Nav/elevation_mapping/test/elevation_mapping.test diff --git a/src/elevation_mapping/test/input_sources/InputSourcesTest.cpp b/src/Nav/elevation_mapping/test/input_sources/InputSourcesTest.cpp similarity index 100% rename from src/elevation_mapping/test/input_sources/InputSourcesTest.cpp rename to src/Nav/elevation_mapping/test/input_sources/InputSourcesTest.cpp diff --git a/src/elevation_mapping/test/input_sources/config/TestConfigurations.yaml b/src/Nav/elevation_mapping/test/input_sources/config/TestConfigurations.yaml similarity index 100% rename from src/elevation_mapping/test/input_sources/config/TestConfigurations.yaml rename to src/Nav/elevation_mapping/test/input_sources/config/TestConfigurations.yaml diff --git a/src/elevation_mapping/test/input_sources/config/rosconsole.config b/src/Nav/elevation_mapping/test/input_sources/config/rosconsole.config similarity index 100% rename from src/elevation_mapping/test/input_sources/config/rosconsole.config rename to src/Nav/elevation_mapping/test/input_sources/config/rosconsole.config diff --git a/src/elevation_mapping/test/input_sources/input_sources.test b/src/Nav/elevation_mapping/test/input_sources/input_sources.test similarity index 100% rename from src/elevation_mapping/test/input_sources/input_sources.test rename to src/Nav/elevation_mapping/test/input_sources/input_sources.test diff --git a/src/elevation_mapping/test/input_sources/test_input_sources.cpp b/src/Nav/elevation_mapping/test/input_sources/test_input_sources.cpp similarity index 100% rename from src/elevation_mapping/test/input_sources/test_input_sources.cpp rename to src/Nav/elevation_mapping/test/input_sources/test_input_sources.cpp diff --git a/src/elevation_mapping/test/postprocessing/PostprocessorTest.cpp b/src/Nav/elevation_mapping/test/postprocessing/PostprocessorTest.cpp similarity index 100% rename from src/elevation_mapping/test/postprocessing/PostprocessorTest.cpp rename to src/Nav/elevation_mapping/test/postprocessing/PostprocessorTest.cpp diff --git a/src/elevation_mapping/test/postprocessing/main.cpp b/src/Nav/elevation_mapping/test/postprocessing/main.cpp similarity index 100% rename from src/elevation_mapping/test/postprocessing/main.cpp rename to src/Nav/elevation_mapping/test/postprocessing/main.cpp diff --git a/src/elevation_mapping/test/postprocessing/postprocessor.test b/src/Nav/elevation_mapping/test/postprocessing/postprocessor.test similarity index 100% rename from src/elevation_mapping/test/postprocessing/postprocessor.test rename to src/Nav/elevation_mapping/test/postprocessing/postprocessor.test diff --git a/src/elevation_mapping/test/postprocessing/postprocessor_pipeline.yaml b/src/Nav/elevation_mapping/test/postprocessing/postprocessor_pipeline.yaml similarity index 100% rename from src/elevation_mapping/test/postprocessing/postprocessor_pipeline.yaml rename to src/Nav/elevation_mapping/test/postprocessing/postprocessor_pipeline.yaml diff --git a/src/elevation_mapping/test/test_elevation_mapping.cpp b/src/Nav/elevation_mapping/test/test_elevation_mapping.cpp similarity index 100% rename from src/elevation_mapping/test/test_elevation_mapping.cpp rename to src/Nav/elevation_mapping/test/test_elevation_mapping.cpp diff --git a/src/gps/config/Heading_config.ubx b/src/Nav/gps/config/Heading_config.ubx similarity index 100% rename from src/gps/config/Heading_config.ubx rename to src/Nav/gps/config/Heading_config.ubx diff --git a/src/gps/config/Rover_config.ubx b/src/Nav/gps/config/Rover_config.ubx similarity index 100% rename from src/gps/config/Rover_config.ubx rename to src/Nav/gps/config/Rover_config.ubx diff --git a/src/gps/config/gps.yaml b/src/Nav/gps/config/gps.yaml similarity index 100% rename from src/gps/config/gps.yaml rename to src/Nav/gps/config/gps.yaml diff --git a/src/localization/localization/__init__.py b/src/Nav/gps/gps/__init__.py similarity index 100% rename from src/localization/localization/__init__.py rename to src/Nav/gps/gps/__init__.py diff --git a/src/gps/gps/heading_pub_node.py b/src/Nav/gps/gps/heading_pub_node.py similarity index 100% rename from src/gps/gps/heading_pub_node.py rename to src/Nav/gps/gps/heading_pub_node.py diff --git a/src/gps/gps/rtcm_pub_node.py b/src/Nav/gps/gps/rtcm_pub_node.py similarity index 100% rename from src/gps/gps/rtcm_pub_node.py rename to src/Nav/gps/gps/rtcm_pub_node.py diff --git a/src/gps/gps/ubx_io_manager.py b/src/Nav/gps/gps/ubx_io_manager.py similarity index 100% rename from src/gps/gps/ubx_io_manager.py rename to src/Nav/gps/gps/ubx_io_manager.py diff --git a/src/gps/launch/basestation.launch.py b/src/Nav/gps/launch/basestation.launch.py similarity index 100% rename from src/gps/launch/basestation.launch.py rename to src/Nav/gps/launch/basestation.launch.py diff --git a/src/gps/launch/heading.launch.py b/src/Nav/gps/launch/heading.launch.py similarity index 100% rename from src/gps/launch/heading.launch.py rename to src/Nav/gps/launch/heading.launch.py diff --git a/src/gps/launch/rover.launch.py b/src/Nav/gps/launch/rover.launch.py similarity index 100% rename from src/gps/launch/rover.launch.py rename to src/Nav/gps/launch/rover.launch.py diff --git a/src/gps/package.xml b/src/Nav/gps/package.xml similarity index 100% rename from src/gps/package.xml rename to src/Nav/gps/package.xml diff --git a/src/gps/resource/gps b/src/Nav/gps/resource/gps similarity index 100% rename from src/gps/resource/gps rename to src/Nav/gps/resource/gps diff --git a/src/gps/setup.cfg b/src/Nav/gps/setup.cfg similarity index 100% rename from src/gps/setup.cfg rename to src/Nav/gps/setup.cfg diff --git a/src/gps/setup.py b/src/Nav/gps/setup.py similarity index 100% rename from src/gps/setup.py rename to src/Nav/gps/setup.py diff --git a/src/nav_commanders/test/test_copyright.py b/src/Nav/gps/test/test_copyright.py similarity index 100% rename from src/nav_commanders/test/test_copyright.py rename to src/Nav/gps/test/test_copyright.py diff --git a/src/gps/test/test_flake8.py b/src/Nav/gps/test/test_flake8.py similarity index 100% rename from src/gps/test/test_flake8.py rename to src/Nav/gps/test/test_flake8.py diff --git a/src/gps/test/test_pep257.py b/src/Nav/gps/test/test_pep257.py similarity index 100% rename from src/gps/test/test_pep257.py rename to src/Nav/gps/test/test_pep257.py diff --git a/src/Nav/kindr_ros/.gitignore b/src/Nav/kindr_ros/.gitignore new file mode 100644 index 00000000..39403780 --- /dev/null +++ b/src/Nav/kindr_ros/.gitignore @@ -0,0 +1,36 @@ +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +# CLion +.idea/ + +# Eclipse +.project +.cproject +.settings diff --git a/src/Nav/kindr_ros/LICENSE b/src/Nav/kindr_ros/LICENSE new file mode 100644 index 00000000..5d62a455 --- /dev/null +++ b/src/Nav/kindr_ros/LICENSE @@ -0,0 +1,29 @@ +Copyright 2019, ANYbotics AG. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/Nav/kindr_ros/README.md b/src/Nav/kindr_ros/README.md new file mode 100644 index 00000000..b551dca6 --- /dev/null +++ b/src/Nav/kindr_ros/README.md @@ -0,0 +1,43 @@ +# Kindr ROS + +Contact : Remo Diethelm [rdiethelm ( at ) anybotics.com] + +Author(s): Christian Gehring, Peter Fankhauser, Remo Diethelm + +**Authors: Christian Gehring, Peter Fankhauser, Remo Diethelm
+Maintainer: Remo Diethelm, rdiethelm@anybotics.com
+Affiliation: [ANYbotics](https://www.anybotics.com/)** + +This projected was initially developed at ETH Zurich (Autonomous Systems Lab & Robotic Systems Lab). + +This work is conducted as part of [ANYmal Research](https://www.anymal-research.org/), a community to advance legged robotics. + +The source code is released under a [BSD 3-Clause license](LICENSE). + +## Overview + +ROS messages and RViz plugins for kindr objects. + +## Packages + +### kindr_msgs + +ROS messages for kindr objects. + +### kindr_ros + +Conversion between official ROS messages and kindr objects. + +### kindr_rviz_plugins + +RViz plugins to visualize kindr ROS messages. + +### multi_dof_joint_trajectory_rviz_plugins + +RViz plugins to visualize trajectory ROS messages. + +## Usage + +To build, clone this repository into your catkin workspace and type + + catkin build diff --git a/src/Nav/kindr_ros/kindr_msgs/CMakeLists.txt b/src/Nav/kindr_ros/kindr_msgs/CMakeLists.txt new file mode 100644 index 00000000..9046da19 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_msgs/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.5.1) +project(kindr_msgs) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +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(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## Generate messages in the 'msg' folder +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/VectorAtPosition.msg" + DEPENDENCIES + geometry_msgs + std_msgs) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/Nav/kindr_ros/kindr_msgs/msg/VectorAtPosition.msg b/src/Nav/kindr_ros/kindr_msgs/msg/VectorAtPosition.msg new file mode 100644 index 00000000..8bcd9899 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_msgs/msg/VectorAtPosition.msg @@ -0,0 +1,22 @@ +# Vector type definition (based on Kindr definitions) + +std_msgs/Header header + +uint8 TYPE_TYPELESS=0 +uint8 TYPE_JERK=6 +uint8 TYPE_ACCELERATION=7 +uint8 TYPE_VELOCITY=8 +uint8 TYPE_POSITION=9 +uint8 TYPE_FORCE=10 +uint8 TYPE_MOMEMTUM=11 +uint8 TYPE_ANGULAR_JERK=12 +uint8 TYPE_ANGULAR_ACCELERATION=13 +uint8 TYPE_ANGULAR_VELOCITY=14 +uint8 TYPE_TORQUE=16 +uint8 TYPE_ANGULAR_MOMEMTUM=17 + +uint8 type +string name +geometry_msgs/Vector3 vector # Frame defined in header +geometry_msgs/Point position # Point of origin of the vector +string position_frame_id # If empty same as vector frame diff --git a/src/Nav/kindr_ros/kindr_msgs/package.xml b/src/Nav/kindr_ros/kindr_msgs/package.xml new file mode 100644 index 00000000..ec83a9df --- /dev/null +++ b/src/Nav/kindr_ros/kindr_msgs/package.xml @@ -0,0 +1,34 @@ + + + + kindr_msgs + 0.3.1 + Kindr common msgs + + Remo Diethelm + Philipp Leemann + + BSD + https://github.com/anybotics/kindr_ros + + Peter Fankhauser + + ament_cmake + + rosidl_default_generators + geometry_msgs + std_msgs + + rosidl_default_runtime + geometry_msgs + std_msgs + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/Nav/kindr_ros/kindr_ros/CMakeLists.txt b/src/Nav/kindr_ros/kindr_ros/CMakeLists.txt new file mode 100644 index 00000000..7cc81dac --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/CMakeLists.txt @@ -0,0 +1,100 @@ +cmake_minimum_required(VERSION 3.5.1) +project(kindr_ros) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +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(tf2 REQUIRED) +find_package(geometry_msgs REQUIRED) + +find_package(kindr QUIET) +if(NOT kindr_FOUND) + # Attempt to find package-based kindr + find_package(PkgConfig REQUIRED) + pkg_check_modules(kindr kindr REQUIRED) + set(kindr_INCLUDE_DIRS ${kindr_INCLUDE_DIRS}) +endif() + +find_package(Eigen3 QUIET) +if(NOT EIGEN3_FOUND) + find_package(PkgConfig REQUIRED) + pkg_check_modules(EIGEN3 REQUIRED eigen3) + set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) +endif() + +include_directories( + include + ${EIGERN_INCLUDE_DIRS} + ${kindr_INCLUDE_DIRS} +) + +set(dependencies + tf2 + geometry_msgs +) + +############# +## Install ## +############# + +# Mark cpp header files for installation +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.hpp" +) + +############# +## Testing ## +############# + +if(BUILD_TESTING) + # Linting is setup this way to add a filter + # to ament_cpplint to ignore the lack of + # copyright messages at the top of files. + # Copyright messages are being checked for by both + # ament_cmake_cpplint & ament_cmake_copyright. + + find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto QUIET) + if(ament_lint_auto_FOUND) + # exclude copyright checks + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_copyright + ) + ament_lint_auto_find_test_dependencies() + + # run cpplint without copyright filter + find_package(ament_cmake_cpplint) + ament_cpplint( + FILTERS -legal/copyright + ) + endif() + + find_package(ament_cmake_gtest REQUIRED) + + add_subdirectory(test) +endif() + +ament_export_include_directories( + include + ${EIGEN3_INCLUDE_DIRS} + ${kindr_INCLUDE_DIRS}) + +ament_export_dependencies(${dependencies}) + +ament_package() diff --git a/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPhysicalQuantities.hpp b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPhysicalQuantities.hpp new file mode 100644 index 00000000..2e272b39 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPhysicalQuantities.hpp @@ -0,0 +1,99 @@ +/* + * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul + * Furgale, Remo Diethelm All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL Peter Fankhauser, Christian Gehring, Hannes + * Sommer, Paul Furgale, Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef KINDR_ROS__ROSGEOMETRYMSGPHYSICALQUANTITIES_HPP_ +#define KINDR_ROS__ROSGEOMETRYMSGPHYSICALQUANTITIES_HPP_ + +// kindr +#include + +// ros +#include +#include +#include + +namespace kindr_ros { + +template +inline static void convertFromRosGeometryMsg( + const geometry_msgs::msg::Vector3& geometryVector3Msg, + kindr::Vector& vector) { + vector.x() = static_cast(geometryVector3Msg.x); + vector.y() = static_cast(geometryVector3Msg.y); + vector.z() = static_cast(geometryVector3Msg.z); +} + +template +inline static void convertToRosGeometryMsg( + const kindr::Vector& vector, + geometry_msgs::msg::Vector3& geometryVector3Msg) { + geometryVector3Msg.x = static_cast(vector.x()); + geometryVector3Msg.y = static_cast(vector.y()); + geometryVector3Msg.z = static_cast(vector.z()); +} + +template +inline static void convertFromRosGeometryMsg( + const geometry_msgs::msg::Point32& geometryPoint32Msg, + kindr::Position& position) { + position.x() = static_cast(geometryPoint32Msg.x); + position.y() = static_cast(geometryPoint32Msg.y); + position.z() = static_cast(geometryPoint32Msg.z); +} + +template +inline static void convertToRosGeometryMsg( + const kindr::Position& position, + geometry_msgs::msg::Point32& geometryPoint32Msg) { + geometryPoint32Msg.x = static_cast(position.x()); + geometryPoint32Msg.y = static_cast(position.y()); + geometryPoint32Msg.z = static_cast(position.z()); +} + +template +inline static void convertFromRosGeometryMsg( + const geometry_msgs::msg::Point& geometryPointMsg, + kindr::Position& position) { + position.x() = static_cast(geometryPointMsg.x); + position.y() = static_cast(geometryPointMsg.y); + position.z() = static_cast(geometryPointMsg.z); +} + +template +inline static void convertToRosGeometryMsg( + const kindr::Position& position, + geometry_msgs::msg::Point& geometryPointMsg) { + geometryPointMsg.x = static_cast(position.x()); + geometryPointMsg.y = static_cast(position.y()); + geometryPointMsg.z = static_cast(position.z()); +} + +} // namespace kindr_ros + +#endif // KINDR_ROS__ROSGEOMETRYMSGPHYSICALQUANTITIES_HPP_ diff --git a/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPose.hpp b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPose.hpp new file mode 100644 index 00000000..339ae118 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPose.hpp @@ -0,0 +1,110 @@ +/* + * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul + * Furgale, Remo Diethelm All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL Peter Fankhauser, Christian Gehring, Hannes + * Sommer, Paul Furgale, Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef KINDR_ROS__ROSGEOMETRYMSGPOSE_HPP_ +#define KINDR_ROS__ROSGEOMETRYMSGPOSE_HPP_ + +// kindr +#include + +// ros +#include +#include + +// kindr ros +#include "kindr_ros/RosGeometryMsgPhysicalQuantities.hpp" +#include "kindr_ros/RosGeometryMsgRotation.hpp" + +namespace kindr_ros { + +template +inline static void convertFromRosGeometryMsg( + const geometry_msgs::msg::Pose& geometryPoseMsg, + kindr::HomogeneousTransformation& pose) { + convertFromRosGeometryMsg(geometryPoseMsg.position, pose.getPosition()); + + // This is the definition of ROS Geometry pose. + typedef kindr::RotationQuaternion RotationQuaternionGeometryPoseLike; + + RotationQuaternionGeometryPoseLike rotation; + convertFromRosGeometryMsg(geometryPoseMsg.orientation, rotation); + pose.getRotation() = rotation; +} + +template +inline static void convertToRosGeometryMsg( + const kindr::HomogeneousTransformation& + pose, + geometry_msgs::msg::Pose& geometryPoseMsg) { + convertToRosGeometryMsg(pose.getPosition(), geometryPoseMsg.position); + + // This is the definition of ROS Geometry pose. + typedef kindr::RotationQuaternion RotationQuaternionGeometryPoseLike; + + RotationQuaternionGeometryPoseLike rotation(pose.getRotation()); + convertToRosGeometryMsg(rotation, geometryPoseMsg.orientation); +} + +template +inline static void convertFromRosGeometryMsg( + const geometry_msgs::msg::Transform& geometryTransformMsg, + kindr::HomogeneousTransformation& + transformation) { + convertFromRosGeometryMsg(geometryTransformMsg.translation, + transformation.getPosition()); + + // This is the definition of ROS Geometry transform. + typedef kindr::RotationQuaternion + RotationQuaternionGeometryTransformLike; + + RotationQuaternionGeometryTransformLike rotation; + convertFromRosGeometryMsg(geometryTransformMsg.rotation, rotation); + transformation.getRotation() = rotation; +} + +template +inline static void convertToRosGeometryMsg( + const kindr::HomogeneousTransformation& + transformation, + geometry_msgs::msg::Transform& geometryTransformMsg) { + convertToRosGeometryMsg(transformation.getPosition(), + geometryTransformMsg.translation); + + // This is the definition of ROS Geometry transform. + typedef kindr::RotationQuaternion + RotationQuaternionGeometryTransformLike; + + RotationQuaternionGeometryTransformLike rotation( + transformation.getRotation()); + convertToRosGeometryMsg(rotation, geometryTransformMsg.rotation); +} + +} // namespace kindr_ros + +#endif // KINDR_ROS__ROSGEOMETRYMSGPOSE_HPP_ diff --git a/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgRotation.hpp b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgRotation.hpp new file mode 100644 index 00000000..46afefa3 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgRotation.hpp @@ -0,0 +1,63 @@ +/* + * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul + * Furgale, Remo Diethelm All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL Peter Fankhauser, Christian Gehring, Hannes + * Sommer, Paul Furgale, Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef KINDR_ROS__ROSGEOMETRYMSGROTATION_HPP_ +#define KINDR_ROS__ROSGEOMETRYMSGROTATION_HPP_ + +// kindr +#include + +// ros +#include + +namespace kindr_ros { + +template +inline static void convertFromRosGeometryMsg( + const geometry_msgs::msg::Quaternion& geometryQuaternionMsg, + kindr::RotationQuaternion& rotationQuaternion) { + rotationQuaternion.setValues(static_cast(geometryQuaternionMsg.w), + static_cast(geometryQuaternionMsg.x), + static_cast(geometryQuaternionMsg.y), + static_cast(geometryQuaternionMsg.z)); +} + +template +inline static void convertToRosGeometryMsg( + const kindr::RotationQuaternion& rotationQuaternion, + geometry_msgs::msg::Quaternion& geometryQuaternionMsg) { + geometryQuaternionMsg.w = static_cast(rotationQuaternion.w()); + geometryQuaternionMsg.x = static_cast(rotationQuaternion.x()); + geometryQuaternionMsg.y = static_cast(rotationQuaternion.y()); + geometryQuaternionMsg.z = static_cast(rotationQuaternion.z()); +} + +} // namespace kindr_ros + +#endif // KINDR_ROS__ROSGEOMETRYMSGROTATION_HPP_ diff --git a/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgTwist.hpp b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgTwist.hpp new file mode 100644 index 00000000..151a76e2 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgTwist.hpp @@ -0,0 +1,66 @@ +/* + * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul + * Furgale, Remo Diethelm All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL Peter Fankhauser, Christian Gehring, Hannes + * Sommer, Paul Furgale, Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef KINDR_ROS__ROSGEOMETRYMSGTWIST_HPP_ +#define KINDR_ROS__ROSGEOMETRYMSGTWIST_HPP_ + +// kindr +#include + +// ros +#include + +// kindr ros +#include "kindr_ros/RosGeometryMsgPhysicalQuantities.hpp" + +namespace kindr_ros { + +template +inline static void convertFromRosGeometryMsg( + const geometry_msgs::msg::Twist& geometryTwistMsg, + kindr::Twist& twist) { + convertFromRosGeometryMsg(geometryTwistMsg.linear, + twist.getTranslationalVelocity()); + convertFromRosGeometryMsg(geometryTwistMsg.angular, + twist.getRotationalVelocity()); +} + +template +inline static void convertToRosGeometryMsg( + const kindr::Twist& twist, + geometry_msgs::msg::Twist& geometryTwistMsg) { + convertToRosGeometryMsg(twist.getTranslationalVelocity(), + geometryTwistMsg.linear); + convertToRosGeometryMsg(twist.getRotationalVelocity(), + geometryTwistMsg.angular); +} + +} // namespace kindr_ros + +#endif // KINDR_ROS__ROSGEOMETRYMSGTWIST_HPP_ diff --git a/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosTfPose.hpp b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosTfPose.hpp new file mode 100644 index 00000000..4a7df956 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosTfPose.hpp @@ -0,0 +1,103 @@ +/* + * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul + * Furgale, Remo Diethelm All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL Peter Fankhauser, Christian Gehring, Hannes + * Sommer, Paul Furgale, Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef KINDR_ROS__ROSTFPOSE_HPP_ +#define KINDR_ROS__ROSTFPOSE_HPP_ + +// kindr +#include + +// ros +#include +#include +#include +#include + +namespace kindr_ros { + +template +inline static void convertFromRosTf( + const tf2::Transform& tfTransform, + kindr::HomogeneousTransformation& pose) { + typedef Position_ Position; + typedef Rotation_ Rotation; + + // This is the definition of TF. + typedef kindr::RotationMatrix RotationMatrixTfLike; + + const tf2::Vector3& rowX = tfTransform.getBasis().getRow(0); + const tf2::Vector3& rowY = tfTransform.getBasis().getRow(1); + const tf2::Vector3& rowZ = tfTransform.getBasis().getRow(2); + + RotationMatrixTfLike rotation; + rotation.setMatrix(rowX.x(), rowX.y(), rowX.z(), rowY.x(), rowY.y(), rowY.z(), + rowZ.x(), rowZ.y(), rowZ.z()); + + Position position(tfTransform.getOrigin().getX(), + tfTransform.getOrigin().getY(), + tfTransform.getOrigin().getZ()); + + pose.getRotation() = Rotation(rotation); + pose.getPosition() = position; +} + +template +inline static void convertToRosTfQuaternion(tf2::Quaternion& tfQuat, + const Rotation_& rotation) { + const auto quat = + kindr::RotationQuaternion(rotation); + tfQuat = tf2::Quaternion(quat.x(), quat.y(), quat.z(), quat.w()); +} + +template +inline static void convertToRosTf(const kindr::HomogeneousTransformation< + PrimType_, Position_, Rotation_>& pose, + tf2::Transform& tfTransform) { + // This is the definition of TF. + typedef kindr::RotationMatrix RotationMatrixTfLike; + Eigen::Matrix3d rotationMatrix( + RotationMatrixTfLike(pose.getRotation()).matrix()); + tf2::Matrix3x3 tfRotationMatrix( + rotationMatrix(0, 0), rotationMatrix(0, 1), rotationMatrix(0, 2), + rotationMatrix(1, 0), rotationMatrix(1, 1), rotationMatrix(1, 2), + rotationMatrix(2, 0), rotationMatrix(2, 1), rotationMatrix(2, 2)); + tfTransform.setBasis(tfRotationMatrix); + + // less accurate by quaternion: + // tf::Quaternion tfQuat; + // convertToRosTfQuaternion(tfQuat, pose.getRotation()); + // tfTransform.setRotation(tfQuat); + + tfTransform.setOrigin(tf2::Vector3( + pose.getPosition().x(), pose.getPosition().y(), pose.getPosition().z())); +} + +} // namespace kindr_ros + +#endif // KINDR_ROS__ROSTFPOSE_HPP_ diff --git a/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/kindr_ros.hpp b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/kindr_ros.hpp new file mode 100644 index 00000000..84a73492 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/kindr_ros.hpp @@ -0,0 +1,45 @@ +/* + * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul + * Furgale, Remo Diethelm All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL Peter Fankhauser, Christian Gehring, Hannes + * Sommer, Paul Furgale, Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ +/* + * kindr_ros.hpp + * + * Created on: Jun 15, 2016 + * Author: gech + */ + +#ifndef KINDR_ROS__KINDR_ROS_HPP_ +#define KINDR_ROS__KINDR_ROS_HPP_ + +#include "kindr_ros/RosGeometryMsgPhysicalQuantities.hpp" +#include "kindr_ros/RosGeometryMsgPose.hpp" +#include "kindr_ros/RosGeometryMsgRotation.hpp" +#include "kindr_ros/RosGeometryMsgTwist.hpp" +#include "kindr_ros/RosTfPose.hpp" + +#endif // KINDR_ROS__KINDR_ROS_HPP_ diff --git a/src/Nav/kindr_ros/kindr_ros/package.xml b/src/Nav/kindr_ros/kindr_ros/package.xml new file mode 100644 index 00000000..161670c1 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/package.xml @@ -0,0 +1,32 @@ + + + + kindr_ros + 0.3.1 + The kindr_ros package + + Remo Diethelm + Philipp Leemann + + BSD + https://github.com/anybotics/kindr_ros + + Christian Gehring + + ament_cmake + + kindr + geometry_msgs + tf2 + + eigen + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + + ament_cmake + + diff --git a/src/Nav/kindr_ros/kindr_ros/test/CMakeLists.txt b/src/Nav/kindr_ros/kindr_ros/test/CMakeLists.txt new file mode 100644 index 00000000..0b8ea270 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/test/CMakeLists.txt @@ -0,0 +1,13 @@ +ament_add_gtest(${PROJECT_NAME}-test + test_main.cpp + RosGeometryMsgPhysicalQuantitiesTest.cpp + RosGeometryMsgRotationTest.cpp + RosGeometryMsgPoseTest.cpp + RosTfPoseTest.cpp + TfConventionTest.cpp +) + + +ament_target_dependencies(${PROJECT_NAME}-test + ${dependencies} +) diff --git a/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgPhysicalQuantitiesTest.cpp b/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgPhysicalQuantitiesTest.cpp new file mode 100644 index 00000000..7b1716cb --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgPhysicalQuantitiesTest.cpp @@ -0,0 +1,125 @@ +/* + * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul + * Furgale, Remo Diethelm All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL Peter Fankhauser, Christian Gehring, Hannes + * Sommer, Paul Furgale, Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include + +#include +#include +#include +#include +#include + +#include "kindr_ros/RosGeometryMsgPhysicalQuantities.hpp" + +TEST(RosGeometryMsgVectorEigen, convertFromRosGeometryMsg) { + const kindr::VectorTypeless3D referenceVector(-9.12, -25.5, 0.6); + + geometry_msgs::msg::Vector3 geometryVector3Msg; + geometryVector3Msg.x = referenceVector.x(); + geometryVector3Msg.y = referenceVector.y(); + geometryVector3Msg.z = referenceVector.z(); + + kindr::VectorTypeless3D vector; + kindr_ros::convertFromRosGeometryMsg(geometryVector3Msg, vector); + + kindr::expectNear(vector.toImplementation(), + referenceVector.toImplementation(), 1e-8, + KINDR_SOURCE_FILE_POS); +} + +TEST(RosGeometryMsgVectorEigen, convertToRosGeometryMsg) { + const kindr::VectorTypeless3D referenceVector(0.3, -1.1, -0.6); + + kindr::VectorTypeless3D vector(referenceVector); + + geometry_msgs::msg::Vector3 geometryVectorMsg; + kindr_ros::convertToRosGeometryMsg(vector, geometryVectorMsg); + + EXPECT_NEAR(geometryVectorMsg.x, referenceVector.x(), 1e-8); + EXPECT_NEAR(geometryVectorMsg.y, referenceVector.y(), 1e-8); + EXPECT_NEAR(geometryVectorMsg.z, referenceVector.z(), 1e-8); +} + +TEST(RosGeometryMsgPositionEigen, convertFromRosGeometryPoint32Msg) { + const kindr::Position3F referenceTranslation(-9.3, 2.5, 5.6); + + geometry_msgs::msg::Point32 geometryPointMsg; + geometryPointMsg.x = referenceTranslation.x(); + geometryPointMsg.y = referenceTranslation.y(); + geometryPointMsg.z = referenceTranslation.z(); + + kindr::Position3F position; + kindr_ros::convertFromRosGeometryMsg(geometryPointMsg, position); + + kindr::expectNear(position.toImplementation(), + referenceTranslation.toImplementation(), 1e-8, + KINDR_SOURCE_FILE_POS); +} + +TEST(RosGeometryMsgPositionEigen, convertToRosGeometryPoint32Msg) { + const kindr::Position3F referenceTranslation(0.3, -1.1, -0.6); + + kindr::Position3F position(referenceTranslation); + + geometry_msgs::msg::Point32 geometryPointMsg; + kindr_ros::convertToRosGeometryMsg(position, geometryPointMsg); + + EXPECT_NEAR(geometryPointMsg.x, referenceTranslation.x(), 1e-8); + EXPECT_NEAR(geometryPointMsg.y, referenceTranslation.y(), 1e-8); + EXPECT_NEAR(geometryPointMsg.z, referenceTranslation.z(), 1e-8); +} + +TEST(RosGeometryMsgPositionEigen, convertFromRosGeometryPointMsg) { + const kindr::Position3D referenceTranslation(19.3, -2.5, 5.6); + + geometry_msgs::msg::Point geometryPointMsg; + geometryPointMsg.x = referenceTranslation.x(); + geometryPointMsg.y = referenceTranslation.y(); + geometryPointMsg.z = referenceTranslation.z(); + + kindr::Position3D position; + kindr_ros::convertFromRosGeometryMsg(geometryPointMsg, position); + + kindr::expectNear(position.toImplementation(), + referenceTranslation.toImplementation(), 1e-8, + KINDR_SOURCE_FILE_POS); +} + +TEST(RosGeometryMsgPositionEigen, convertToRosGeometryPointMsg) { + const kindr::Position3D referenceTranslation(0.3, -1.1, -0.6); + + kindr::Position3D position(referenceTranslation); + + geometry_msgs::msg::Point geometryPointMsg; + kindr_ros::convertToRosGeometryMsg(position, geometryPointMsg); + + EXPECT_NEAR(geometryPointMsg.x, referenceTranslation.x(), 1e-8); + EXPECT_NEAR(geometryPointMsg.y, referenceTranslation.y(), 1e-8); + EXPECT_NEAR(geometryPointMsg.z, referenceTranslation.z(), 1e-8); +} diff --git a/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgPoseTest.cpp b/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgPoseTest.cpp new file mode 100644 index 00000000..af60b583 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgPoseTest.cpp @@ -0,0 +1,131 @@ +/* + * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul + * Furgale, Remo Diethelm All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL Peter Fankhauser, Christian Gehring, Hannes + * Sommer, Paul Furgale, Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "kindr_ros/RosGeometryMsgPose.hpp" + +TEST(RosGeometryMsgPoseEigen, convertFromRosGeometryMsg) { + const kindr::Position3D referenceTranslation(0.3, -1.5, 0.6); + const kindr::RotationQuaternionPD referenceQuaternion(0.113, 0.071, -0.924, + 0.35835); + + geometry_msgs::msg::Pose geometryPoseMsg; + geometryPoseMsg.orientation.x = referenceQuaternion.x(); + geometryPoseMsg.orientation.y = referenceQuaternion.y(); + geometryPoseMsg.orientation.z = referenceQuaternion.z(); + geometryPoseMsg.orientation.w = referenceQuaternion.w(); + geometryPoseMsg.position.x = referenceTranslation.x(); + geometryPoseMsg.position.y = referenceTranslation.y(); + geometryPoseMsg.position.z = referenceTranslation.z(); + + kindr::HomogeneousTransformationPosition3RotationQuaternionD pose; + kindr_ros::convertFromRosGeometryMsg(geometryPoseMsg, pose); + + kindr::expectNear(pose.getPosition().toImplementation(), + referenceTranslation.toImplementation(), 1e-4, + KINDR_SOURCE_FILE_POS); + EXPECT_TRUE(pose.getRotation().isNear(referenceQuaternion, 1e-8)); +} + +TEST(RosGeometryMsgPoseEigen, convertToRosGeometryMsg) { + const kindr::Position3D referenceTranslation(0.3, -1.1, -0.6); + const kindr::RotationQuaternionPD referenceQuaternion(0.212, 0.0421, -0.958, + 0.1885); + + kindr::HomogeneousTransformationPosition3RotationQuaternionD pose; + pose.getPosition() = referenceTranslation; + pose.getRotation() = referenceQuaternion; + + geometry_msgs::msg::Pose geometryPoseMsg; + kindr_ros::convertToRosGeometryMsg(pose, geometryPoseMsg); + + EXPECT_NEAR(geometryPoseMsg.orientation.x, referenceQuaternion.x(), 1e-8); + EXPECT_NEAR(geometryPoseMsg.orientation.y, referenceQuaternion.y(), 1e-8); + EXPECT_NEAR(geometryPoseMsg.orientation.z, referenceQuaternion.z(), 1e-8); + EXPECT_NEAR(geometryPoseMsg.orientation.w, referenceQuaternion.w(), 1e-8); + EXPECT_NEAR(geometryPoseMsg.position.x, referenceTranslation.x(), 1e-8); + EXPECT_NEAR(geometryPoseMsg.position.y, referenceTranslation.y(), 1e-8); + EXPECT_NEAR(geometryPoseMsg.position.z, referenceTranslation.z(), 1e-8); +} + +TEST(RosGeometryMsgTransformationEigen, convertFromRosGeometryMsg) { + const kindr::Position3D referenceTranslation(0.12, 1.5, 0.6); + const kindr::RotationQuaternionPD referenceQuaternion(0.949, 0.133, 0.169, + 0.230541); + + geometry_msgs::msg::Transform geometryTransformMsg; + geometryTransformMsg.rotation.x = referenceQuaternion.x(); + geometryTransformMsg.rotation.y = referenceQuaternion.y(); + geometryTransformMsg.rotation.z = referenceQuaternion.z(); + geometryTransformMsg.rotation.w = referenceQuaternion.w(); + geometryTransformMsg.translation.x = referenceTranslation.x(); + geometryTransformMsg.translation.y = referenceTranslation.y(); + geometryTransformMsg.translation.z = referenceTranslation.z(); + + kindr::HomogeneousTransformationPosition3RotationQuaternionD transformation; + kindr_ros::convertFromRosGeometryMsg(geometryTransformMsg, transformation); + + kindr::expectNear(transformation.getPosition().toImplementation(), + referenceTranslation.toImplementation(), 1e-4, + KINDR_SOURCE_FILE_POS); + EXPECT_TRUE(transformation.getRotation().isNear(referenceQuaternion, 1e-8)); +} + +TEST(RosGeometryMsgTransformationEigen, convertToRosGeometryMsg) { + const kindr::Position3D referenceTranslation(0.3, -1.1345, -0.6); + const kindr::RotationQuaternionPD referenceQuaternion(0.212, 0.0421, -0.958, + 0.1885); + + kindr::HomogeneousTransformationPosition3RotationQuaternionD transformation; + transformation.getPosition() = referenceTranslation; + transformation.getRotation() = referenceQuaternion; + + geometry_msgs::msg::Transform geometryTransformMsg; + kindr_ros::convertToRosGeometryMsg(transformation, geometryTransformMsg); + + EXPECT_NEAR(geometryTransformMsg.rotation.x, referenceQuaternion.x(), 1e-8); + EXPECT_NEAR(geometryTransformMsg.rotation.y, referenceQuaternion.y(), 1e-8); + EXPECT_NEAR(geometryTransformMsg.rotation.z, referenceQuaternion.z(), 1e-8); + EXPECT_NEAR(geometryTransformMsg.rotation.w, referenceQuaternion.w(), 1e-8); + EXPECT_NEAR(geometryTransformMsg.translation.x, referenceTranslation.x(), + 1e-8); + EXPECT_NEAR(geometryTransformMsg.translation.y, referenceTranslation.y(), + 1e-8); + EXPECT_NEAR(geometryTransformMsg.translation.z, referenceTranslation.z(), + 1e-8); +} diff --git a/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgRotationTest.cpp b/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgRotationTest.cpp new file mode 100644 index 00000000..ec189fe2 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgRotationTest.cpp @@ -0,0 +1,69 @@ +/* + * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul + * Furgale, Remo Diethelm All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL Peter Fankhauser, Christian Gehring, Hannes + * Sommer, Paul Furgale, Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include + +#include +#include +#include +#include + +#include "kindr_ros/RosGeometryMsgRotation.hpp" + +TEST(RosGeometryMsgRotationQuaternionEigen, convertFromRosGeometryMsg) { + const kindr::RotationQuaternionPD referenceQuaternion(0.113, 0.071, -0.924, + 0.35835); + + geometry_msgs::msg::Quaternion geometryQuaternionMsg; + geometryQuaternionMsg.x = referenceQuaternion.x(); + geometryQuaternionMsg.y = referenceQuaternion.y(); + geometryQuaternionMsg.z = referenceQuaternion.z(); + geometryQuaternionMsg.w = referenceQuaternion.w(); + + kindr::RotationQuaternionPD rotationQuaternion; + kindr_ros::convertFromRosGeometryMsg(geometryQuaternionMsg, + rotationQuaternion); + + EXPECT_TRUE(rotationQuaternion.isNear(referenceQuaternion, 1e-8)); +} + +TEST(RosGeometryMsgRotationQuaternionEigen, convertToRosGeometryMsg) { + const kindr::RotationQuaternionPD referenceQuaternion(0.212, 0.0421, -0.958, + 0.1885); + + kindr::RotationQuaternionPD rotationQuaternion(referenceQuaternion); + + geometry_msgs::msg::Quaternion geometryQuaternionMsg; + kindr_ros::convertToRosGeometryMsg(rotationQuaternion, geometryQuaternionMsg); + + EXPECT_NEAR(geometryQuaternionMsg.x, referenceQuaternion.x(), 1e-8); + EXPECT_NEAR(geometryQuaternionMsg.y, referenceQuaternion.y(), 1e-8); + EXPECT_NEAR(geometryQuaternionMsg.z, referenceQuaternion.z(), 1e-8); + EXPECT_NEAR(geometryQuaternionMsg.w, referenceQuaternion.w(), 1e-8); +} diff --git a/src/Nav/kindr_ros/kindr_ros/test/RosTfPoseTest.cpp b/src/Nav/kindr_ros/kindr_ros/test/RosTfPoseTest.cpp new file mode 100644 index 00000000..b582493a --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/test/RosTfPoseTest.cpp @@ -0,0 +1,305 @@ +/* + * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul + * Furgale, Remo Diethelm All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL Peter Fankhauser, Christian Gehring, Hannes + * Sommer, Paul Furgale, Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF + * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING + * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "kindr_ros/RosTfPose.hpp" + +TEST(RosTfPoseEigen, convertFromRosTf) { + const kindr::RotationQuaternionD referenceQuaternion(0.949, 0.133, 0.169, + 0.2305); + const kindr::RotationMatrixD referenceRotationMatrix(referenceQuaternion); + const Eigen::Matrix3d referenceRotationMatrixEigen( + referenceRotationMatrix.matrix()); + const kindr::Position3D referenceTranslation(0.3, -1.5, 0.6); + const tf2::Vector3 referenceOriginTf(referenceTranslation.x(), + referenceTranslation.y(), + referenceTranslation.z()); + const tf2::Matrix3x3 referenceBasisTf( + referenceRotationMatrixEigen(0, 0), referenceRotationMatrixEigen(0, 1), + referenceRotationMatrixEigen(0, 2), referenceRotationMatrixEigen(1, 0), + referenceRotationMatrixEigen(1, 1), referenceRotationMatrixEigen(1, 2), + referenceRotationMatrixEigen(2, 0), referenceRotationMatrixEigen(2, 1), + referenceRotationMatrixEigen(2, 2)); + const tf2::Quaternion referenceQuaternionTf( + referenceQuaternion.x(), referenceQuaternion.y(), referenceQuaternion.z(), + referenceQuaternion.w()); + + kindr::HomogeneousTransformationPosition3RotationQuaternionD pose; + tf2::Transform tfTransform; + tf2::Transform tfTransformInverse; + + // Setting & getting identity + tfTransform.setIdentity(); + kindr_ros::convertFromRosTf(tfTransform, pose); + kindr::expectNear(pose.getTransformationMatrix(), Eigen::Matrix4d::Identity(), + 1e-4, KINDR_SOURCE_FILE_POS); + kindr::expectNear(pose.getPosition().toImplementation(), + kindr::Position3D::Zero().toImplementation(), 1e-4, + KINDR_SOURCE_FILE_POS); + + // Setting & getting with rotation matrix + tfTransform.setOrigin(referenceOriginTf); + tfTransform.setBasis(referenceBasisTf); + kindr_ros::convertFromRosTf(tfTransform, pose); + kindr::expectNear(kindr::RotationMatrixD(pose.getRotation()).matrix(), + referenceRotationMatrix.matrix(), 1e-4, + KINDR_SOURCE_FILE_POS); + kindr::expectNear(pose.getPosition().toImplementation(), + referenceTranslation.toImplementation(), 1e-4, + KINDR_SOURCE_FILE_POS); + + // Identity transformation + const kindr::Position3D testVector1(6.6, 0.0, -3.2); + tfTransform.setIdentity(); + tf2::Vector3 tfTestVectorTransformed1 = tfTransform( + tf2::Vector3(testVector1.x(), testVector1.y(), testVector1.z())); + kindr_ros::convertFromRosTf(tfTransform, pose); + kindr::Position3D testVectorTransformed1 = pose.transform(testVector1); + EXPECT_NEAR(testVectorTransformed1.x(), testVector1.x(), 1e-8); + EXPECT_NEAR(testVectorTransformed1.y(), testVector1.y(), 1e-8); + EXPECT_NEAR(testVectorTransformed1.z(), testVector1.z(), 1e-8); + + // General transformation + const kindr::Position3D testVector2(-18.4, 41.4, -0.2); + tfTransform.setOrigin(referenceOriginTf); + tfTransform.setBasis(referenceBasisTf); + tf2::Vector3 tfTestVectorTransformed2 = tfTransform( + tf2::Vector3(testVector2.x(), testVector2.y(), testVector2.z())); + kindr_ros::convertFromRosTf(tfTransform, pose); + kindr::Position3D testVectorTransformed2 = pose.transform(testVector2); + EXPECT_NEAR(testVectorTransformed2.x(), tfTestVectorTransformed2.x(), 1e-2); + EXPECT_NEAR(testVectorTransformed2.y(), tfTestVectorTransformed2.y(), 1e-2); + EXPECT_NEAR(testVectorTransformed2.z(), tfTestVectorTransformed2.z(), 1e-2); + + // General inverse transformation + const kindr::Position3D testVector3(0.6, -0.4, -3.2); + tfTransform.setOrigin(referenceOriginTf); + tfTransform.setBasis(referenceBasisTf); + tfTransformInverse = tfTransform.inverse(); + tf2::Vector3 tfTestVectorTransformed3 = tfTransformInverse( + tf2::Vector3(testVector3.x(), testVector3.y(), testVector3.z())); + kindr_ros::convertFromRosTf(tfTransform, pose); + kindr::Position3D testVectorTransformed3 = pose.inverseTransform(testVector3); + EXPECT_NEAR(testVectorTransformed3.x(), tfTestVectorTransformed3.x(), 1e-2); + EXPECT_NEAR(testVectorTransformed3.y(), tfTestVectorTransformed3.y(), 1e-2); + EXPECT_NEAR(testVectorTransformed3.z(), tfTestVectorTransformed3.z(), 1e-2); + + // Zero vector transformation + const kindr::Position3D testVector4(0.0, 0.0, 0.0); + tfTransform.setOrigin(referenceOriginTf); + tfTransform.setBasis(referenceBasisTf); + tf2::Vector3 tfTestVectorTransformed4 = tfTransform( + tf2::Vector3(testVector4.x(), testVector4.y(), testVector4.z())); + kindr_ros::convertFromRosTf(tfTransform, pose); + kindr::Position3D testVectorTransformed4 = pose.transform(testVector4); + EXPECT_NEAR(testVectorTransformed4.x(), tfTestVectorTransformed4.x(), 1e-4); + EXPECT_NEAR(testVectorTransformed4.y(), tfTestVectorTransformed4.y(), 1e-4); + EXPECT_NEAR(testVectorTransformed4.z(), tfTestVectorTransformed4.z(), 1e-4); + + // Only rotation transformation + const kindr::Position3D testVector5(1.0, 3.0, -4.0); + tfTransform.setIdentity(); + tfTransform.setBasis(referenceBasisTf); + tf2::Vector3 tfTestVectorTransformed5 = tfTransform( + tf2::Vector3(testVector5.x(), testVector5.y(), testVector5.z())); + kindr_ros::convertFromRosTf(tfTransform, pose); + kindr::Position3D testVectorTransformed5 = pose.transform(testVector5); + EXPECT_NEAR(testVectorTransformed5.x(), tfTestVectorTransformed5.x(), 1e-2); + EXPECT_NEAR(testVectorTransformed5.y(), tfTestVectorTransformed5.y(), 1e-2); + EXPECT_NEAR(testVectorTransformed5.z(), tfTestVectorTransformed5.z(), 1e-2); + + // Only translation + const kindr::Position3D testVector6(0.0, 1.0, -44.0); + tfTransform.setIdentity(); + tfTransform.setOrigin(referenceOriginTf); + tf2::Vector3 tfTestVectorTransformed6 = tfTransform( + tf2::Vector3(testVector6.x(), testVector6.y(), testVector6.z())); + kindr_ros::convertFromRosTf(tfTransform, pose); + kindr::Position3D testVectorTransformed6 = pose.transform(testVector6); + EXPECT_NEAR(testVectorTransformed6.x(), tfTestVectorTransformed6.x(), 1e-8); + EXPECT_NEAR(testVectorTransformed6.y(), tfTestVectorTransformed6.y(), 1e-8); + EXPECT_NEAR(testVectorTransformed6.z(), tfTestVectorTransformed6.z(), 1e-8); +} + +TEST(RosTfPoseEigen, convertToRosTf) { + const kindr::RotationQuaternionD referenceQuaternion(0.113, 0.071, -0.924, + 0.3585); + const kindr::RotationMatrixD referenceRotationMatrix(referenceQuaternion); + const kindr::Position3D referenceTranslation(-22.4, 0.31, -4.6); + + kindr::HomogeneousTransformationPosition3RotationQuaternionD pose; + tf2::Transform tfTransform; + tf2::Transform tfTransformInverse; + + // Identity transformation + const kindr::Position3D testVector1(0.6, -0.4, -3.2); + pose.setIdentity(); + kindr::Position3D testVector1Transformed = pose.transform(testVector1); + kindr_ros::convertToRosTf(pose, tfTransform); + tf2::Vector3 tfTestVector1Transformed = tfTransform( + tf2::Vector3(testVector1.x(), testVector1.y(), testVector1.z())); + EXPECT_NEAR(tfTestVector1Transformed.x(), testVector1.x(), 1e-8); + EXPECT_NEAR(tfTestVector1Transformed.y(), testVector1.y(), 1e-8); + EXPECT_NEAR(tfTestVector1Transformed.z(), testVector1.z(), 1e-8); + + // General transformation + const kindr::Position3D testVector2(57.6, -4.4, 333.2); + pose.getPosition() = referenceTranslation; + pose.getRotation() = referenceQuaternion; + kindr::Position3D testVector2Transformed = pose.transform(testVector2); + kindr_ros::convertToRosTf(pose, tfTransform); + tf2::Vector3 tfTestVector2Transformed = tfTransform( + tf2::Vector3(testVector2.x(), testVector2.y(), testVector2.z())); + EXPECT_NEAR(tfTestVector2Transformed.x(), testVector2Transformed.x(), 1e-8); + EXPECT_NEAR(tfTestVector2Transformed.y(), testVector2Transformed.y(), 1e-8); + EXPECT_NEAR(tfTestVector2Transformed.z(), testVector2Transformed.z(), 1e-8); + + // General inverse transformation + const kindr::Position3D testVector3(27.6, 41.4, -3.2); + pose.getPosition() = referenceTranslation; + pose.getRotation() = referenceQuaternion; + kindr::Position3D testVector3Transformed = pose.inverseTransform(testVector3); + kindr_ros::convertToRosTf(pose, tfTransform); + tfTransformInverse = tfTransform.inverse(); + tf2::Vector3 tfTestVector3Transformed = tfTransformInverse( + tf2::Vector3(testVector3.x(), testVector3.y(), testVector3.z())); + EXPECT_NEAR(tfTestVector3Transformed.x(), testVector3Transformed.x(), 1e-8); + EXPECT_NEAR(tfTestVector3Transformed.y(), testVector3Transformed.y(), 1e-8); + EXPECT_NEAR(tfTestVector3Transformed.z(), testVector3Transformed.z(), 1e-8); + + // Zero vector transformation + const kindr::Position3D testVector4(0.0, 0.0, 0.0); + pose.getPosition() = referenceTranslation; + pose.getRotation() = referenceQuaternion; + kindr::Position3D testVector4Transformed = pose.transform(testVector4); + kindr_ros::convertToRosTf(pose, tfTransform); + tf2::Vector3 tfTestVector4Transformed = tfTransform( + tf2::Vector3(testVector4.x(), testVector4.y(), testVector4.z())); + EXPECT_NEAR(tfTestVector4Transformed.x(), testVector4Transformed.x(), 1e-8); + EXPECT_NEAR(tfTestVector4Transformed.y(), testVector4Transformed.y(), 1e-8); + EXPECT_NEAR(tfTestVector4Transformed.z(), testVector4Transformed.z(), 1e-8); + + // Only rotation transformation + const kindr::Position3D testVector5(1.0, 3.0, -4.0); + pose.setIdentity(); + pose.getRotation() = referenceQuaternion; + kindr::Position3D testVector5Transformed = pose.transform(testVector5); + kindr_ros::convertToRosTf(pose, tfTransform); + tf2::Vector3 tfTestVector5Transformed = tfTransform( + tf2::Vector3(testVector5.x(), testVector5.y(), testVector5.z())); + EXPECT_NEAR(tfTestVector5Transformed.x(), testVector5Transformed.x(), 1e-8); + EXPECT_NEAR(tfTestVector5Transformed.y(), testVector5Transformed.y(), 1e-8); + EXPECT_NEAR(tfTestVector5Transformed.z(), testVector5Transformed.z(), 1e-8); + + // Only translation + const kindr::Position3D testVector6(0.0, 1.0, -44.0); + pose.setIdentity(); + pose.getPosition() = referenceTranslation; + kindr::Position3D testVector6Transformed = pose.transform(testVector6); + kindr_ros::convertToRosTf(pose, tfTransform); + tf2::Vector3 tfTestVector6Transformed = tfTransform( + tf2::Vector3(testVector6.x(), testVector6.y(), testVector6.z())); + EXPECT_NEAR(tfTestVector6Transformed.x(), testVector6Transformed.x(), 1e-8); + EXPECT_NEAR(tfTestVector6Transformed.y(), testVector6Transformed.y(), 1e-8); + EXPECT_NEAR(tfTestVector6Transformed.z(), testVector6Transformed.z(), 1e-8); +} + +TEST(RosTfPoseEigen, convertTwoWays) { + const kindr::RotationQuaternionD referenceQuaternion(0.212, 0.0421, -0.958, + 0.1885); + const kindr::RotationMatrixD referenceRotationMatrix(referenceQuaternion); + const Eigen::Matrix3d referenceRotationMatrixEigen( + referenceRotationMatrix.matrix()); + const kindr::Position3D referenceTranslation(13.3, 2.6, -7.6); + const tf2::Vector3 referenceOriginTf(referenceTranslation.x(), + referenceTranslation.y(), + referenceTranslation.z()); + const tf2::Matrix3x3 referenceBasisTf( + referenceRotationMatrixEigen(0, 0), referenceRotationMatrixEigen(0, 1), + referenceRotationMatrixEigen(0, 2), referenceRotationMatrixEigen(1, 0), + referenceRotationMatrixEigen(1, 1), referenceRotationMatrixEigen(1, 2), + referenceRotationMatrixEigen(2, 0), referenceRotationMatrixEigen(2, 1), + referenceRotationMatrixEigen(2, 2)); + const tf2::Quaternion referenceQuaternionTf( + referenceQuaternion.x(), referenceQuaternion.y(), referenceQuaternion.z(), + referenceQuaternion.w()); + + kindr::HomogeneousTransformationPosition3RotationQuaternionD pose, + poseConverted; + tf2::Transform tfTransform, tfTransformConverted; + + // Kindr -> TF -> Kindr + pose.getPosition() = referenceTranslation; + pose.getRotation() = referenceQuaternion; + kindr_ros::convertToRosTf(pose, tfTransform); + kindr_ros::convertFromRosTf(tfTransform, poseConverted); + kindr::expectNear(poseConverted.getTransformationMatrix(), + pose.getTransformationMatrix(), 1e-4, + KINDR_SOURCE_FILE_POS); + + // TF -> Kindr -> TF + tfTransform.setOrigin(referenceOriginTf); + tfTransform.setBasis(referenceBasisTf); + kindr_ros::convertFromRosTf(tfTransform, pose); + kindr_ros::convertToRosTf(pose, tfTransformConverted); + EXPECT_NEAR(tfTransformConverted.getBasis().getRow(0).x(), + tfTransform.getBasis().getRow(0).x(), 1e-8); + EXPECT_NEAR(tfTransformConverted.getBasis().getRow(0).y(), + tfTransform.getBasis().getRow(0).y(), 1e-8); + EXPECT_NEAR(tfTransformConverted.getBasis().getRow(0).z(), + tfTransform.getBasis().getRow(0).z(), 1e-8); + EXPECT_NEAR(tfTransformConverted.getBasis().getRow(1).x(), + tfTransform.getBasis().getRow(1).x(), 1e-8); + EXPECT_NEAR(tfTransformConverted.getBasis().getRow(1).y(), + tfTransform.getBasis().getRow(1).y(), 1e-8); + EXPECT_NEAR(tfTransformConverted.getBasis().getRow(1).z(), + tfTransform.getBasis().getRow(1).z(), 1e-8); + EXPECT_NEAR(tfTransformConverted.getBasis().getRow(2).x(), + tfTransform.getBasis().getRow(2).x(), 1e-8); + EXPECT_NEAR(tfTransformConverted.getBasis().getRow(2).y(), + tfTransform.getBasis().getRow(2).y(), 1e-8); + EXPECT_NEAR(tfTransformConverted.getBasis().getRow(2).z(), + tfTransform.getBasis().getRow(2).z(), 1e-8); + EXPECT_NEAR(tfTransformConverted.getOrigin().x(), tfTransform.getOrigin().x(), + 1e-8); + EXPECT_NEAR(tfTransformConverted.getOrigin().y(), tfTransform.getOrigin().y(), + 1e-8); + EXPECT_NEAR(tfTransformConverted.getOrigin().z(), tfTransform.getOrigin().z(), + 1e-8); +} diff --git a/src/Nav/kindr_ros/kindr_ros/test/TfConventionTest.cpp b/src/Nav/kindr_ros/kindr_ros/test/TfConventionTest.cpp new file mode 100644 index 00000000..ecba6915 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/test/TfConventionTest.cpp @@ -0,0 +1,145 @@ +/* + * tf_kindr_test.cpp + * + * Created on: Jun 17, 2016 + * Author: Christian Gehring + */ + +#include +#include +#include +#include + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +namespace kindr { + +template <> +class RotationConversion { + typedef tf2::Quaternion Rotation; + typedef tf2::Vector3 Vector; + + public: + inline static void convertToOtherRotation( + Rotation& out, const kindr::RotationQuaternion& in) { + kindr::RotationQuaternion in2 = in; + out = tf2::Quaternion(in2.x(), in2.y(), in2.z(), in2.w()); + } + + inline static void convertToKindr(kindr::RotationQuaternion& out, + Rotation& in) { + out = kindr::RotationQuaternion(in.w(), in.x(), in.y(), in.z()); + } + + inline static void convertToVelocityVector( + Vector& out, Rotation& rot, const Eigen::Matrix& in) { + out = tf2::Vector3(in.x(), in.y(), in.z()); + } + + inline static void concatenate(Rotation& res, const Rotation& rot1, + const Rotation& rot2) { + res = rot2 * rot1; + } + + inline static void getRotationMatrixFromRotation( + Eigen::Matrix3d& rotationMatrix, const Rotation& quaternion) { + tf2::Matrix3x3 tfRotationmatrix; + tfRotationmatrix.setRotation(quaternion); + for (int i = 0; i < 3; i++) { + rotationMatrix(i, 0) = tfRotationmatrix.getRow(i).x(); + rotationMatrix(i, 1) = tfRotationmatrix.getRow(i).y(); + rotationMatrix(i, 2) = tfRotationmatrix.getRow(i).z(); + } + } + + inline static void rotateVector(Eigen::Matrix& A_r, + const Rotation& rotationBToA, + const Eigen::Matrix& B_r) { + tf2::Vector3 A_v = + tf2::quatRotate(rotationBToA, tf2::Vector3(B_r.x(), B_r.y(), B_r.z())); + A_r.x() = A_v.x(); + A_r.y() = A_v.y(); + A_r.z() = A_v.z(); + } +}; + +template <> +class RotationConversion { + typedef tf2::Matrix3x3 Rotation; + typedef tf2::Vector3 Vector; + + public: + inline static void convertToOtherRotation( + Rotation& out, const kindr::RotationQuaternion& in) { + kindr::RotationQuaternion in2 = in; + out.setRotation(tf2::Quaternion(in2.x(), in2.y(), in2.z(), in2.w())); + } + + inline static void convertToKindr(kindr::RotationQuaternion& out, + Rotation& matrix) { + tf2::Quaternion quat; + matrix.getRotation(quat); + out = kindr::RotationQuaternion(quat.w(), quat.x(), quat.y(), + quat.z()); + } + + inline static void convertToOtherVelocityVector( + Vector& out, Rotation& rot, const Eigen::Matrix& in) { + out = tf2::Vector3(in.x(), in.y(), in.z()); + } + + inline static void concatenate(Rotation& res, const Rotation& rot1, + const Rotation& rot2) { + res = rot2 * rot1; + } + + inline static void getRotationMatrixFromRotation( + Eigen::Matrix3d& rotationMatrix, const Rotation& matrix) { + for (int i = 0; i < 3; i++) { + rotationMatrix(i, 0) = matrix.getRow(i).x(); + rotationMatrix(i, 1) = matrix.getRow(i).y(); + rotationMatrix(i, 2) = matrix.getRow(i).z(); + } + } + + inline static void rotateVector(Eigen::Matrix& A_r, + const Rotation& rotationBToA, + const Eigen::Matrix& B_r) { + tf2::Vector3 B_v(B_r.x(), B_r.y(), B_r.z()); + tf2::Vector3 A_v = rotationBToA * B_v; + A_r.x() = A_v.x(); + A_r.y() = A_v.y(); + A_r.z() = A_v.z(); + } +}; + +} // namespace kindr +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +TEST(TfConventionTest, Concatenation) { + kindr::ConventionTest::testConcatenation(); + kindr::ConventionTest::testConcatenation(); +} + +TEST(TfConventionTest, Rotation) { + kindr::ConventionTest::testRotationMatrix(); + kindr::ConventionTest::testRotationMatrix(); +} + +// TEST(TfConventionTest, BoxPlus) { +// kindr::ConventionTest::testBoxPlus(); +//} + +TEST(TfConventionTest, GeometricalInterpretation) { + kindr::ConventionTest::testGeometricalInterpretation(); + kindr::ConventionTest::testGeometricalInterpretation(); +} diff --git a/src/Nav/kindr_ros/kindr_ros/test/test_main.cpp b/src/Nav/kindr_ros/kindr_ros/test/test_main.cpp new file mode 100644 index 00000000..b7473a58 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_ros/test/test_main.cpp @@ -0,0 +1,35 @@ +/* + * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo + * Diethelm All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul + * Furgale, Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ +#include + +/// Run all the tests that were declared with TEST() +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/Nav/kindr_ros/kindr_rviz_plugins/CMakeLists.txt b/src/Nav/kindr_ros/kindr_rviz_plugins/CMakeLists.txt new file mode 100644 index 00000000..0fadb3ad --- /dev/null +++ b/src/Nav/kindr_ros/kindr_rviz_plugins/CMakeLists.txt @@ -0,0 +1,122 @@ +cmake_minimum_required(VERSION 3.5.1) +project(kindr_rviz_plugins) + +# Set compiler flags +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + +find_package(catkin REQUIRED COMPONENTS + rviz + kindr_msgs +) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + rviz + kindr_msgs +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +link_directories(${catkin_LIBRARY_DIRS}) + +## This plugin includes Qt widgets, so we must include Qt like so: +if(rviz_QT_VERSION VERSION_GREATER "5.0.0") + find_package(Qt5Widgets REQUIRED) +else() + find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) + include(${QT_USE_FILE}) +endif() + +## I prefer the Qt signals and slots to avoid defining "emit", "slots", +## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. +add_definitions(-DQT_NO_KEYWORDS) + +## Here we specify which header files need to be run through "moc", +## Qt's meta-object compiler. +set(kindr_rviz_plugins_HDRS_QT + include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp +) +set(kindr_rviz_plugins_HDRS + include/kindr_rviz_plugins/VectorAtPositionVisual.hpp +) + +## Here we specify the list of source files, including the output of +## the previous command which is stored in ``${MOC_FILES}``. +set(kindr_rviz_plugins_SRCS + src/VectorAtPositionDisplay.cpp + src/VectorAtPositionVisual.cpp +) + +if(rviz_QT_VERSION VERSION_GREATER "5.0.0") + qt5_wrap_cpp(kindr_rviz_plugins_MOCS ${kindr_rviz_plugins_HDRS_QT}) +else() + qt4_wrap_cpp(kindr_rviz_plugins_MOCS ${kindr_rviz_plugins_HDRS_QT}) +endif() + +add_library(${PROJECT_NAME} + ${kindr_rviz_plugins_SRCS} + ${kindr_rviz_plugins_HDRS_QT} + ${kindr_rviz_plugins_HDRS} + ${kindr_rviz_plugins_MOCS} +) + +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} + ${catkin_LIBRARIES} + pthread +) + +if(rviz_QT_VERSION VERSION_GREATER "5.0.0") + target_link_libraries(${PROJECT_NAME} Qt5::Widgets) +else() + target_link_libraries(${PROJECT_NAME} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) +endif() + +## Install rules + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(FILES plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY icons + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +########## +## Test ## +########## +if(CATKIN_ENABLE_TESTING) + catkin_add_gtest(test_${PROJECT_NAME} + test/EmptyTests.cpp + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test + ) + + if(TARGET test_${PROJECT_NAME}) + target_link_libraries( test_${PROJECT_NAME} + ${PROJECT_NAME} + ${catkin_LIBRARIES} + gtest_main + ) + find_package(cmake_code_coverage QUIET) + if(cmake_code_coverage_FOUND) + add_gtest_coverage() + endif(cmake_code_coverage_FOUND) + endif() +endif() diff --git a/src/camera_controller/resource/camera_controller b/src/Nav/kindr_ros/kindr_rviz_plugins/COLCON_IGNORE similarity index 100% rename from src/camera_controller/resource/camera_controller rename to src/Nav/kindr_ros/kindr_rviz_plugins/COLCON_IGNORE diff --git a/src/Nav/kindr_ros/kindr_rviz_plugins/icons/classes/VectorAtPosition.png b/src/Nav/kindr_ros/kindr_rviz_plugins/icons/classes/VectorAtPosition.png new file mode 100644 index 00000000..4c789065 Binary files /dev/null and b/src/Nav/kindr_ros/kindr_rviz_plugins/icons/classes/VectorAtPosition.png differ diff --git a/src/Nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp b/src/Nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp new file mode 100644 index 00000000..3c2b6f0d --- /dev/null +++ b/src/Nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp @@ -0,0 +1,108 @@ +#pragma once + +#ifndef Q_MOC_RUN +// boost +#include + +// rviz +#include + +// kindr ros +#include + +// ogre +#include +#endif + +namespace Ogre { +class SceneNode; +} // namespace Ogre + +namespace rviz { +class ColorProperty; +class FloatProperty; +class IntProperty; +} // namespace rviz + +// All the source in this plugin is in its own namespace. This is not +// required but is good practice. +namespace kindr_rviz_plugins { + +class VectorAtPositionVisual; + +// Here we declare our new subclass of rviz::Display. Every display +// which can be listed in the "Displays" panel is a subclass of +// rviz::Display. +// +// VectorAtPositionDisplay will show a 3D arrow showing the direction and +// magnitude of the IMU acceleration vector. The base of the arrow will be at +// the frame listed in the header of the VectorAtPosition message, and the +// direction of the arrow will be relative to the orientation of that +// frame. It will also optionally show a history of recent +// acceleration vectors, which will be stored in a circular buffer. +// +// The VectorAtPositionDisplay class itself just implements the circular buffer, +// editable parameters, and Display subclass machinery. The visuals +// themselves are represented by a separate class, VectorAtPositionVisual. The +// idiom for the visuals is that when the objects exist, they appear +// in the scene, and when they are deleted, they disappear. +class VectorAtPositionDisplay + : public rviz::MessageFilterDisplay { + Q_OBJECT + public: + // Constructor. pluginlib::ClassLoader creates instances by calling + // the default constructor, so make sure you have one. + VectorAtPositionDisplay(); + virtual ~VectorAtPositionDisplay(); + + // Overrides of protected virtual functions from Display. As much + // as possible, when Displays are not enabled, they should not be + // subscribed to incoming data and should not show anything in the + // 3D view. These functions are where these connections are made + // and broken. + protected: + virtual void onInitialize(); + + // A helper to clear this display back to the initial state. + virtual void reset(); + + Q_SIGNALS: + void updateVectorAtPositionSignal(); + + // These Qt slots get connected to signals indicating changes in the + // user-editable properties. + private Q_SLOTS: + void updateScale(); + void updateShowText(); + void updateColorAndAlpha(); + void updateHistoryLength(); + + void updateVectorAtPosition(); + + // Function to handle an incoming ROS message. + private: + void processMessage(const kindr_msgs::VectorAtPosition::ConstPtr& msg); + + // Storage for the list of visuals. It is a circular buffer where + // data gets popped from the front (oldest) and pushed to the back (newest) + boost::circular_buffer > visuals_; + + // User-editable property variables. + rviz::FloatProperty* length_scale_property_; + rviz::FloatProperty* width_scale_property_; + rviz::BoolProperty* show_text_property_; + rviz::ColorProperty* color_property_; + rviz::FloatProperty* alpha_property_; + rviz::IntProperty* history_length_property_; + + // Storage of user editable values + float lengthScale_; + float widthScale_; + bool showText_; + Ogre::ColourValue color_; + float alpha_; + + kindr_msgs::VectorAtPosition::ConstPtr current_vector_at_position_; +}; + +} // namespace kindr_rviz_plugins diff --git a/src/Nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionVisual.hpp b/src/Nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionVisual.hpp new file mode 100644 index 00000000..34219359 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionVisual.hpp @@ -0,0 +1,110 @@ +#pragma once + +// rviz +#include + +// kindr ros +#include + +namespace Ogre { +class Vector3; +class Quaternion; +} // namespace Ogre + +namespace rviz { +class Arrow; +class BillboardLine; +class MovableText; +} // namespace rviz + +namespace kindr_rviz_plugins { + +// Declare the visual class for this display. +// +// Each instance of VectorAtPositionVisual represents the visualization of a +// single sensor_msgs::VectorAtPosition message. Currently it just shows an +// arrow with the direction and magnitude of the acceleration vector, but could +// easily be expanded to include more of the message data. +class VectorAtPositionVisual { + public: + // Constructor. Creates the visual stuff and puts it into the + // scene, but in an unconfigured state. + VectorAtPositionVisual(Ogre::SceneManager* scene_manager, + Ogre::SceneNode* parent_node); + + // Destructor. Removes the visual stuff from the scene. + virtual ~VectorAtPositionVisual(); + + // Configure the visual to show the data in the message. + void setMessage(const kindr_msgs::VectorAtPosition::ConstPtr& msg); + + // Set the pose of the coordinate frame the message refers to. + // These could be done inside setMessage(), but that would require + // calls to FrameManager and error handling inside setMessage(), + // which doesn't seem as clean. This way VectorAtPositionVisual is only + // responsible for visualization. + void setArrowPosition(const Ogre::Vector3& position); + void setArrowOrientation(const Ogre::Quaternion& orientation); + + // Set the scale of the visual, which are user-editable + // parameters and therefore don't come from the VectorAtPosition message. + void setScalingFactors(float lengthScalingFactor, float widthScalingFactor); + + // Enable or disable text rendering, which are user-editable + // parameters and therefore don't come from the VectorAtPosition message. + void setShowText(bool showText); + + // Set the color and alpha of the visual, which are user-editable + // parameters and therefore don't come from the VectorAtPosition message. + void setColor(const Ogre::ColourValue& color); + + protected: + // The object implementing the actual arrow shape + boost::shared_ptr arrow_; + boost::shared_ptr circleArrow_; + boost::shared_ptr circle_; + + // The object implementing the arrow description text + boost::shared_ptr text_; + + // Store the name + std::string name_; + + // Store ogre vector and orientation + Ogre::Vector3 vector_; + + // Store the scaling factor and the length of the vector, so that it can be + // scale afterwards + float length_; + float lengthScalingFactor_; + float widthScalingFactor_; + + // Store show text + bool showText_; + bool showTorque_; + + // Store the arrow color + Ogre::ColourValue color_; + + // A SceneNode whose pose is set to match the coordinate frame of + // the VectorAtPosition message header. + Ogre::SceneNode* scene_node_frame_; + Ogre::SceneNode* scene_node_arrow_; + Ogre::SceneNode* scene_node_circle_; + Ogre::SceneNode* scene_node_text_; + + // The SceneManager, kept here only so the destructor can ask it to + // destroy the ``frame_node_``. + Ogre::SceneManager* scene_manager_; + + // Update the scaling of the arrow + void updateScaling(); + + // Update the color of the arrow + void updateColor(); + + // Update the text of the arrow + void updateText(); +}; + +} // namespace kindr_rviz_plugins diff --git a/src/Nav/kindr_ros/kindr_rviz_plugins/package.xml b/src/Nav/kindr_ros/kindr_rviz_plugins/package.xml new file mode 100644 index 00000000..391956e4 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_rviz_plugins/package.xml @@ -0,0 +1,26 @@ + + + kindr_rviz_plugins + 0.3.1 + Plugins for visualizing kindr objects in rviz. + Remo Diethelm + Philipp Leemann + BSD + https://github.com/anybotics/kindr_ros + + none + Remo Diethelm + + catkin + + rviz + kindr_msgs + qtbase5-dev + + gtest + + + + + + diff --git a/src/Nav/kindr_ros/kindr_rviz_plugins/plugin_description.xml b/src/Nav/kindr_ros/kindr_rviz_plugins/plugin_description.xml new file mode 100644 index 00000000..0e16ab2b --- /dev/null +++ b/src/Nav/kindr_ros/kindr_rviz_plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + A class for displaying the kindr vector at position message. + + + diff --git a/src/Nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp b/src/Nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp new file mode 100644 index 00000000..bdffb776 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp @@ -0,0 +1,218 @@ +// ogre +#include +#include + +// tf +#include + +// rviz +#include +#include +#include +#include +#include + +// kindr rviz plugins +#include "kindr_rviz_plugins/VectorAtPositionDisplay.hpp" +#include "kindr_rviz_plugins/VectorAtPositionVisual.hpp" + +namespace kindr_rviz_plugins { + +// The constructor must have no arguments, so we can't give the +// constructor the parameters it needs to fully initialize. +VectorAtPositionDisplay::VectorAtPositionDisplay() + : lengthScale_(1.0f), + widthScale_(1.0f), + showText_(true), + color_(Ogre::ColourValue::Black), + alpha_(1.0) { + connect(this, SIGNAL(updateVectorAtPositionSignal()), this, + SLOT(updateVectorAtPosition())); + + length_scale_property_ = new rviz::FloatProperty( + "Length scale", 1.0, "Scale of the length of the vector.", this, + SLOT(updateScale())); + + width_scale_property_ = new rviz::FloatProperty( + "Width scale", 1.0, "Scale of the width of the vector.", this, + SLOT(updateScale())); + width_scale_property_->setMin(0); + + show_text_property_ = new rviz::BoolProperty( + "Show text", true, "Enable or disable text rendering.", this, + SLOT(updateShowText())); + + color_property_ = new rviz::ColorProperty( + "Color", QColor(0, 0, 0), + "Color to draw the vector (if not defined by vector type).", this, + SLOT(updateColorAndAlpha())); + + alpha_property_ = new rviz::FloatProperty( + "Alpha", 1.0, "0 is fully transparent, 1.0 is fully opaque.", this, + SLOT(updateColorAndAlpha())); + + history_length_property_ = new rviz::IntProperty( + "History Length", 1, "Number of prior measurements to display.", this, + SLOT(updateHistoryLength())); + history_length_property_->setMin(1); + history_length_property_->setMax(100000); +} + +// After the top-level rviz::Display::initialize() does its own setup, +// it calls the subclass's onInitialize() function. This is where we +// instantiate all the workings of the class. We make sure to also +// call our immediate super-class's onInitialize() function, since it +// does important stuff setting up the message filter. +// +// Note that "MFDClass" is a typedef of +// ``MessageFilterDisplay``, to save typing that long +// templated class name every time you need to refer to the +// superclass. +void VectorAtPositionDisplay::onInitialize() { + MFDClass::onInitialize(); + updateHistoryLength(); +} + +VectorAtPositionDisplay::~VectorAtPositionDisplay() {} + +// Clear the visuals by deleting their objects. +void VectorAtPositionDisplay::reset() { + MFDClass::reset(); + visuals_.clear(); +} + +// Set the scale. +void VectorAtPositionDisplay::updateScale() { + lengthScale_ = length_scale_property_->getFloat(); + widthScale_ = width_scale_property_->getFloat(); + + for (size_t i = 0; i < visuals_.size(); i++) { + visuals_[i]->setScalingFactors(lengthScale_, widthScale_); + } +} + +// Show text. +void VectorAtPositionDisplay::updateShowText() { + showText_ = show_text_property_->getBool(); + + for (size_t i = 0; i < visuals_.size(); i++) { + visuals_[i]->setShowText(showText_); + } +} + +// Set the current color and alpha values for each visual. +void VectorAtPositionDisplay::updateColorAndAlpha() { + color_ = color_property_->getOgreColor(); + color_.a = alpha_property_->getFloat(); + + for (size_t i = 0; i < visuals_.size(); i++) { + visuals_[i]->setColor(color_); + } +} + +// Set the number of past visuals to show. +void VectorAtPositionDisplay::updateHistoryLength() { + visuals_.rset_capacity(history_length_property_->getInt()); +} + +void VectorAtPositionDisplay::updateVectorAtPosition() { + // Here we call the rviz::FrameManager to get the transform from the + // fixed frame to the frame in the header of this VectorAtPosition message. + Ogre::Vector3 positionFixedToPositionFrameInFixedFrame; + Ogre::Vector3 positionFixedToArrowInFixedFrame; + Ogre::Quaternion orientationArrowFrameToFixedFrame; + Ogre::Quaternion orientationPositionFrameToFixedFrame; + + // Check if the position has an empty or the same frame as the vector + if (current_vector_at_position_->position_frame_id.empty() || + current_vector_at_position_->position_frame_id == + current_vector_at_position_->header.frame_id) { + // Arrow and position frame coincide. + + // Get arrow position and orientation + if (!context_->getFrameManager()->getTransform( + current_vector_at_position_->header.frame_id, + current_vector_at_position_->header.stamp, + positionFixedToPositionFrameInFixedFrame, + orientationArrowFrameToFixedFrame)) { + ROS_ERROR("Error transforming from frame '%s' to frame '%s'", + current_vector_at_position_->position_frame_id.c_str(), + qPrintable(fixed_frame_)); + return; + } + + orientationPositionFrameToFixedFrame = orientationArrowFrameToFixedFrame; + } else { + // Get arrow position + if (!context_->getFrameManager()->getTransform( + current_vector_at_position_->position_frame_id, + current_vector_at_position_->header.stamp, + positionFixedToPositionFrameInFixedFrame, + orientationPositionFrameToFixedFrame)) { + ROS_ERROR("Error transforming from frame '%s' to frame '%s'", + current_vector_at_position_->position_frame_id.c_str(), + qPrintable(fixed_frame_)); + return; + } + + // Get arrow orientation + Ogre::Vector3 dummyPosition; + if (!context_->getFrameManager()->getTransform( + current_vector_at_position_->header.frame_id, + current_vector_at_position_->header.stamp, dummyPosition, + orientationArrowFrameToFixedFrame)) { + ROS_ERROR("Error transforming from frame '%s' to frame '%s'", + current_vector_at_position_->header.frame_id.c_str(), + qPrintable(fixed_frame_)); + return; + } + } + + Ogre::Matrix3 rotMat; + orientationPositionFrameToFixedFrame.ToRotationMatrix(rotMat); + positionFixedToArrowInFixedFrame = + positionFixedToPositionFrameInFixedFrame + + rotMat * Ogre::Vector3(current_vector_at_position_->position.x, + current_vector_at_position_->position.y, + current_vector_at_position_->position.z); + + // We are keeping a circular buffer of visual pointers. This gets + // the next one, or creates and stores it if the buffer is not full + boost::shared_ptr visual; + if (visuals_.full()) { + visual = visuals_.front(); + } else { + visual.reset( + new VectorAtPositionVisual(context_->getSceneManager(), scene_node_)); + } + + // Now set or update the contents of the chosen visual. + visual->setMessage(current_vector_at_position_); + visual->setArrowPosition( + positionFixedToArrowInFixedFrame); // position is taken from position in + // msg + visual->setArrowOrientation( + orientationArrowFrameToFixedFrame); // orientation is taken from vector + // in msg + visual->setScalingFactors(lengthScale_, widthScale_); + visual->setShowText(showText_); + visual->setColor(color_); + + // And send it to the end of the circular buffer + visuals_.push_back(visual); +} + +// This is our callback to handle an incoming message. +void VectorAtPositionDisplay::processMessage( + const kindr_msgs::VectorAtPosition::ConstPtr& msg) { + current_vector_at_position_ = msg; + Q_EMIT updateVectorAtPositionSignal(); +} + +} // namespace kindr_rviz_plugins + +// Tell pluginlib about this class. It is important to do this in +// global scope, outside our package's namespace. +#include +PLUGINLIB_EXPORT_CLASS(kindr_rviz_plugins::VectorAtPositionDisplay, + rviz::Display) diff --git a/src/Nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp b/src/Nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp new file mode 100644 index 00000000..5ed7c7d3 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp @@ -0,0 +1,200 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "kindr_rviz_plugins/VectorAtPositionVisual.hpp" + +#include +#include +#include +#include +#include +#include + +namespace kindr_rviz_plugins { + +VectorAtPositionVisual::VectorAtPositionVisual( + Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node) + : vector_(Ogre::Vector3::UNIT_Z), + length_(0.0), + lengthScalingFactor_(1.0), + widthScalingFactor_(1.0), + showText_(true), + showTorque_(false), + color_(Ogre::ColourValue(0, 0, 0, 1)) { + scene_manager_ = scene_manager; + + // Ogre::SceneNode s form a tree, with each node storing the + // transform (position and orientation) of itself relative to its + // parent. Ogre does the math of combining those transforms when it + // is time to render. + // + // Here we create a node to store the pose of the VectorAtPosition's header + // frame relative to the RViz fixed frame. + scene_node_frame_ = parent_node->createChildSceneNode(); + scene_node_arrow_ = scene_node_frame_->createChildSceneNode(); + scene_node_circle_ = scene_node_frame_->createChildSceneNode(); + scene_node_text_ = scene_node_arrow_->createChildSceneNode(); + scene_node_text_->setPosition(Ogre::Vector3(0, 0, -0.05)); + + // We create the arrow object within the frame node so that we can + // set its position and direction relative to its header frame. + arrow_.reset(new rviz::Arrow(scene_manager_, scene_node_arrow_, 0.8f, 0.07f, + 0.2f, 0.15f)); + circle_.reset(new rviz::BillboardLine(scene_manager_, scene_node_circle_)); + circleArrow_.reset(new rviz::Arrow(scene_manager_, scene_node_circle_)); +} + +VectorAtPositionVisual::~VectorAtPositionVisual() { + // Destroy the frame node since we don't need it anymore. + scene_manager_->destroySceneNode(scene_node_frame_); + scene_manager_->destroySceneNode(scene_node_arrow_); + scene_manager_->destroySceneNode(scene_node_circle_); + scene_manager_->destroySceneNode(scene_node_text_); +} + +void VectorAtPositionVisual::setMessage( + const kindr_msgs::VectorAtPosition::ConstPtr& msg) { + // Check if type is torque + showTorque_ = msg->type == msg->TYPE_TORQUE; + + // Set the name of the arrow. + name_ = msg->name; + + // Convert the geometry_msgs::Vector3 to an Ogre::Vector3. Set torque + // orientation. + vector_ = Ogre::Vector3(msg->vector.x, msg->vector.y, msg->vector.z); + + // Set the position of the arrow. + arrow_->setPosition(Ogre::Vector3(0, 0, 0)); + + // Set the orientation of the arrow to match the direction of the vector. + arrow_->setDirection(vector_); + + // Find the magnitude of the vector. + length_ = vector_.length(); + + // update the scaling + updateScaling(); + + // update the color + updateColor(); + + // update the text + updateText(); + + // show circle arrow only if type is torque + scene_node_circle_->setVisible(showTorque_); +} + +void VectorAtPositionVisual::setArrowPosition(const Ogre::Vector3& position) { + scene_node_frame_->setPosition(position); +} + +void VectorAtPositionVisual::setArrowOrientation( + const Ogre::Quaternion& orientation) { + scene_node_frame_->setOrientation(orientation); +} + +void VectorAtPositionVisual::setScalingFactors(float lengthScalingFactor, + float widthScalingFactor) { + lengthScalingFactor_ = lengthScalingFactor; + widthScalingFactor_ = widthScalingFactor; + updateScaling(); +} + +void VectorAtPositionVisual::setColor(const Ogre::ColourValue& color) { + color_ = color; + updateColor(); +} + +void VectorAtPositionVisual::setShowText(bool showText) { + showText_ = showText; + updateText(); +} + +void VectorAtPositionVisual::updateScaling() { + // Scale the arrow's thickness in each dimension along with its length and + // scaling factors. + arrow_->setScale(Ogre::Vector3(lengthScalingFactor_ * length_, + widthScalingFactor_, widthScalingFactor_)); + + // Scale the circle arrow + if (showTorque_) { + // Get orientation of arrow in scene + Ogre::Vector3 axis_z(0, 0, 1); + Ogre::Quaternion orientation = + axis_z.getRotationTo(lengthScalingFactor_ * vector_); + if (orientation.isNaN()) { + orientation = Ogre::Quaternion::IDENTITY; + } + + // Set position and direction of the arrowhead + circleArrow_->set(0, widthScalingFactor_ * 0.1, + widthScalingFactor_ * 0.1 * 1.0, + widthScalingFactor_ * 0.1 * 2.0); + circleArrow_->setPosition( + orientation * + Ogre::Vector3(std::fabs(lengthScalingFactor_) * length_ / 4, 0, + std::fabs(lengthScalingFactor_) * length_ / 2)); + circleArrow_->setDirection(orientation * Ogre::Vector3(0, 1, 0)); + + // Add circular arrow points + circle_->clear(); + circle_->setLineWidth(widthScalingFactor_ * 0.05); + for (int i = 4; i <= 32; i++) { + Ogre::Vector3 point = + Ogre::Vector3((std::fabs(lengthScalingFactor_) * length_ / 4) * + cos(i * 2 * M_PI / 32), + (std::fabs(lengthScalingFactor_) * length_ / 4) * + sin(i * 2 * M_PI / 32), + std::fabs(lengthScalingFactor_) * length_ / 2); + circle_->addPoint(orientation * point); + } + } +} + +void VectorAtPositionVisual::updateColor() { + arrow_->setColor(color_); + if (showTorque_) { + circle_->setColor(color_.r, color_.g, color_.b, color_.a); + circleArrow_->setColor(color_.r, color_.g, color_.b, color_.a); + } +} + +void VectorAtPositionVisual::updateText() { + // add description text if available + const std::string textString = showText_ ? name_ : ""; + text_.reset(new rviz::MovableText(textString)); + text_->setCharacterHeight(0.1); + text_->setTextAlignment(rviz::MovableText::H_CENTER, + rviz::MovableText::V_BELOW); + scene_node_text_->attachObject(text_.get()); +} + +} // namespace kindr_rviz_plugins diff --git a/src/Nav/kindr_ros/kindr_rviz_plugins/test/EmptyTests.cpp b/src/Nav/kindr_ros/kindr_rviz_plugins/test/EmptyTests.cpp new file mode 100644 index 00000000..4c702df3 --- /dev/null +++ b/src/Nav/kindr_ros/kindr_rviz_plugins/test/EmptyTests.cpp @@ -0,0 +1,5 @@ +#include + +TEST(Placeholder, emptyTest) { // NOLINT + ASSERT_TRUE(true); +} diff --git a/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/CMakeLists.txt b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/CMakeLists.txt new file mode 100644 index 00000000..e6937bc4 --- /dev/null +++ b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/CMakeLists.txt @@ -0,0 +1,120 @@ +cmake_minimum_required(VERSION 3.5.1) +project(multi_dof_joint_trajectory_rviz_plugins) + +# Set compiler flags +set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") + +find_package(catkin REQUIRED COMPONENTS + rviz + trajectory_msgs +) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + rviz + trajectory_msgs +) + +link_directories(${catkin_LIBRARY_DIRS}) + +## This plugin includes Qt widgets, so we must include Qt like so: +if(rviz_QT_VERSION VERSION_GREATER "5.0.0") + find_package(Qt5Widgets REQUIRED) +else() + find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) + include(${QT_USE_FILE}) +endif() + +## I prefer the Qt signals and slots to avoid defining "emit", "slots", +## etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. +add_definitions(-DQT_NO_KEYWORDS) + +## Here we specify which header files need to be run through "moc", +## Qt's meta-object compiler. +set(multi_dof_joint_trajectory_rviz_plugins_HDRS_QT + include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp +) +set(multi_dof_joint_trajectory_rviz_plugins_HDRS + include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp + include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp +) + +set(multi_dof_joint_trajectory_rviz_plugins_SRCS + src/MultiDOFJointTrajectoryDisplay.cpp + src/MultiDOFJointTrajectoryPointConnectionVisual.cpp + src/MultiDOFJointTrajectoryPointVisual.cpp +) + +if(rviz_QT_VERSION VERSION_GREATER "5.0.0") + qt5_wrap_cpp(multi_dof_joint_trajectory_rviz_plugins_MOCS + ${multi_dof_joint_trajectory_rviz_plugins_HDRS_QT} + ) +else() + qt4_wrap_cpp(multi_dof_joint_trajectory_rviz_plugins_MOCS + ${multi_dof_joint_trajectory_rviz_plugins_HDRS_QT} + ) +endif() + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + ${multi_dof_joint_trajectory_rviz_plugins_SRCS} + ${multi_dof_joint_trajectory_rviz_plugins_HDRS_QT} + ${multi_dof_joint_trajectory_rviz_plugins_HDRS} + ${multi_dof_joint_trajectory_rviz_plugins_MOCS} +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} ${catkin_LIBRARIES} +) + +if(rviz_QT_VERSION VERSION_GREATER "5.0.0") + target_link_libraries(${PROJECT_NAME} Qt5::Widgets) +else() + target_link_libraries(${PROJECT_NAME} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) +endif() + +## Install rules + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(FILES plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY icons + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +########## +## Test ## +########## +if(CATKIN_ENABLE_TESTING) + catkin_add_gtest(test_${PROJECT_NAME} + test/EmptyTests.cpp + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test + ) + + if(TARGET test_${PROJECT_NAME}) + target_link_libraries( test_${PROJECT_NAME} + ${PROJECT_NAME} + ${catkin_LIBRARIES} + gtest_main + ) + find_package(cmake_code_coverage QUIET) + if(cmake_code_coverage_FOUND) + add_gtest_coverage() + endif(cmake_code_coverage_FOUND) + endif() +endif() diff --git a/src/drive/resource/drive b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/COLCON_IGNORE similarity index 100% rename from src/drive/resource/drive rename to src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/COLCON_IGNORE diff --git a/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/icons/classes/MultiDOFJointTrajectory.png b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/icons/classes/MultiDOFJointTrajectory.png new file mode 100644 index 00000000..4e415e0f Binary files /dev/null and b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/icons/classes/MultiDOFJointTrajectory.png differ diff --git a/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp new file mode 100644 index 00000000..d141d7d6 --- /dev/null +++ b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp @@ -0,0 +1,160 @@ +#pragma once + +#ifndef Q_MOC_RUN +#include +#include + +#include +#include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp" +#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp" + +namespace multi_dof_joint_trajectory_rviz_plugins { + +class MultiDOFJointTrajectoryDisplay + : public rviz::MessageFilterDisplay< + trajectory_msgs::MultiDOFJointTrajectory> { + Q_OBJECT + public: + MultiDOFJointTrajectoryDisplay(); + virtual ~MultiDOFJointTrajectoryDisplay(); + + protected: + virtual void onInitialize(); + virtual void reset(); + + Q_SIGNALS: + void updateTrajectorySignal(); + + private Q_SLOTS: + void setShowConnection(); + void setShowTransformRotation(); + void setShowVelocityLinear(); + void setShowVelocityAngular(); + void setShowAccelerationLinear(); + void setShowAccelerationAngular(); + + void setSizeTransformRotation(); + void setDiameterArrows(); + void setScaleVelocityLinear(); + void setScaleVelocityAngular(); + void setScaleAccelerationLinear(); + void setScaleAccelerationAngular(); + + void setAlpha(); + void setColorConnection(); + void setColorVelocityLinear(); + void setColorVelocityAngular(); + void setColorAccelerationLinear(); + void setColorAccelerationAngular(); + + void setFontSize(); + void setShowText(); + + void setHistoryLength(); + + void updateTrajectory(); + + private: + void processMessage( + const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr& msg); + + void updateShowConnection(); + void updateShowTransformRotation(); + void updateShowVelocityLinear(); + void updateShowVelocityAngular(); + void updateShowAccelerationLinear(); + void updateShowAccelerationAngular(); + + void updateSizeTransformRotation(); + void updateDiameterArrows(); + void updateScaleVelocityLinear(); + void updateScaleVelocityAngular(); + void updateScaleAccelerationLinear(); + void updateScaleAccelerationAngular(); + + void updateColorConnection(); + void updateAlphaTransformRotation(); + void updateColorVelocityLinear(); + void updateColorVelocityAngular(); + void updateColorAccelerationLinear(); + void updateColorAccelerationAngular(); + + void updateFontSize(); + void updateShowText(); + + boost::circular_buffer< + std::vector>> + visuals_points_; + boost::circular_buffer>> + visuals_connections_; + + rviz::BoolProperty* property_show_connection_; + rviz::BoolProperty* property_show_transform_rotation_; + rviz::BoolProperty* property_show_velocity_linear_; + rviz::BoolProperty* property_show_velocity_angular_; + rviz::BoolProperty* property_show_acceleration_linear_; + rviz::BoolProperty* property_show_acceleration_angular_; + + rviz::FloatProperty* property_size_transform_rotation_; + rviz::FloatProperty* property_diameter_arrows_; + rviz::FloatProperty* property_scale_velocity_linear_; + rviz::FloatProperty* property_scale_velocity_angular_; + rviz::FloatProperty* property_scale_acceleration_linear_; + rviz::FloatProperty* property_scale_acceleration_angular_; + + rviz::ColorProperty* property_color_connection_; + rviz::ColorProperty* property_color_velocity_linear_; + rviz::ColorProperty* property_color_velocity_angular_; + rviz::ColorProperty* property_color_acceleration_linear_; + rviz::ColorProperty* property_color_acceleration_angular_; + + rviz::FloatProperty* property_alpha_; + + rviz::FloatProperty* property_font_size_; + rviz::BoolProperty* property_show_text_; + + rviz::IntProperty* property_history_length_; + + bool show_connection_; + bool show_transform_rotation_; + bool show_velocity_linear_; + bool show_velocity_angular_; + bool show_acceleration_linear_; + bool show_acceleration_angular_; + + float size_transform_rotation_; + float diameter_arrows_; + float scale_velocity_linear_; + float scale_velocity_angular_; + float scale_acceleration_linear_; + float scale_acceleration_angular_; + + float alpha_; + Ogre::ColourValue color_connection_; + Ogre::ColourValue color_velocity_linear_; + Ogre::ColourValue color_velocity_angular_; + Ogre::ColourValue color_acceleration_linear_; + Ogre::ColourValue color_acceleration_angular_; + + float font_size_; + bool show_text_; + + trajectory_msgs::MultiDOFJointTrajectory::ConstPtr current_trajectory_; +}; + +} // namespace multi_dof_joint_trajectory_rviz_plugins diff --git a/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp new file mode 100644 index 00000000..726adb21 --- /dev/null +++ b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace multi_dof_joint_trajectory_rviz_plugins { + +class MultiDOFJointTrajectoryPointConnectionVisual { + public: + MultiDOFJointTrajectoryPointConnectionVisual( + Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const trajectory_msgs::MultiDOFJointTrajectoryPoint& from, + const trajectory_msgs::MultiDOFJointTrajectoryPoint& to, + float show_connection, const Ogre::ColourValue& color); + virtual ~MultiDOFJointTrajectoryPointConnectionVisual(); + + void setShowConnection(bool visible); + void setColor(const Ogre::ColourValue& color); + + private: + void updateShowConnection(); + void updateColor(); + + Ogre::SceneManager* scene_manager_; + + Ogre::SceneNode* scene_node_; + + std::vector> lines_; + + float show_connection_; + Ogre::ColourValue color_; +}; + +} // namespace multi_dof_joint_trajectory_rviz_plugins diff --git a/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp new file mode 100644 index 00000000..7c9cdb60 --- /dev/null +++ b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp @@ -0,0 +1,120 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace multi_dof_joint_trajectory_rviz_plugins { + +class MultiDOFJointTrajectoryPointVisual { + public: + MultiDOFJointTrajectoryPointVisual( + Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const trajectory_msgs::MultiDOFJointTrajectoryPoint& msg, + bool show_transform_rotation, bool show_velocity_linear, + bool show_velocity_angular, bool show_acceleration_linear, + bool show_acceleration_angular, float size_transform_rotation, + float diameter_arrows, float scale_velocity_linear, + float scale_velocity_angular, float scale_acceleration_linear, + float scale_acceleration_angular, float alpha_transform_rotatation, + const Ogre::ColourValue& color_velocity_linear, + const Ogre::ColourValue& color_velocity_angular, + const Ogre::ColourValue& color_acceleration_linear, + const Ogre::ColourValue& color_acceleration_angular, + const std::vector& captions, float font_size, + bool show_text); + virtual ~MultiDOFJointTrajectoryPointVisual(); + + void setShowTransformRotation(bool visible); + void setShowVelocityLinear(bool visible); + void setShowVelocityAngular(bool visible); + void setShowAccelerationLinear(bool visible); + void setShowAccelerationAngular(bool visible); + + void setSizeTransformRotation(float size); + void setDiametersArrows(float diameter); + void setScaleVelocityLinear(float scale); + void setScaleVelocityAngular(float scale); + void setScaleAccelerationLinear(float scale); + void setScaleAccelerationAngular(float scale); + + void setAlphaTransformRotation(float alpha); + void setColorVelocityLinear(const Ogre::ColourValue& color); + void setColorVelocityAngular(const Ogre::ColourValue& color); + void setColorAccelerationLinear(const Ogre::ColourValue& color); + void setColorAccelerationAngular(const Ogre::ColourValue& color); + + void setCaptions(const std::vector& captions); + void setFontSize(float font_size); + void setShowText(bool show_text); + + private: + void updateSizeTransformRotation(); + void updateDiametersArrows(); + void updateScaleVelocityLinear(); + void updateScaleVelocityAngular(); + void updateScaleAccelerationLinear(); + void updateScaleAccelerationAngular(); + + void updateAlphaTransformRotation(); + void updateColorVelocityLinear(); + void updateColorVelocityAngular(); + void updateColorAccelerationLinear(); + void updateColorAccelerationAngular(); + + void updateCaptions(); + void updateFontSize(); + void updateShowText(); + + Ogre::ColourValue getColor(const Ogre::ColourValue& color, bool visible); + float getCharacterHeight(); + + Ogre::SceneManager* scene_manager_; + + std::vector transforms_position_; + std::vector> transforms_rotation_; + std::vector> velocities_linear_; + std::vector> velocities_angular_; + std::vector> accelerations_linear_; + std::vector> accelerations_angular_; + std::vector> texts_; + + std::vector velocities_linear_absolute_; + std::vector velocities_angular_absolute_; + std::vector accelerations_linear_absolute_; + std::vector accelerations_angular_absolute_; + + bool show_transform_rotation_; + bool show_velocity_linear_; + bool show_velocity_angular_; + bool show_acceleration_linear_; + bool show_acceleration_angular_; + + const float axis_radius_per_size_; + + float size_transform_rotation_; + float diameter_arrows_; + float scale_velocity_linear_; + float scale_velocity_angular_; + float scale_acceleration_linear_; + float scale_acceleration_angular_; + + float alpha_transform_rotatation_; + Ogre::ColourValue color_velocity_linear_; + Ogre::ColourValue color_velocity_angular_; + Ogre::ColourValue color_acceleration_linear_; + Ogre::ColourValue color_acceleration_angular_; + + std::vector captions_; + float font_size_; + bool show_text_; +}; + +} // namespace multi_dof_joint_trajectory_rviz_plugins diff --git a/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/package.xml b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/package.xml new file mode 100644 index 00000000..15de2035 --- /dev/null +++ b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/package.xml @@ -0,0 +1,26 @@ + + + multi_dof_joint_trajectory_rviz_plugins + 0.3.1 + Plugins for visualizing trajectory_msgs/MultiDOFJointTrajectory in rviz. + Remo Diethelm + Samuel Bachmann + BSD + https://github.com/anybotics/kindr_ros + + none + Remo Diethelm + + catkin + + rviz + trajectory_msgs + qtbase5-dev + + gtest + + + + + + diff --git a/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/plugin_description.xml b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/plugin_description.xml new file mode 100644 index 00000000..2c70a7e3 --- /dev/null +++ b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + A class for displaying the trajectory_msgs/MultiDOFJointTrajectory message. + + + diff --git a/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp new file mode 100644 index 00000000..4718bd28 --- /dev/null +++ b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp @@ -0,0 +1,556 @@ +#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp" + +#include + +namespace multi_dof_joint_trajectory_rviz_plugins { + +MultiDOFJointTrajectoryDisplay::MultiDOFJointTrajectoryDisplay() + : show_connection_(true), + show_transform_rotation_(true), + show_velocity_linear_(true), + show_velocity_angular_(true), + show_acceleration_linear_(true), + show_acceleration_angular_(true), + size_transform_rotation_(0.2), + diameter_arrows_(0.05), + scale_velocity_linear_(1.0), + scale_velocity_angular_(1.0), + scale_acceleration_linear_(1.0), + scale_acceleration_angular_(1.0), + alpha_(1.0), + color_connection_(Ogre::ColourValue(1.0, 1.0, 1.0, alpha_)), // white + color_velocity_linear_( + Ogre::ColourValue(0.4, 0.0, 0.0, alpha_)), // dark red + color_velocity_angular_( + Ogre::ColourValue(0.0, 0.4, 0.0, alpha_)), // dark green + color_acceleration_linear_( + Ogre::ColourValue(1.0, 1.0, 0.0, alpha_)), // yellow + color_acceleration_angular_( + Ogre::ColourValue(0.75, 0.0, 0.75, alpha_)), // purple + font_size_(0.05), + show_text_(true) { + connect(this, SIGNAL(updateTrajectorySignal()), this, + SLOT(updateTrajectory())); + + property_show_connection_ = + new rviz::BoolProperty("Show Connections", show_connection_, + "Enable or disable connections rendering.", this, + SLOT(setShowConnection())); + + property_show_transform_rotation_ = new rviz::BoolProperty( + "Show Transform Rotation", show_transform_rotation_, + "Enable or disable rotation transforms rendering.", this, + SLOT(setShowTransformRotation())); + + property_show_velocity_linear_ = + new rviz::BoolProperty("Show Velocity Linear", show_velocity_linear_, + "Enable or disable linear velocities rendering.", + this, SLOT(setShowVelocityLinear())); + + property_show_velocity_angular_ = + new rviz::BoolProperty("Show Velocity Angular", show_velocity_angular_, + "Enable or disable angular velocities rendering.", + this, SLOT(setShowVelocityAngular())); + + property_show_acceleration_linear_ = new rviz::BoolProperty( + "Show Acceleration Linear", show_acceleration_linear_, + "Enable or disable linear accelerations rendering.", this, + SLOT(setShowAccelerationLinear())); + + property_show_acceleration_angular_ = new rviz::BoolProperty( + "Show Acceleration Angular", show_acceleration_angular_, + "Enable or disable angular accelerations rendering.", this, + SLOT(setShowAccelerationAngular())); + + property_size_transform_rotation_ = new rviz::FloatProperty( + "Size Transform Rotation", size_transform_rotation_, + "Size of the axes of the rotation transforms.", this, + SLOT(setSizeTransformRotation())); + property_size_transform_rotation_->setMin(0); + + property_diameter_arrows_ = new rviz::FloatProperty( + "Diameter Arrows", diameter_arrows_, "Diameter of the arrows.", this, + SLOT(setDiameterArrows())); + property_diameter_arrows_->setMin(0); + + property_scale_velocity_linear_ = new rviz::FloatProperty( + "Scale Velocity Linear", scale_velocity_linear_, + "Scale of the linear velocities.", this, SLOT(setScaleVelocityLinear())); + + property_scale_velocity_angular_ = + new rviz::FloatProperty("Scale Velocity Angular", scale_velocity_angular_, + "Scale of the angular velocities.", this, + SLOT(setScaleVelocityAngular())); + + property_scale_acceleration_linear_ = new rviz::FloatProperty( + "Scale Acceleration Linear", scale_acceleration_linear_, + "Scale of the linear accelerations.", this, + SLOT(setScaleAccelerationLinear())); + + property_scale_acceleration_angular_ = new rviz::FloatProperty( + "Scale Acceleration Angular", scale_acceleration_angular_, + "Scale of the angular accelerations.", this, + SLOT(setScaleAccelerationAngular())); + + property_color_connection_ = new rviz::ColorProperty( + "Color Connection", rviz::ogreToQt(color_connection_), + "Color of connection lines.", this, SLOT(setColorConnection())); + + property_color_velocity_linear_ = new rviz::ColorProperty( + "Color Velocity Linear", rviz::ogreToQt(color_velocity_linear_), + "Color of the linear velocities.", this, SLOT(setColorVelocityLinear())); + + property_color_velocity_angular_ = new rviz::ColorProperty( + "Color Velocity Angular", rviz::ogreToQt(color_velocity_angular_), + "Color of the angular velocities.", this, + SLOT(setColorVelocityAngular())); + + property_color_acceleration_linear_ = new rviz::ColorProperty( + "Color Acceleration Linear", rviz::ogreToQt(color_acceleration_linear_), + "Color of the linear accelerations.", this, + SLOT(setColorAccelerationLinear())); + + property_color_acceleration_angular_ = new rviz::ColorProperty( + "Color Acceleration Angular", rviz::ogreToQt(color_acceleration_angular_), + "Color of the angular accelerations.", this, + SLOT(setColorAccelerationAngular())); + + property_alpha_ = new rviz::FloatProperty( + "Alpha", alpha_, "0 is fully transparent, 1.0 is fully opaque.", this, + SLOT(setAlpha())); + property_alpha_->setMin(0); + property_alpha_->setMax(1); + + property_font_size_ = new rviz::FloatProperty( + "Font Size", font_size_, "Size of the font.", this, SLOT(setFontSize())); + property_font_size_->setMin(0); + + property_show_text_ = new rviz::BoolProperty( + "Show Caption", show_text_, "Enable or disable text rendering.", this, + SLOT(setShowText())); + + property_history_length_ = new rviz::IntProperty( + "History Length", 1, "Number of prior measurements to display.", this, + SLOT(setHistoryLength())); + property_history_length_->setMin(1); + property_history_length_->setMax(100000); +} + +MultiDOFJointTrajectoryDisplay::~MultiDOFJointTrajectoryDisplay() {} + +void MultiDOFJointTrajectoryDisplay::onInitialize() { + MFDClass::onInitialize(); + setHistoryLength(); +} + +void MultiDOFJointTrajectoryDisplay::reset() { + MFDClass::reset(); + visuals_points_.clear(); + visuals_connections_.clear(); +} + +void MultiDOFJointTrajectoryDisplay::setShowConnection() { + show_connection_ = property_show_connection_->getBool(); + updateShowConnection(); +} + +void MultiDOFJointTrajectoryDisplay::setShowTransformRotation() { + show_transform_rotation_ = property_show_transform_rotation_->getBool(); + updateShowTransformRotation(); +} + +void MultiDOFJointTrajectoryDisplay::setShowVelocityLinear() { + show_velocity_linear_ = property_show_velocity_linear_->getBool(); + updateShowVelocityLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setShowVelocityAngular() { + show_velocity_angular_ = property_show_velocity_angular_->getBool(); + updateShowVelocityAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setShowAccelerationLinear() { + show_acceleration_linear_ = property_show_acceleration_linear_->getBool(); + updateShowAccelerationLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setShowAccelerationAngular() { + show_acceleration_angular_ = property_show_acceleration_angular_->getBool(); + updateShowAccelerationAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setSizeTransformRotation() { + size_transform_rotation_ = property_size_transform_rotation_->getFloat(); + updateSizeTransformRotation(); +} + +void MultiDOFJointTrajectoryDisplay::setDiameterArrows() { + diameter_arrows_ = property_diameter_arrows_->getFloat(); + updateDiameterArrows(); +} + +void MultiDOFJointTrajectoryDisplay::setScaleVelocityLinear() { + scale_velocity_linear_ = property_scale_velocity_linear_->getFloat(); + updateScaleVelocityLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setScaleVelocityAngular() { + scale_velocity_angular_ = property_scale_velocity_angular_->getFloat(); + updateScaleVelocityAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setScaleAccelerationLinear() { + scale_acceleration_linear_ = property_scale_acceleration_linear_->getFloat(); + updateScaleAccelerationLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setScaleAccelerationAngular() { + scale_acceleration_angular_ = + property_scale_acceleration_angular_->getFloat(); + updateScaleAccelerationAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setAlpha() { + alpha_ = property_alpha_->getFloat(); + color_connection_.a = alpha_; + color_velocity_linear_.a = alpha_; + color_velocity_angular_.a = alpha_; + color_acceleration_linear_.a = alpha_; + color_acceleration_angular_.a = alpha_; + updateColorConnection(); + updateAlphaTransformRotation(); + updateColorVelocityLinear(); + updateColorVelocityAngular(); + updateColorAccelerationLinear(); + updateColorAccelerationAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setColorConnection() { + color_connection_ = rviz::qtToOgre(property_color_connection_->getColor()); + color_connection_.a = property_alpha_->getFloat(); + updateColorConnection(); +} + +void MultiDOFJointTrajectoryDisplay::setColorVelocityLinear() { + color_velocity_linear_ = + rviz::qtToOgre(property_color_velocity_linear_->getColor()); + color_velocity_linear_.a = property_alpha_->getFloat(); + updateColorVelocityLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setColorVelocityAngular() { + color_velocity_angular_ = + rviz::qtToOgre(property_color_velocity_angular_->getColor()); + color_velocity_angular_.a = property_alpha_->getFloat(); + updateColorVelocityAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setColorAccelerationLinear() { + color_acceleration_linear_ = + rviz::qtToOgre(property_color_acceleration_linear_->getColor()); + color_acceleration_linear_.a = property_alpha_->getFloat(); + updateColorAccelerationLinear(); +} + +void MultiDOFJointTrajectoryDisplay::setColorAccelerationAngular() { + color_acceleration_angular_ = + rviz::qtToOgre(property_color_acceleration_angular_->getColor()); + color_acceleration_angular_.a = property_alpha_->getFloat(); + updateColorAccelerationAngular(); +} + +void MultiDOFJointTrajectoryDisplay::setFontSize() { + font_size_ = property_font_size_->getFloat(); + updateFontSize(); +} + +void MultiDOFJointTrajectoryDisplay::setShowText() { + show_text_ = property_show_text_->getBool(); + updateShowText(); +} + +void MultiDOFJointTrajectoryDisplay::setHistoryLength() { + visuals_points_.rset_capacity(property_history_length_->getInt()); + visuals_connections_.rset_capacity(property_history_length_->getInt()); +} + +void MultiDOFJointTrajectoryDisplay::updateTrajectory() { + show_connection_ = property_show_connection_->getBool(); + show_transform_rotation_ = property_show_transform_rotation_->getBool(); + show_velocity_linear_ = property_show_velocity_linear_->getBool(); + show_velocity_angular_ = property_show_velocity_angular_->getBool(); + show_acceleration_linear_ = property_show_acceleration_linear_->getBool(); + show_acceleration_angular_ = property_show_acceleration_angular_->getBool(); + + size_transform_rotation_ = property_size_transform_rotation_->getFloat(); + scale_velocity_linear_ = property_scale_velocity_linear_->getFloat(); + scale_velocity_angular_ = property_scale_velocity_angular_->getFloat(); + scale_acceleration_linear_ = property_scale_acceleration_linear_->getFloat(); + scale_acceleration_angular_ = + property_scale_acceleration_angular_->getFloat(); + + color_connection_ = rviz::qtToOgre(property_color_connection_->getColor()); + color_velocity_linear_ = + rviz::qtToOgre(property_color_velocity_linear_->getColor()); + color_velocity_angular_ = + rviz::qtToOgre(property_color_velocity_angular_->getColor()); + color_acceleration_linear_ = + rviz::qtToOgre(property_color_acceleration_linear_->getColor()); + color_acceleration_angular_ = + rviz::qtToOgre(property_color_acceleration_angular_->getColor()); + alpha_ = property_alpha_->getFloat(); + color_connection_.a = alpha_; + color_velocity_linear_.a = alpha_; + color_velocity_angular_.a = alpha_; + color_acceleration_linear_.a = alpha_; + color_acceleration_angular_.a = alpha_; + + std::vector> captions; + for (unsigned int i = 0; i < current_trajectory_->points.size(); i++) { + std::vector caption_point; + for (unsigned int j = 0; j < current_trajectory_->joint_names.size(); j++) { + std::stringstream ss; + ss << current_trajectory_->joint_names[j] << ": t" << i << " = " + << current_trajectory_->points[i].time_from_start.toSec() << "s"; + caption_point.push_back(ss.str()); + } + captions.push_back(caption_point); + } + + font_size_ = property_font_size_->getFloat(); + show_text_ = property_show_text_->getBool(); + + std::vector> + visuals_points; + std::vector> + visuals_connections; + + trajectory_msgs::MultiDOFJointTrajectoryPoint last_point; + trajectory_msgs::MultiDOFJointTrajectoryPoint current_point = + current_trajectory_->points[0]; + + // add first point + visuals_points.push_back( + boost::shared_ptr( + new MultiDOFJointTrajectoryPointVisual( + context_->getSceneManager(), scene_node_, current_point, + show_transform_rotation_, show_velocity_linear_, + show_velocity_angular_, show_acceleration_linear_, + show_acceleration_angular_, size_transform_rotation_, + diameter_arrows_, scale_velocity_linear_, scale_velocity_angular_, + scale_acceleration_linear_, scale_acceleration_angular_, alpha_, + color_velocity_linear_, color_velocity_angular_, + color_acceleration_linear_, color_acceleration_angular_, + captions[0], font_size_, show_text_))); + + // add second to last points and connections to predecessors + for (unsigned int i = 1; i < current_trajectory_->points.size(); i++) { + // go one pose further + last_point = current_point; + current_point = current_trajectory_->points[i]; + + // add edge to predecessor + visuals_connections.push_back( + boost::shared_ptr( + new MultiDOFJointTrajectoryPointConnectionVisual( + context_->getSceneManager(), scene_node_, last_point, + current_point, show_connection_, color_connection_))); + + // add pose + visuals_points.push_back( + boost::shared_ptr( + new MultiDOFJointTrajectoryPointVisual( + context_->getSceneManager(), scene_node_, current_point, + show_transform_rotation_, show_velocity_linear_, + show_velocity_angular_, show_acceleration_linear_, + show_acceleration_angular_, size_transform_rotation_, + diameter_arrows_, scale_velocity_linear_, + scale_velocity_angular_, scale_acceleration_linear_, + scale_acceleration_angular_, alpha_, color_velocity_linear_, + color_velocity_angular_, color_acceleration_linear_, + color_acceleration_angular_, captions[i], font_size_, + show_text_))); + } + + visuals_points_.push_back(visuals_points); + visuals_connections_.push_back(visuals_connections); +} + +void MultiDOFJointTrajectoryDisplay::processMessage( + const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr& msg) { + current_trajectory_ = msg; + Q_EMIT updateTrajectorySignal(); +} + +void MultiDOFJointTrajectoryDisplay::updateShowConnection() { + for (size_t i = 0; i < visuals_connections_.size(); i++) { + for (unsigned int j = 0; j < visuals_connections_[i].size(); j++) { + visuals_connections_[i][j]->setShowConnection(show_connection_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateShowTransformRotation() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setShowTransformRotation(show_transform_rotation_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateShowVelocityLinear() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setShowVelocityLinear(show_velocity_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateShowVelocityAngular() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setShowVelocityAngular(show_velocity_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateShowAccelerationLinear() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setShowAccelerationLinear( + show_acceleration_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateShowAccelerationAngular() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setShowAccelerationAngular( + show_acceleration_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateSizeTransformRotation() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setSizeTransformRotation(size_transform_rotation_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateDiameterArrows() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setDiametersArrows(diameter_arrows_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateScaleVelocityLinear() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setScaleVelocityLinear(scale_velocity_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateScaleVelocityAngular() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setScaleVelocityAngular(scale_velocity_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateScaleAccelerationLinear() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setScaleAccelerationLinear( + scale_acceleration_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateScaleAccelerationAngular() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setScaleAccelerationAngular( + scale_acceleration_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorConnection() { + for (size_t i = 0; i < visuals_connections_.size(); i++) { + for (unsigned int j = 0; j < visuals_connections_[i].size(); j++) { + visuals_connections_[i][j]->setColor(color_connection_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateAlphaTransformRotation() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setAlphaTransformRotation(alpha_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorVelocityLinear() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setColorVelocityLinear(color_velocity_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorVelocityAngular() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setColorVelocityAngular(color_velocity_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorAccelerationLinear() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setColorAccelerationLinear( + color_acceleration_linear_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateColorAccelerationAngular() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setColorAccelerationAngular( + color_acceleration_angular_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateFontSize() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setFontSize(font_size_); + } + } +} + +void MultiDOFJointTrajectoryDisplay::updateShowText() { + for (size_t i = 0; i < visuals_points_.size(); i++) { + for (unsigned int j = 0; j < visuals_points_[i].size(); j++) { + visuals_points_[i][j]->setShowText(show_text_); + } + } +} + +} // namespace multi_dof_joint_trajectory_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS( + multi_dof_joint_trajectory_rviz_plugins::MultiDOFJointTrajectoryDisplay, + rviz::Display) diff --git a/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp new file mode 100644 index 00000000..5bcc07a1 --- /dev/null +++ b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp @@ -0,0 +1,67 @@ +#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp" + +namespace multi_dof_joint_trajectory_rviz_plugins { + +MultiDOFJointTrajectoryPointConnectionVisual:: + MultiDOFJointTrajectoryPointConnectionVisual( + Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const trajectory_msgs::MultiDOFJointTrajectoryPoint& from, + const trajectory_msgs::MultiDOFJointTrajectoryPoint& to, + float show_connection, const Ogre::ColourValue& color) + : scene_manager_(scene_manager), + show_connection_(show_connection), + color_(color) { + // check vector lengths + assert(from.transforms.size() == to.transforms.size() && + "ERROR: MultiDOFJointTrajectoryPointConnectionVisual: from and to " + "vectors do not have the same length."); + + // scene node + scene_node_ = parent_node->createChildSceneNode(); + + // lines + for (unsigned int i = 0; i < from.transforms.size(); i++) { + lines_.push_back(boost::shared_ptr( + new rviz::Line(scene_manager_, scene_node_))); + const Ogre::Vector3 fromVector(from.transforms[i].translation.x, + from.transforms[i].translation.y, + from.transforms[i].translation.z); + const Ogre::Vector3 toVector(to.transforms[i].translation.x, + to.transforms[i].translation.y, + to.transforms[i].translation.z); + lines_.back()->setPoints(fromVector, toVector); + updateColor(); + lines_.back()->setVisible(show_connection_); + } +} + +MultiDOFJointTrajectoryPointConnectionVisual:: + ~MultiDOFJointTrajectoryPointConnectionVisual() { + scene_manager_->destroySceneNode(scene_node_); +} + +void MultiDOFJointTrajectoryPointConnectionVisual::setShowConnection( + bool visible) { + show_connection_ = visible; + updateShowConnection(); +} + +void MultiDOFJointTrajectoryPointConnectionVisual::setColor( + const Ogre::ColourValue& color) { + color_ = color; + updateColor(); +} + +void MultiDOFJointTrajectoryPointConnectionVisual::updateShowConnection() { + for (unsigned int i = 0; i < lines_.size(); i++) { + lines_[i]->setVisible(show_connection_); + } +} + +void MultiDOFJointTrajectoryPointConnectionVisual::updateColor() { + for (unsigned int i = 0; i < lines_.size(); i++) { + lines_[i]->setColor(color_); + } +} + +} // namespace multi_dof_joint_trajectory_rviz_plugins diff --git a/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp new file mode 100644 index 00000000..b178dcf8 --- /dev/null +++ b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp @@ -0,0 +1,416 @@ +#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp" + +#include + +namespace multi_dof_joint_trajectory_rviz_plugins { + +MultiDOFJointTrajectoryPointVisual::MultiDOFJointTrajectoryPointVisual( + Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const trajectory_msgs::MultiDOFJointTrajectoryPoint& msg, + bool show_transform_rotation, bool show_velocity_linear, + bool show_velocity_angular, bool show_acceleration_linear, + bool show_acceleration_angular, float size_transform_rotation, + float diameter_arrows, float scale_velocity_linear, + float scale_velocity_angular, float scale_acceleration_linear, + float scale_acceleration_angular, float alpha_transform_rotatation, + const Ogre::ColourValue& color_velocity_linear, + const Ogre::ColourValue& color_velocity_angular, + const Ogre::ColourValue& color_acceleration_linear, + const Ogre::ColourValue& color_acceleration_angular, + const std::vector& captions, float font_size, bool show_text) + : scene_manager_(scene_manager), + axis_radius_per_size_(0.1), + show_transform_rotation_(show_transform_rotation), + show_velocity_linear_(show_velocity_linear), + show_velocity_angular_(show_velocity_angular), + show_acceleration_linear_(show_acceleration_linear), + show_acceleration_angular_(show_acceleration_angular), + size_transform_rotation_(size_transform_rotation), + diameter_arrows_(diameter_arrows), + scale_velocity_linear_(scale_velocity_linear), + scale_velocity_angular_(scale_velocity_angular), + scale_acceleration_linear_(scale_acceleration_linear), + scale_acceleration_angular_(scale_acceleration_angular), + alpha_transform_rotatation_(alpha_transform_rotatation), + color_velocity_linear_(color_velocity_linear), + color_velocity_angular_(color_velocity_angular), + color_acceleration_linear_(color_acceleration_linear), + color_acceleration_angular_(color_acceleration_angular), + captions_(captions), + font_size_(font_size), + show_text_(show_text) { + // check vector lengths + assert((msg.transforms.size() == captions.size()) && + "ERROR: MultiDOFJointTrajectoryPointVisual: captions and transforms " + "vectors do not have the same length."); + assert((msg.velocities.size() == captions.size() || + msg.velocities.size() == 0) && + "ERROR: MultiDOFJointTrajectoryPointVisual: velocity vector has " + "invalid length."); + assert((msg.accelerations.size() == captions.size() || + msg.accelerations.size() == 0) && + "ERROR: MultiDOFJointTrajectoryPointVisual: acceleration vector has " + "invalid length."); + + const bool message_contains_velocities = + (msg.velocities.size() == msg.transforms.size()); + const bool message_contains_accelerations = + (msg.accelerations.size() == msg.transforms.size()); + + // loop through all joints + for (unsigned int i = 0; i < msg.transforms.size(); i++) { + // transform position + transforms_position_.push_back(parent_node->createChildSceneNode()); + transforms_position_.back()->setPosition(msg.transforms[i].translation.x, + msg.transforms[i].translation.y, + msg.transforms[i].translation.z); + + // transform rotation + const Ogre::Quaternion orientation( + msg.transforms[i].rotation.w, msg.transforms[i].rotation.x, + msg.transforms[i].rotation.y, msg.transforms[i].rotation.z); + transforms_rotation_.push_back(boost::shared_ptr(new rviz::Axes( + scene_manager_, transforms_position_.back(), size_transform_rotation, + axis_radius_per_size_ * size_transform_rotation))); + transforms_rotation_.back()->setOrientation(orientation); + + // define common variables + Ogre::Vector3 vector; + + if (message_contains_velocities) { + // velocity linear + vector = + Ogre::Vector3(msg.velocities[i].linear.x, msg.velocities[i].linear.y, + msg.velocities[i].linear.z); + velocities_linear_absolute_.push_back(vector.length()); + velocities_linear_.push_back( + boost::shared_ptr(new rviz::Arrow( + scene_manager, transforms_position_.back(), + 0.8 * scale_velocity_linear_ * velocities_linear_absolute_.back(), + diameter_arrows_, + 0.2 * scale_velocity_linear_ * velocities_linear_absolute_.back(), + 2.0 * diameter_arrows_))); + velocities_linear_.back()->setDirection(vector); + + // velocity angular + vector = Ogre::Vector3(msg.velocities[i].angular.x, + msg.velocities[i].angular.y, + msg.velocities[i].angular.z); + velocities_angular_absolute_.push_back(vector.length()); + velocities_angular_.push_back(boost::shared_ptr( + new rviz::Arrow(scene_manager, transforms_position_.back(), + 0.8 * scale_velocity_angular_ * + velocities_angular_absolute_.back(), + diameter_arrows_, + 0.2 * scale_velocity_angular_ * + velocities_angular_absolute_.back(), + 2.0 * diameter_arrows_))); + velocities_angular_.back()->setDirection(vector); + } + + if (message_contains_accelerations) { + // acceleration linear + vector = Ogre::Vector3(msg.accelerations[i].linear.x, + msg.accelerations[i].linear.y, + msg.accelerations[i].linear.z); + accelerations_linear_absolute_.push_back(vector.length()); + accelerations_linear_.push_back(boost::shared_ptr( + new rviz::Arrow(scene_manager, transforms_position_.back(), + 0.8 * scale_acceleration_linear_ * + accelerations_linear_absolute_.back(), + diameter_arrows_, + 0.2 * scale_acceleration_linear_ * + accelerations_linear_absolute_.back(), + 2.0 * diameter_arrows_))); + accelerations_linear_.back()->setDirection(vector); + + // acceleration angular + vector = Ogre::Vector3(msg.accelerations[i].angular.x, + msg.accelerations[i].angular.y, + msg.accelerations[i].angular.z); + accelerations_angular_absolute_.push_back(vector.length()); + accelerations_angular_.push_back(boost::shared_ptr( + new rviz::Arrow(scene_manager, transforms_position_.back(), + 0.8 * scale_acceleration_angular_ * + accelerations_angular_absolute_.back(), + diameter_arrows_, + 0.2 * scale_acceleration_angular_ * + accelerations_angular_absolute_.back(), + 2.0 * diameter_arrows_))); + accelerations_angular_.back()->setDirection(vector); + } + + // text + auto text = boost::shared_ptr( + new rviz::MovableText(captions_[i])); + text->setCharacterHeight(getCharacterHeight()); + texts_.push_back(text); + texts_.back()->setTextAlignment(rviz::MovableText::H_CENTER, + rviz::MovableText::V_BELOW); + transforms_position_.back()->attachObject(texts_.back().get()); + } + + // update all colors + updateAlphaTransformRotation(); + updateColorVelocityLinear(); + updateColorVelocityAngular(); + updateColorAccelerationLinear(); + updateColorAccelerationAngular(); +} + +MultiDOFJointTrajectoryPointVisual::~MultiDOFJointTrajectoryPointVisual() { + for (unsigned int i = 0; i < transforms_position_.size(); i++) { + scene_manager_->destroySceneNode(transforms_position_[i]); + } +} + +void MultiDOFJointTrajectoryPointVisual::setShowTransformRotation( + bool visible) { + show_transform_rotation_ = visible; + updateAlphaTransformRotation(); +} + +void MultiDOFJointTrajectoryPointVisual::setShowVelocityLinear(bool visible) { + show_velocity_linear_ = visible; + updateColorVelocityLinear(); +} + +void MultiDOFJointTrajectoryPointVisual::setShowVelocityAngular(bool visible) { + show_velocity_angular_ = visible; + updateColorVelocityAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::setShowAccelerationLinear( + bool visible) { + show_acceleration_linear_ = visible; + updateColorAccelerationLinear(); +} + +void MultiDOFJointTrajectoryPointVisual::setShowAccelerationAngular( + bool visible) { + show_acceleration_angular_ = visible; + updateColorAccelerationAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::setSizeTransformRotation(float size) { + size_transform_rotation_ = size; + updateSizeTransformRotation(); +} + +void MultiDOFJointTrajectoryPointVisual::setDiametersArrows(float diameter) { + diameter_arrows_ = diameter; + updateDiametersArrows(); +} + +void MultiDOFJointTrajectoryPointVisual::setScaleVelocityLinear(float scale) { + scale_velocity_linear_ = scale; + updateScaleVelocityLinear(); +} + +void MultiDOFJointTrajectoryPointVisual::setScaleVelocityAngular(float scale) { + scale_velocity_angular_ = scale; + updateScaleVelocityAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::setScaleAccelerationLinear( + float scale) { + scale_acceleration_linear_ = scale; + updateScaleAccelerationLinear(); +} + +void MultiDOFJointTrajectoryPointVisual::setScaleAccelerationAngular( + float scale) { + scale_acceleration_angular_ = scale; + updateScaleAccelerationAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::setAlphaTransformRotation( + float alpha) { + alpha_transform_rotatation_ = alpha; + updateAlphaTransformRotation(); +} + +void MultiDOFJointTrajectoryPointVisual::setColorVelocityLinear( + const Ogre::ColourValue& color) { + color_velocity_linear_ = color; + updateColorVelocityLinear(); +} + +void MultiDOFJointTrajectoryPointVisual::setColorVelocityAngular( + const Ogre::ColourValue& color) { + color_velocity_angular_ = color; + updateColorVelocityAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::setColorAccelerationLinear( + const Ogre::ColourValue& color) { + color_acceleration_linear_ = color; + updateColorAccelerationLinear(); +} + +void MultiDOFJointTrajectoryPointVisual::setColorAccelerationAngular( + const Ogre::ColourValue& color) { + color_acceleration_angular_ = color; + updateColorAccelerationAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::setCaptions( + const std::vector& captions) { + // check vector lengths + assert(captions_.size() == captions.size() && + "ERROR: MultiDOFJointTrajectoryPointVisual: old and new captions " + "vectors do not have the same length."); + + captions_ = captions; + updateCaptions(); +} + +void MultiDOFJointTrajectoryPointVisual::setFontSize(float font_size) { + font_size_ = font_size; + updateFontSize(); +} + +void MultiDOFJointTrajectoryPointVisual::setShowText(bool show_text) { + show_text_ = show_text; + updateShowText(); +} + +void MultiDOFJointTrajectoryPointVisual::updateSizeTransformRotation() { + for (unsigned int i = 0; i < transforms_rotation_.size(); i++) { + transforms_rotation_[i]->set( + size_transform_rotation_, + axis_radius_per_size_ * size_transform_rotation_); + } + updateAlphaTransformRotation(); // colors are reset by set() +} + +void MultiDOFJointTrajectoryPointVisual::updateDiametersArrows() { + updateScaleVelocityLinear(); + updateScaleVelocityAngular(); + updateScaleAccelerationLinear(); + updateScaleAccelerationAngular(); +} + +void MultiDOFJointTrajectoryPointVisual::updateScaleVelocityLinear() { + for (unsigned int i = 0; i < velocities_linear_.size(); i++) { + velocities_linear_[i]->set( + 0.8 * scale_velocity_linear_ * velocities_linear_absolute_[i], + diameter_arrows_, + 0.2 * scale_velocity_linear_ * velocities_linear_absolute_[i], + 2.0 * diameter_arrows_); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateScaleVelocityAngular() { + for (unsigned int i = 0; i < velocities_angular_.size(); i++) { + velocities_angular_[i]->set( + 0.8 * scale_velocity_angular_ * velocities_angular_absolute_[i], + diameter_arrows_, + 0.2 * scale_velocity_angular_ * velocities_angular_absolute_[i], + 2.0 * diameter_arrows_); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateScaleAccelerationLinear() { + for (unsigned int i = 0; i < accelerations_linear_.size(); i++) { + accelerations_linear_[i]->set( + 0.8 * scale_acceleration_linear_ * accelerations_linear_absolute_[i], + diameter_arrows_, + 0.2 * scale_acceleration_linear_ * accelerations_linear_absolute_[i], + 2.0 * diameter_arrows_); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateScaleAccelerationAngular() { + for (unsigned int i = 0; i < accelerations_angular_.size(); i++) { + accelerations_angular_[i]->set( + 0.8 * scale_acceleration_angular_ * accelerations_angular_absolute_[i], + diameter_arrows_, + 0.2 * scale_acceleration_angular_ * accelerations_angular_absolute_[i], + 2.0 * diameter_arrows_); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateAlphaTransformRotation() { + Ogre::ColourValue color; + for (unsigned int i = 0; i < transforms_rotation_.size(); i++) { + color = transforms_rotation_[i]->getDefaultXColor(); + color.a = show_transform_rotation_ * alpha_transform_rotatation_; + transforms_rotation_[i]->setXColor(color); + color = transforms_rotation_[i]->getDefaultYColor(); + color.a = show_transform_rotation_ * alpha_transform_rotatation_; + transforms_rotation_[i]->setYColor(color); + color = transforms_rotation_[i]->getDefaultZColor(); + color.a = show_transform_rotation_ * alpha_transform_rotatation_; + transforms_rotation_[i]->setZColor(color); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateColorVelocityLinear() { + for (unsigned int i = 0; i < velocities_linear_.size(); i++) { + velocities_linear_[i]->setColor( + getColor(color_velocity_linear_, show_velocity_linear_)); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateColorVelocityAngular() { + for (unsigned int i = 0; i < velocities_angular_.size(); i++) { + velocities_angular_[i]->setColor( + getColor(color_velocity_angular_, show_velocity_angular_)); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateColorAccelerationLinear() { + for (unsigned int i = 0; i < accelerations_linear_.size(); i++) { + accelerations_linear_[i]->setColor( + getColor(color_acceleration_linear_, show_acceleration_linear_)); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateColorAccelerationAngular() { + for (unsigned int i = 0; i < accelerations_angular_.size(); i++) { + accelerations_angular_[i]->setColor( + getColor(color_acceleration_angular_, show_acceleration_angular_)); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateCaptions() { + for (unsigned int i = 0; i < texts_.size(); i++) { + texts_[i]->setCaption(captions_[i]); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateFontSize() { + double character_height = getCharacterHeight(); + for (unsigned int i = 0; i < texts_.size(); i++) { + texts_[i]->setCharacterHeight(character_height); + } +} + +void MultiDOFJointTrajectoryPointVisual::updateShowText() { + double character_height = getCharacterHeight(); + for (unsigned int i = 0; i < texts_.size(); i++) { + texts_[i]->setCharacterHeight(character_height); + } +} + +Ogre::ColourValue MultiDOFJointTrajectoryPointVisual::getColor( + const Ogre::ColourValue& color, bool visible) { + if (!visible) { + Ogre::ColourValue color_invisible = color; + color_invisible.a = 0; + return color_invisible; + } else { + return color; + } +} + +float MultiDOFJointTrajectoryPointVisual::getCharacterHeight() { + if (show_text_) { + return font_size_; + } else { + return 0; + } +} + +} // namespace multi_dof_joint_trajectory_rviz_plugins diff --git a/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/test/EmptyTests.cpp b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/test/EmptyTests.cpp new file mode 100644 index 00000000..4c702df3 --- /dev/null +++ b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/test/EmptyTests.cpp @@ -0,0 +1,5 @@ +#include + +TEST(Placeholder, emptyTest) { // NOLINT + ASSERT_TRUE(true); +} diff --git a/src/localization/config/driver_params.yaml b/src/Nav/localization/config/driver_params.yaml similarity index 100% rename from src/localization/config/driver_params.yaml rename to src/Nav/localization/config/driver_params.yaml diff --git a/src/localization/config/ekf/gps_ekf_global.yaml b/src/Nav/localization/config/ekf/gps_ekf_global.yaml similarity index 100% rename from src/localization/config/ekf/gps_ekf_global.yaml rename to src/Nav/localization/config/ekf/gps_ekf_global.yaml diff --git a/src/localization/config/ekf/gps_ekf_local.yaml b/src/Nav/localization/config/ekf/gps_ekf_local.yaml similarity index 100% rename from src/localization/config/ekf/gps_ekf_local.yaml rename to src/Nav/localization/config/ekf/gps_ekf_local.yaml diff --git a/src/localization/config/navsat.yaml b/src/Nav/localization/config/navsat.yaml similarity index 100% rename from src/localization/config/navsat.yaml rename to src/Nav/localization/config/navsat.yaml diff --git a/src/localization/config/rtabmap.yaml b/src/Nav/localization/config/rtabmap.yaml similarity index 100% rename from src/localization/config/rtabmap.yaml rename to src/Nav/localization/config/rtabmap.yaml diff --git a/src/localization/config/rviz.views/basicNavMap.rviz b/src/Nav/localization/config/rviz.views/basicNavMap.rviz similarity index 100% rename from src/localization/config/rviz.views/basicNavMap.rviz rename to src/Nav/localization/config/rviz.views/basicNavMap.rviz diff --git a/src/localization/config/slam.yaml b/src/Nav/localization/config/slam.yaml similarity index 100% rename from src/localization/config/slam.yaml rename to src/Nav/localization/config/slam.yaml diff --git a/src/localization/config/urdf/spike.urdf.xml b/src/Nav/localization/config/urdf/spike.urdf.xml similarity index 100% rename from src/localization/config/urdf/spike.urdf.xml rename to src/Nav/localization/config/urdf/spike.urdf.xml diff --git a/src/localization/launch/description.launch.py b/src/Nav/localization/launch/description.launch.py similarity index 100% rename from src/localization/launch/description.launch.py rename to src/Nav/localization/launch/description.launch.py diff --git a/src/localization/launch/ekf.launch.py b/src/Nav/localization/launch/ekf.launch.py similarity index 100% rename from src/localization/launch/ekf.launch.py rename to src/Nav/localization/launch/ekf.launch.py diff --git a/src/localization/launch/localization.launch.py b/src/Nav/localization/launch/localization.launch.py similarity index 100% rename from src/localization/launch/localization.launch.py rename to src/Nav/localization/launch/localization.launch.py diff --git a/src/localization/launch/gps.launch.py b/src/Nav/localization/launch/navsat.launch.py similarity index 51% rename from src/localization/launch/gps.launch.py rename to src/Nav/localization/launch/navsat.launch.py index 6412e989..12ec2fb0 100644 --- a/src/localization/launch/gps.launch.py +++ b/src/Nav/localization/launch/navsat.launch.py @@ -12,42 +12,33 @@ def generate_launch_description(): - pkg_gps = get_package_share_directory("gps") - config_dir = os.path.join(get_package_share_directory("localization"), "config") params_file = os.path.join(config_dir, "navsat.yaml") - ublox_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_gps, "launch", "rover.launch.py") - ) + navsat_remappings = [ + ("imu", "gps/heading"), + ("gps/fix", "gps/fix"), + ("odometry/filtered", "odometry/filtered/global"), + ("odometry/gps", "gps/odom"), + ] + + navsat_node = launch_ros.actions.Node( + package="robot_localization", + executable="navsat_transform_node", + output="log", + parameters=[params_file], + remappings=navsat_remappings, + arguments=["--ros-args", "--log-level", "Warn"], ) - # navsat_remappings = [ - # ("imu", "gps/heading"), - # ("gps/fix", "gps/fix"), - # ("odometry/filtered", "odometry/filtered/global"), - # ("odometry/gps", "gps/odom"), - # ] - - # navsat_node = launch_ros.actions.Node( - # package="robot_localization", - # executable="navsat_transform_node", - # output="log", - # parameters=[params_file], - # remappings=navsat_remappings, - # arguments=["--ros-args", "--log-level", "Warn"], - # ) - repub_odom_node = launch_ros.actions.Node( package="localization", executable="repub_odom", name="repub_odom" ) return launch.LaunchDescription( [ - # navsat_node, + navsat_node, repub_odom_node, - ublox_cmd, ] ) diff --git a/src/localization/launch/rtabmap.launch.py b/src/Nav/localization/launch/rtabmap.launch.py similarity index 100% rename from src/localization/launch/rtabmap.launch.py rename to src/Nav/localization/launch/rtabmap.launch.py diff --git a/src/nav_commanders/nav_commanders/__init__.py b/src/Nav/localization/localization/__init__.py similarity index 100% rename from src/nav_commanders/nav_commanders/__init__.py rename to src/Nav/localization/localization/__init__.py diff --git a/src/localization/localization/imu_filter.py b/src/Nav/localization/localization/imu_filter.py similarity index 100% rename from src/localization/localization/imu_filter.py rename to src/Nav/localization/localization/imu_filter.py diff --git a/src/localization/localization/lidar_mask_tool.py b/src/Nav/localization/localization/lidar_mask_tool.py similarity index 100% rename from src/localization/localization/lidar_mask_tool.py rename to src/Nav/localization/localization/lidar_mask_tool.py diff --git a/src/localization/localization/republish_odometry.py b/src/Nav/localization/localization/republish_odometry.py similarity index 100% rename from src/localization/localization/republish_odometry.py rename to src/Nav/localization/localization/republish_odometry.py diff --git a/src/localization/package.xml b/src/Nav/localization/package.xml similarity index 100% rename from src/localization/package.xml rename to src/Nav/localization/package.xml diff --git a/src/localization/resource/localization b/src/Nav/localization/resource/localization similarity index 100% rename from src/localization/resource/localization rename to src/Nav/localization/resource/localization diff --git a/src/localization/setup.cfg b/src/Nav/localization/setup.cfg similarity index 100% rename from src/localization/setup.cfg rename to src/Nav/localization/setup.cfg diff --git a/src/localization/setup.py b/src/Nav/localization/setup.py similarity index 100% rename from src/localization/setup.py rename to src/Nav/localization/setup.py diff --git a/src/nav_commanders/known_gps_coords/old/drumheller_traversal_guess.yaml b/src/Nav/nav_commanders/known_gps_coords/old/drumheller_traversal_guess.yaml similarity index 100% rename from src/nav_commanders/known_gps_coords/old/drumheller_traversal_guess.yaml rename to src/Nav/nav_commanders/known_gps_coords/old/drumheller_traversal_guess.yaml diff --git a/src/nav_commanders/known_gps_coords/old/edc_crosswalk_to_res.yaml b/src/Nav/nav_commanders/known_gps_coords/old/edc_crosswalk_to_res.yaml similarity index 100% rename from src/nav_commanders/known_gps_coords/old/edc_crosswalk_to_res.yaml rename to src/Nav/nav_commanders/known_gps_coords/old/edc_crosswalk_to_res.yaml diff --git a/src/nav_commanders/known_gps_coords/old/edcfront_northeast.yaml b/src/Nav/nav_commanders/known_gps_coords/old/edcfront_northeast.yaml similarity index 100% rename from src/nav_commanders/known_gps_coords/old/edcfront_northeast.yaml rename to src/Nav/nav_commanders/known_gps_coords/old/edcfront_northeast.yaml diff --git a/src/nav_commanders/known_gps_coords/old/edcfront_ramp_to_edcparkinglot.yaml b/src/Nav/nav_commanders/known_gps_coords/old/edcfront_ramp_to_edcparkinglot.yaml similarity index 100% rename from src/nav_commanders/known_gps_coords/old/edcfront_ramp_to_edcparkinglot.yaml rename to src/Nav/nav_commanders/known_gps_coords/old/edcfront_ramp_to_edcparkinglot.yaml diff --git a/src/nav_commanders/known_gps_coords/old/edcfront_southwest.yaml b/src/Nav/nav_commanders/known_gps_coords/old/edcfront_southwest.yaml similarity index 100% rename from src/nav_commanders/known_gps_coords/old/edcfront_southwest.yaml rename to src/Nav/nav_commanders/known_gps_coords/old/edcfront_southwest.yaml diff --git a/src/nav_commanders/known_gps_coords/old/edcfront_trees_grass.yaml b/src/Nav/nav_commanders/known_gps_coords/old/edcfront_trees_grass.yaml similarity index 100% rename from src/nav_commanders/known_gps_coords/old/edcfront_trees_grass.yaml rename to src/Nav/nav_commanders/known_gps_coords/old/edcfront_trees_grass.yaml diff --git a/src/nav_commanders/known_gps_coords/old/useful_locations.md b/src/Nav/nav_commanders/known_gps_coords/old/useful_locations.md similarity index 100% rename from src/nav_commanders/known_gps_coords/old/useful_locations.md rename to src/Nav/nav_commanders/known_gps_coords/old/useful_locations.md diff --git a/src/nav_commanders/known_gps_coords/send_gps_command.py b/src/Nav/nav_commanders/known_gps_coords/send_gps_command.py similarity index 100% rename from src/nav_commanders/known_gps_coords/send_gps_command.py rename to src/Nav/nav_commanders/known_gps_coords/send_gps_command.py diff --git a/src/nav_commanders/launch/aruco_gps_commander.py b/src/Nav/nav_commanders/launch/aruco_gps_commander.py similarity index 100% rename from src/nav_commanders/launch/aruco_gps_commander.py rename to src/Nav/nav_commanders/launch/aruco_gps_commander.py diff --git a/src/nav_commanders/launch/gps_commander.launch.py b/src/Nav/nav_commanders/launch/gps_commander.launch.py similarity index 100% rename from src/nav_commanders/launch/gps_commander.launch.py rename to src/Nav/nav_commanders/launch/gps_commander.launch.py diff --git a/src/nav_commanders/launch/incremental_gps_commander.launch.py b/src/Nav/nav_commanders/launch/incremental_gps_commander.launch.py similarity index 100% rename from src/nav_commanders/launch/incremental_gps_commander.launch.py rename to src/Nav/nav_commanders/launch/incremental_gps_commander.launch.py diff --git a/src/science_sensors/science_sensors/__init__.py b/src/Nav/nav_commanders/nav_commanders/__init__.py similarity index 100% rename from src/science_sensors/science_sensors/__init__.py rename to src/Nav/nav_commanders/nav_commanders/__init__.py diff --git a/src/nav_commanders/nav_commanders/gps_utils.py b/src/Nav/nav_commanders/nav_commanders/gps_utils.py similarity index 100% rename from src/nav_commanders/nav_commanders/gps_utils.py rename to src/Nav/nav_commanders/nav_commanders/gps_utils.py diff --git a/src/nav_commanders/nav_commanders/incremental_gps_commander.py b/src/Nav/nav_commanders/nav_commanders/incremental_gps_commander.py similarity index 100% rename from src/nav_commanders/nav_commanders/incremental_gps_commander.py rename to src/Nav/nav_commanders/nav_commanders/incremental_gps_commander.py diff --git a/src/nav_commanders/nav_commanders/nav_to_gps_aruco.py b/src/Nav/nav_commanders/nav_commanders/nav_to_gps_aruco.py similarity index 100% rename from src/nav_commanders/nav_commanders/nav_to_gps_aruco.py rename to src/Nav/nav_commanders/nav_commanders/nav_to_gps_aruco.py diff --git a/src/nav_commanders/nav_commanders/nav_to_gps_coords.py b/src/Nav/nav_commanders/nav_commanders/nav_to_gps_coords.py similarity index 100% rename from src/nav_commanders/nav_commanders/nav_to_gps_coords.py rename to src/Nav/nav_commanders/nav_commanders/nav_to_gps_coords.py diff --git a/src/nav_commanders/package.xml b/src/Nav/nav_commanders/package.xml similarity index 100% rename from src/nav_commanders/package.xml rename to src/Nav/nav_commanders/package.xml diff --git a/src/nav_commanders/resource/nav_commanders b/src/Nav/nav_commanders/resource/nav_commanders similarity index 100% rename from src/nav_commanders/resource/nav_commanders rename to src/Nav/nav_commanders/resource/nav_commanders diff --git a/src/nav_commanders/setup.cfg b/src/Nav/nav_commanders/setup.cfg similarity index 100% rename from src/nav_commanders/setup.cfg rename to src/Nav/nav_commanders/setup.cfg diff --git a/src/nav_commanders/setup.py b/src/Nav/nav_commanders/setup.py similarity index 100% rename from src/nav_commanders/setup.py rename to src/Nav/nav_commanders/setup.py diff --git a/src/science_sensors/test/test_copyright.py b/src/Nav/nav_commanders/test/test_copyright.py similarity index 100% rename from src/science_sensors/test/test_copyright.py rename to src/Nav/nav_commanders/test/test_copyright.py diff --git a/src/nav_commanders/test/test_flake8.py b/src/Nav/nav_commanders/test/test_flake8.py similarity index 100% rename from src/nav_commanders/test/test_flake8.py rename to src/Nav/nav_commanders/test/test_flake8.py diff --git a/src/nav_commanders/test/test_pep257.py b/src/Nav/nav_commanders/test/test_pep257.py similarity index 100% rename from src/nav_commanders/test/test_pep257.py rename to src/Nav/nav_commanders/test/test_pep257.py diff --git a/src/navigation/CMakeLists.txt b/src/Nav/navigation/CMakeLists.txt similarity index 100% rename from src/navigation/CMakeLists.txt rename to src/Nav/navigation/CMakeLists.txt diff --git a/src/navigation/behavior_trees/behaviorTree.xml b/src/Nav/navigation/behavior_trees/behaviorTree.xml similarity index 100% rename from src/navigation/behavior_trees/behaviorTree.xml rename to src/Nav/navigation/behavior_trees/behaviorTree.xml diff --git a/src/navigation/behavior_trees/behaviorTree_NavPathingTestTree.xml b/src/Nav/navigation/behavior_trees/behaviorTree_NavPathingTestTree.xml similarity index 100% rename from src/navigation/behavior_trees/behaviorTree_NavPathingTestTree.xml rename to src/Nav/navigation/behavior_trees/behaviorTree_NavPathingTestTree.xml diff --git a/src/navigation/behavior_trees/behaviorTree_original.xml b/src/Nav/navigation/behavior_trees/behaviorTree_original.xml similarity index 100% rename from src/navigation/behavior_trees/behaviorTree_original.xml rename to src/Nav/navigation/behavior_trees/behaviorTree_original.xml diff --git a/src/navigation/behavior_trees/bt_dynamic_replanning.xml b/src/Nav/navigation/behavior_trees/bt_dynamic_replanning.xml similarity index 100% rename from src/navigation/behavior_trees/bt_dynamic_replanning.xml rename to src/Nav/navigation/behavior_trees/bt_dynamic_replanning.xml diff --git a/src/navigation/behavior_trees/spike_bt.xml b/src/Nav/navigation/behavior_trees/spike_bt.xml similarity index 100% rename from src/navigation/behavior_trees/spike_bt.xml rename to src/Nav/navigation/behavior_trees/spike_bt.xml diff --git a/src/navigation/config/elevation_mapping/ouster_OS0_32U_sensor_processor.yaml b/src/Nav/navigation/config/elevation_mapping/ouster_OS0_32U_sensor_processor.yaml similarity index 100% rename from src/navigation/config/elevation_mapping/ouster_OS0_32U_sensor_processor.yaml rename to src/Nav/navigation/config/elevation_mapping/ouster_OS0_32U_sensor_processor.yaml diff --git a/src/navigation/config/elevation_mapping/ouster_elevation_mapping.yaml b/src/Nav/navigation/config/elevation_mapping/ouster_elevation_mapping.yaml similarity index 100% rename from src/navigation/config/elevation_mapping/ouster_elevation_mapping.yaml rename to src/Nav/navigation/config/elevation_mapping/ouster_elevation_mapping.yaml diff --git a/src/navigation/config/elevation_mapping/postprocessing_traversability.yaml b/src/Nav/navigation/config/elevation_mapping/postprocessing_traversability.yaml similarity index 100% rename from src/navigation/config/elevation_mapping/postprocessing_traversability.yaml rename to src/Nav/navigation/config/elevation_mapping/postprocessing_traversability.yaml diff --git a/src/navigation/config/elevation_mapping/spike_robot.yaml b/src/Nav/navigation/config/elevation_mapping/spike_robot.yaml similarity index 100% rename from src/navigation/config/elevation_mapping/spike_robot.yaml rename to src/Nav/navigation/config/elevation_mapping/spike_robot.yaml diff --git a/src/navigation/config/elevation_mapping/zed2i_elevation_mapping.yaml b/src/Nav/navigation/config/elevation_mapping/zed2i_elevation_mapping.yaml similarity index 100% rename from src/navigation/config/elevation_mapping/zed2i_elevation_mapping.yaml rename to src/Nav/navigation/config/elevation_mapping/zed2i_elevation_mapping.yaml diff --git a/src/navigation/config/elevation_mapping/zed2i_sensor_processor.yaml b/src/Nav/navigation/config/elevation_mapping/zed2i_sensor_processor.yaml similarity index 100% rename from src/navigation/config/elevation_mapping/zed2i_sensor_processor.yaml rename to src/Nav/navigation/config/elevation_mapping/zed2i_sensor_processor.yaml diff --git a/src/navigation/config/ouster/driver_params.yaml b/src/Nav/navigation/config/ouster/driver_params.yaml similarity index 100% rename from src/navigation/config/ouster/driver_params.yaml rename to src/Nav/navigation/config/ouster/driver_params.yaml diff --git a/src/navigation/config/zed/zed_override_params.yaml b/src/Nav/navigation/config/zed/zed_override_params.yaml similarity index 100% rename from src/navigation/config/zed/zed_override_params.yaml rename to src/Nav/navigation/config/zed/zed_override_params.yaml diff --git a/src/navigation/launch/bringup.launch.py b/src/Nav/navigation/launch/bringup.launch.py similarity index 100% rename from src/navigation/launch/bringup.launch.py rename to src/Nav/navigation/launch/bringup.launch.py diff --git a/src/navigation/launch/nav2.launch.py b/src/Nav/navigation/launch/nav2.launch.py similarity index 100% rename from src/navigation/launch/nav2.launch.py rename to src/Nav/navigation/launch/nav2.launch.py diff --git a/src/navigation/launch/navigation.launch.py b/src/Nav/navigation/launch/navigation.launch.py similarity index 100% rename from src/navigation/launch/navigation.launch.py rename to src/Nav/navigation/launch/navigation.launch.py diff --git a/src/navigation/launch/ouster.launch.py b/src/Nav/navigation/launch/ouster.launch.py similarity index 100% rename from src/navigation/launch/ouster.launch.py rename to src/Nav/navigation/launch/ouster.launch.py diff --git a/src/navigation/launch/ouster_composable.launch.py b/src/Nav/navigation/launch/ouster_composable.launch.py similarity index 100% rename from src/navigation/launch/ouster_composable.launch.py rename to src/Nav/navigation/launch/ouster_composable.launch.py diff --git a/src/navigation/launch/traversability_gridmap.launch.py b/src/Nav/navigation/launch/traversability_gridmap.launch.py similarity index 100% rename from src/navigation/launch/traversability_gridmap.launch.py rename to src/Nav/navigation/launch/traversability_gridmap.launch.py diff --git a/src/navigation/launch/zed.launch.py b/src/Nav/navigation/launch/zed.launch.py similarity index 100% rename from src/navigation/launch/zed.launch.py rename to src/Nav/navigation/launch/zed.launch.py diff --git a/src/navigation/lidar_masks/ouster_mask_combined_shifted.png b/src/Nav/navigation/lidar_masks/ouster_mask_combined_shifted.png similarity index 100% rename from src/navigation/lidar_masks/ouster_mask_combined_shifted.png rename to src/Nav/navigation/lidar_masks/ouster_mask_combined_shifted.png diff --git a/src/navigation/lidar_masks/ouster_mask_front.png b/src/Nav/navigation/lidar_masks/ouster_mask_front.png similarity index 100% rename from src/navigation/lidar_masks/ouster_mask_front.png rename to src/Nav/navigation/lidar_masks/ouster_mask_front.png diff --git a/src/navigation/lidar_masks/ouster_mask_left.png b/src/Nav/navigation/lidar_masks/ouster_mask_left.png similarity index 100% rename from src/navigation/lidar_masks/ouster_mask_left.png rename to src/Nav/navigation/lidar_masks/ouster_mask_left.png diff --git a/src/navigation/lidar_masks/ouster_mask_rear.png b/src/Nav/navigation/lidar_masks/ouster_mask_rear.png similarity index 100% rename from src/navigation/lidar_masks/ouster_mask_rear.png rename to src/Nav/navigation/lidar_masks/ouster_mask_rear.png diff --git a/src/navigation/lidar_masks/ouster_mask_right.png b/src/Nav/navigation/lidar_masks/ouster_mask_right.png similarity index 100% rename from src/navigation/lidar_masks/ouster_mask_right.png rename to src/Nav/navigation/lidar_masks/ouster_mask_right.png diff --git a/src/navigation/package.xml b/src/Nav/navigation/package.xml similarity index 100% rename from src/navigation/package.xml rename to src/Nav/navigation/package.xml diff --git a/src/navigation/params/nav2.yaml b/src/Nav/navigation/params/nav2.yaml similarity index 100% rename from src/navigation/params/nav2.yaml rename to src/Nav/navigation/params/nav2.yaml diff --git a/src/navigation/rviz/nav2_cprt.rviz b/src/Nav/navigation/rviz/nav2_cprt.rviz similarity index 100% rename from src/navigation/rviz/nav2_cprt.rviz rename to src/Nav/navigation/rviz/nav2_cprt.rviz diff --git a/src/navigation/rviz/nav2_offical_default_config.rviz b/src/Nav/navigation/rviz/nav2_offical_default_config.rviz similarity index 100% rename from src/navigation/rviz/nav2_offical_default_config.rviz rename to src/Nav/navigation/rviz/nav2_offical_default_config.rviz diff --git a/src/navigation/rviz/traversibility_gridmap.rviz b/src/Nav/navigation/rviz/traversibility_gridmap.rviz similarity index 100% rename from src/navigation/rviz/traversibility_gridmap.rviz rename to src/Nav/navigation/rviz/traversibility_gridmap.rviz diff --git a/src/system-telemetry-cpp/CMakeLists.txt b/src/Telemetry/system-telemetry-cpp/CMakeLists.txt similarity index 100% rename from src/system-telemetry-cpp/CMakeLists.txt rename to src/Telemetry/system-telemetry-cpp/CMakeLists.txt diff --git a/src/system-telemetry-cpp/LICENSE b/src/Telemetry/system-telemetry-cpp/LICENSE similarity index 100% rename from src/system-telemetry-cpp/LICENSE rename to src/Telemetry/system-telemetry-cpp/LICENSE diff --git a/src/system-telemetry-cpp/include/system-telemetry-cpp/cpu_collector.hpp b/src/Telemetry/system-telemetry-cpp/include/system-telemetry-cpp/cpu_collector.hpp similarity index 100% rename from src/system-telemetry-cpp/include/system-telemetry-cpp/cpu_collector.hpp rename to src/Telemetry/system-telemetry-cpp/include/system-telemetry-cpp/cpu_collector.hpp diff --git a/src/system-telemetry-cpp/include/system-telemetry-cpp/gpu_collector.hpp b/src/Telemetry/system-telemetry-cpp/include/system-telemetry-cpp/gpu_collector.hpp similarity index 100% rename from src/system-telemetry-cpp/include/system-telemetry-cpp/gpu_collector.hpp rename to src/Telemetry/system-telemetry-cpp/include/system-telemetry-cpp/gpu_collector.hpp diff --git a/src/system-telemetry-cpp/include/system-telemetry-cpp/memory_collector.hpp b/src/Telemetry/system-telemetry-cpp/include/system-telemetry-cpp/memory_collector.hpp similarity index 100% rename from src/system-telemetry-cpp/include/system-telemetry-cpp/memory_collector.hpp rename to src/Telemetry/system-telemetry-cpp/include/system-telemetry-cpp/memory_collector.hpp diff --git a/src/system-telemetry-cpp/include/system-telemetry-cpp/system_telemetry_publisher.hpp b/src/Telemetry/system-telemetry-cpp/include/system-telemetry-cpp/system_telemetry_publisher.hpp similarity index 100% rename from src/system-telemetry-cpp/include/system-telemetry-cpp/system_telemetry_publisher.hpp rename to src/Telemetry/system-telemetry-cpp/include/system-telemetry-cpp/system_telemetry_publisher.hpp diff --git a/src/system-telemetry-cpp/include/system-telemetry-cpp/telemetry_collector.hpp b/src/Telemetry/system-telemetry-cpp/include/system-telemetry-cpp/telemetry_collector.hpp similarity index 100% rename from src/system-telemetry-cpp/include/system-telemetry-cpp/telemetry_collector.hpp rename to src/Telemetry/system-telemetry-cpp/include/system-telemetry-cpp/telemetry_collector.hpp diff --git a/src/system-telemetry-cpp/package.xml b/src/Telemetry/system-telemetry-cpp/package.xml similarity index 100% rename from src/system-telemetry-cpp/package.xml rename to src/Telemetry/system-telemetry-cpp/package.xml diff --git a/src/system-telemetry-cpp/src/cpu_collector.cpp b/src/Telemetry/system-telemetry-cpp/src/cpu_collector.cpp similarity index 100% rename from src/system-telemetry-cpp/src/cpu_collector.cpp rename to src/Telemetry/system-telemetry-cpp/src/cpu_collector.cpp diff --git a/src/system-telemetry-cpp/src/gpu_collector.cpp b/src/Telemetry/system-telemetry-cpp/src/gpu_collector.cpp similarity index 100% rename from src/system-telemetry-cpp/src/gpu_collector.cpp rename to src/Telemetry/system-telemetry-cpp/src/gpu_collector.cpp diff --git a/src/system-telemetry-cpp/src/memory_collector.cpp b/src/Telemetry/system-telemetry-cpp/src/memory_collector.cpp similarity index 100% rename from src/system-telemetry-cpp/src/memory_collector.cpp rename to src/Telemetry/system-telemetry-cpp/src/memory_collector.cpp diff --git a/src/system-telemetry-cpp/src/system_telemetry_publisher.cpp b/src/Telemetry/system-telemetry-cpp/src/system_telemetry_publisher.cpp similarity index 100% rename from src/system-telemetry-cpp/src/system_telemetry_publisher.cpp rename to src/Telemetry/system-telemetry-cpp/src/system_telemetry_publisher.cpp diff --git a/src/drive_cpp/CMakeLists.txt b/src/Teleop-Control/drive_cpp/CMakeLists.txt similarity index 93% rename from src/drive_cpp/CMakeLists.txt rename to src/Teleop-Control/drive_cpp/CMakeLists.txt index 5ef54e17..f13a6ff5 100644 --- a/src/drive_cpp/CMakeLists.txt +++ b/src/Teleop-Control/drive_cpp/CMakeLists.txt @@ -71,7 +71,12 @@ install( install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME} ) - +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) ament_export_include_directories("include/${PROJECT_NAME}") ament_package() diff --git a/src/drive/config/pxn.yaml b/src/Teleop-Control/drive_cpp/config/pxn.yaml similarity index 100% rename from src/drive/config/pxn.yaml rename to src/Teleop-Control/drive_cpp/config/pxn.yaml diff --git a/src/drive/config/talon_drive_common.yaml b/src/Teleop-Control/drive_cpp/config/talon_drive_common.yaml similarity index 100% rename from src/drive/config/talon_drive_common.yaml rename to src/Teleop-Control/drive_cpp/config/talon_drive_common.yaml diff --git a/src/drive/config/talon_drive_unique.yaml b/src/Teleop-Control/drive_cpp/config/talon_drive_unique.yaml similarity index 100% rename from src/drive/config/talon_drive_unique.yaml rename to src/Teleop-Control/drive_cpp/config/talon_drive_unique.yaml diff --git a/src/drive/config/thrustmaster.yaml b/src/Teleop-Control/drive_cpp/config/thrustmaster.yaml similarity index 100% rename from src/drive/config/thrustmaster.yaml rename to src/Teleop-Control/drive_cpp/config/thrustmaster.yaml diff --git a/src/drive_cpp/include/drive_cpp/TalonDriveController.hpp b/src/Teleop-Control/drive_cpp/include/drive_cpp/TalonDriveController.hpp similarity index 100% rename from src/drive_cpp/include/drive_cpp/TalonDriveController.hpp rename to src/Teleop-Control/drive_cpp/include/drive_cpp/TalonDriveController.hpp diff --git a/src/drive_cpp/include/drive_cpp/WheelControl.hpp b/src/Teleop-Control/drive_cpp/include/drive_cpp/WheelControl.hpp similarity index 100% rename from src/drive_cpp/include/drive_cpp/WheelControl.hpp rename to src/Teleop-Control/drive_cpp/include/drive_cpp/WheelControl.hpp diff --git a/src/drive/launch/talon.launch.py b/src/Teleop-Control/drive_cpp/launch/talon.launch.py similarity index 88% rename from src/drive/launch/talon.launch.py rename to src/Teleop-Control/drive_cpp/launch/talon.launch.py index e1dd501a..fe67cb0f 100644 --- a/src/drive/launch/talon.launch.py +++ b/src/Teleop-Control/drive_cpp/launch/talon.launch.py @@ -10,16 +10,12 @@ def generate_launch_description(): """Generate launch description with multiple components.""" - try: - subprocess.run(["sudo", "enablecan.sh"], check=True) - except Exception: - print("enablecan.sh not found or failed to run.") common_config_path = os.path.join( - get_package_share_directory("drive"), "config", "talon_drive_common.yaml" + get_package_share_directory("drive_cpp"), "config", "talon_drive_common.yaml" ) unique_config_path = os.path.join( - get_package_share_directory("drive"), "config", "talon_drive_unique.yaml" + get_package_share_directory("drive_cpp"), "config", "talon_drive_unique.yaml" ) config_paths_lst = [common_config_path, unique_config_path] diff --git a/src/drive_cpp/package.xml b/src/Teleop-Control/drive_cpp/package.xml similarity index 100% rename from src/drive_cpp/package.xml rename to src/Teleop-Control/drive_cpp/package.xml diff --git a/src/drive_cpp/src/TalonDriveController.cpp b/src/Teleop-Control/drive_cpp/src/TalonDriveController.cpp similarity index 100% rename from src/drive_cpp/src/TalonDriveController.cpp rename to src/Teleop-Control/drive_cpp/src/TalonDriveController.cpp diff --git a/src/drive_cpp/src/WheelControl.cpp b/src/Teleop-Control/drive_cpp/src/WheelControl.cpp similarity index 100% rename from src/drive_cpp/src/WheelControl.cpp rename to src/Teleop-Control/drive_cpp/src/WheelControl.cpp diff --git a/src/drive_cpp/src/main.cpp b/src/Teleop-Control/drive_cpp/src/main.cpp similarity index 100% rename from src/drive_cpp/src/main.cpp rename to src/Teleop-Control/drive_cpp/src/main.cpp diff --git a/src/drive_cpp/talon_drive_controller_plugin.xml b/src/Teleop-Control/drive_cpp/talon_drive_controller_plugin.xml similarity index 100% rename from src/drive_cpp/talon_drive_controller_plugin.xml rename to src/Teleop-Control/drive_cpp/talon_drive_controller_plugin.xml diff --git a/src/joystick_control/CMakeLists.txt b/src/Teleop-Control/joystick_control/CMakeLists.txt similarity index 100% rename from src/joystick_control/CMakeLists.txt rename to src/Teleop-Control/joystick_control/CMakeLists.txt diff --git a/src/joystick_control/include/joystick_control/ArmDummyMode.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/ArmDummyMode.hpp similarity index 100% rename from src/joystick_control/include/joystick_control/ArmDummyMode.hpp rename to src/Teleop-Control/joystick_control/include/joystick_control/ArmDummyMode.hpp diff --git a/src/joystick_control/include/joystick_control/ArmHelpers.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp similarity index 100% rename from src/joystick_control/include/joystick_control/ArmHelpers.hpp rename to src/Teleop-Control/joystick_control/include/joystick_control/ArmHelpers.hpp diff --git a/src/joystick_control/include/joystick_control/ArmIKMode.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/ArmIKMode.hpp similarity index 100% rename from src/joystick_control/include/joystick_control/ArmIKMode.hpp rename to src/Teleop-Control/joystick_control/include/joystick_control/ArmIKMode.hpp diff --git a/src/joystick_control/include/joystick_control/ArmManualMode.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/ArmManualMode.hpp similarity index 100% rename from src/joystick_control/include/joystick_control/ArmManualMode.hpp rename to src/Teleop-Control/joystick_control/include/joystick_control/ArmManualMode.hpp diff --git a/src/joystick_control/include/joystick_control/DriveMode.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/DriveMode.hpp similarity index 100% rename from src/joystick_control/include/joystick_control/DriveMode.hpp rename to src/Teleop-Control/joystick_control/include/joystick_control/DriveMode.hpp diff --git a/src/joystick_control/include/joystick_control/FlightstickControl.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/FlightstickControl.hpp similarity index 100% rename from src/joystick_control/include/joystick_control/FlightstickControl.hpp rename to src/Teleop-Control/joystick_control/include/joystick_control/FlightstickControl.hpp diff --git a/src/joystick_control/include/joystick_control/Mode.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/Mode.hpp similarity index 100% rename from src/joystick_control/include/joystick_control/Mode.hpp rename to src/Teleop-Control/joystick_control/include/joystick_control/Mode.hpp diff --git a/src/joystick_control/include/joystick_control/ScienceMode.hpp b/src/Teleop-Control/joystick_control/include/joystick_control/ScienceMode.hpp similarity index 100% rename from src/joystick_control/include/joystick_control/ScienceMode.hpp rename to src/Teleop-Control/joystick_control/include/joystick_control/ScienceMode.hpp diff --git a/src/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py similarity index 94% rename from src/joystick_control/launch/controller.launch.py rename to src/Teleop-Control/joystick_control/launch/controller.launch.py index e4be50d4..1604716f 100644 --- a/src/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -7,7 +7,7 @@ # Make launch configeration, defult false, launch talon on rover. Defult True for xbox def generate_launch_description(): """Generate launch description with multiple components.""" - pkg_drive = get_package_share_directory("drive") + pkg_drive = get_package_share_directory("drive_cpp") parameters_file = os.path.join(pkg_drive, "config", "pxn.yaml") return launch.LaunchDescription( diff --git a/src/joystick_control/package.xml b/src/Teleop-Control/joystick_control/package.xml similarity index 100% rename from src/joystick_control/package.xml rename to src/Teleop-Control/joystick_control/package.xml diff --git a/src/joystick_control/src/ArmDummyMode.cpp b/src/Teleop-Control/joystick_control/src/ArmDummyMode.cpp similarity index 100% rename from src/joystick_control/src/ArmDummyMode.cpp rename to src/Teleop-Control/joystick_control/src/ArmDummyMode.cpp diff --git a/src/joystick_control/src/ArmHelpers.cpp b/src/Teleop-Control/joystick_control/src/ArmHelpers.cpp similarity index 100% rename from src/joystick_control/src/ArmHelpers.cpp rename to src/Teleop-Control/joystick_control/src/ArmHelpers.cpp diff --git a/src/joystick_control/src/ArmIKMode.cpp b/src/Teleop-Control/joystick_control/src/ArmIKMode.cpp similarity index 100% rename from src/joystick_control/src/ArmIKMode.cpp rename to src/Teleop-Control/joystick_control/src/ArmIKMode.cpp diff --git a/src/joystick_control/src/ArmManualMode.cpp b/src/Teleop-Control/joystick_control/src/ArmManualMode.cpp similarity index 100% rename from src/joystick_control/src/ArmManualMode.cpp rename to src/Teleop-Control/joystick_control/src/ArmManualMode.cpp diff --git a/src/joystick_control/src/DriveMode.cpp b/src/Teleop-Control/joystick_control/src/DriveMode.cpp similarity index 100% rename from src/joystick_control/src/DriveMode.cpp rename to src/Teleop-Control/joystick_control/src/DriveMode.cpp diff --git a/src/joystick_control/src/FlightstickControl.cpp b/src/Teleop-Control/joystick_control/src/FlightstickControl.cpp similarity index 100% rename from src/joystick_control/src/FlightstickControl.cpp rename to src/Teleop-Control/joystick_control/src/FlightstickControl.cpp diff --git a/src/joystick_control/src/ScienceMode.cpp b/src/Teleop-Control/joystick_control/src/ScienceMode.cpp similarity index 100% rename from src/joystick_control/src/ScienceMode.cpp rename to src/Teleop-Control/joystick_control/src/ScienceMode.cpp diff --git a/src/camera_controller/camera_controller/cam_control.py b/src/camera_controller/camera_controller/cam_control.py deleted file mode 100755 index 432489ea..00000000 --- a/src/camera_controller/camera_controller/cam_control.py +++ /dev/null @@ -1,103 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from rclpy.node import Node -from interfaces.srv import MoveServo -from geometry_msgs.msg import Twist -from interfaces.srv import Cam_Reset - - -class Cam_Servo_Client(Node): - def __init__(self): - super().__init__("cam_servo_client") - self.cli = self.create_client(MoveServo, "cam_servo_service") - while not self.cli.wait_for_service(timeout_sec=1.0): - self.get_logger().warn("service not available, waiting again...") - self.load_params() - self.goal_pos_x = self.default_pos - self.goal_pos_y = self.default_pos - self.last_pos_x = self.default_pos - self.last_pos_y = self.default_pos - period = ( - 1.0 / self.get_parameter("frequency").get_parameter_value().double_value - ) - self.timer = self.create_timer(period, self.camera_mover) - self.subscription = self.create_subscription( - Twist, "Direction", self.direction_callback, 10 - ) - self.start = self.create_service(Cam_Reset, "cam/reset", self.reset_callback) - - def load_params(self): - self.declare_parameter("port_x", 0) - self.declare_parameter("port_y", 1) - self.declare_parameter("min_servo", 0) - self.declare_parameter("max_servo", 180) - self.declare_parameter("default_x_pos", 90) - self.declare_parameter("default_y_pos", 90) - self.declare_parameter("step_size", 2) - self.declare_parameter("frequency", 10.0) - self.port_x = self.get_parameter("port_x").get_parameter_value().integer_value - self.port_y = self.get_parameter("port_y").get_parameter_value().integer_value - self.min_servo = ( - self.get_parameter("min_servo").get_parameter_value().integer_value - ) - self.max_servo = ( - self.get_parameter("max_servo").get_parameter_value().integer_value - ) - self.default_x_pos = ( - self.get_parameter("default_x_pos").get_parameter_value().integer_value - ) - self.default_y_pos = ( - self.get_parameter("default_y_pos").get_parameter_value().integer_value - ) - self.step_size = ( - self.get_parameter("step_size").get_parameter_value().double_value - ) - - def direction_callback(self, msg: Twist) -> None: - self.goal_pos_x = self.last_pos_x + msg.angular.y * self.step_size - self.goal_pos_y = self.last_pos_y + msg.angular.z * self.step_size - self.goal_pos_x = max(self.min_servo, min(self.max_servo, self.goal_pos_x)) - self.goal_pos_y = max(self.min_servo, min(self.max_servo, self.goal_pos_y)) - - def reset_callback(self, request, response): - if request.reset == True: - self.goal_pos_x = self.default_x_pos - self.goal_pos_y = self.default_y_pos - response.yaw = self.goal_pos_x - response.pitch = self.goal_pos_y - return - - def send_request(self, port: int, pos: int, min: int, max: int) -> MoveServo: - req = MoveServo.Request() - req.port = port - req.pos = pos - req.min = min - req.max = max - future = self.cli.call_async(req) - - def camera_mover(self) -> None: - if self.goal_pos_x != self.last_pos_x: - self.send_request( - self.port_x, self.goal_pos_x, self.min_servo, self.max_servo - ) - self.last_pos_x = self.goal_pos_x - if self.goal_pos_y != self.last_pos_y: - self.send_request( - self.port_y, self.goal_pos_y, self.min_servo, self.max_servo - ) - self.last_pos_y = self.goal_pos_y - - -def main(args=None): - rclpy.init(args=args) - - servo_Client = Cam_Servo_Client() - rclpy.spin(servo_Client) - servo_Client.destroy_node() - - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/camera_controller/launch/camera_controller.launch.py b/src/camera_controller/launch/camera_controller.launch.py deleted file mode 100644 index 0b543b6a..00000000 --- a/src/camera_controller/launch/camera_controller.launch.py +++ /dev/null @@ -1,27 +0,0 @@ -import launch -import launch_ros.actions - - -def generate_launch_description(): - return launch.LaunchDescription( - [ - launch_ros.actions.Node( - package="camera_controller", - executable="cam_control", - name="gas_sensor", - parameters=[ - {"port_x": 0}, - {"port_y": 1}, - {"min_servo": 0}, - {"max_servo": 180}, - {"default_x": 90}, - {"default_y": 90}, - {"step_size": 1.0}, - {"x_axis_index": 1}, - {"y_axis_index": 0}, - {"default_button": -1}, - {"frequency": 10.0}, - ], - ), - ] - ) diff --git a/src/camera_controller/package.xml b/src/camera_controller/package.xml deleted file mode 100644 index 6864fc90..00000000 --- a/src/camera_controller/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - camera_controller - 0.0.0 - TODO: Package description - isaac - TODO: License declaration - - rclpy - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/camera_controller/setup.cfg b/src/camera_controller/setup.cfg deleted file mode 100644 index 5a7c2b1d..00000000 --- a/src/camera_controller/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/camera_controller -[install] -install_scripts=$base/lib/camera_controller diff --git a/src/camera_controller/setup.py b/src/camera_controller/setup.py deleted file mode 100644 index 99b72a25..00000000 --- a/src/camera_controller/setup.py +++ /dev/null @@ -1,31 +0,0 @@ -from setuptools import find_packages, setup -import os -from glob import glob - -package_name = "camera_controller" - -setup( - name=package_name, - version="0.0.0", - packages=find_packages(exclude=["test"]), - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - ( - os.path.join("share", package_name, "launch"), - glob(os.path.join("launch", "*launch.[pxy][yma]*")), - ), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="isaac", - maintainer_email="isaaccervantesg@gmail.com", - description="TODO: Package description", - license="TODO: License declaration", - tests_require=["pytest"], - entry_points={ - "console_scripts": [ - "cam_control = camera_controller.cam_control:main", - ], - }, -) diff --git a/src/drive/drive/drive_station.py b/src/drive/drive/drive_station.py deleted file mode 100644 index 965fb184..00000000 --- a/src/drive/drive/drive_station.py +++ /dev/null @@ -1,94 +0,0 @@ -from math import radians -import rclpy -from rclpy.node import Node - -from std_msgs.msg import Int8, Float32 -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Twist - -from interfaces.msg import LiveTune -from interfaces.msg import SixFloats -from interfaces.msg import PIDmsg - - -class driveStation(Node): - def __init__(self): - super().__init__("drivestation") - self.liveTunePub = self.create_publisher(LiveTune, "drive_liveTune", 10) - self.statePub = self.create_publisher(Int8, "cmd_drive_state", 10) - - self.cmddrivePub = self.create_publisher(SixFloats, "cmd_move", 10) - - self.setVoltsPub = self.create_publisher(SixFloats, "cmd_voltage_drive", 10) - - self.setTwistPub = self.create_publisher(Twist, "/cmd_vel", 10) - - self.setPid = self.create_publisher(PIDmsg, "pid_update", 10) - - self.create_timer(0.2, self.loopRun) - - def loopRun(self): - print("Enter command: ") - u = input("Enter command: ").lower().split(" ") - - if u[0] == "t": - twist = Twist() - twist.linear.x = float(u[1]) - twist.angular.z = float(u[2]) - self.setTwistPub.publish(twist) - return - elif u[0] == "v": - volts = SixFloats() - volts.m0 = float(u[1]) - volts.m1 = float(u[2]) - volts.m2 = float(u[3]) - volts.m3 = float(u[4]) - volts.m4 = float(u[5]) - volts.m5 = float(u[6]) - self.setVoltsPub.publish(volts) - return - - elif u[0] == "p": - pid = PIDmsg() - pid.p = float(u[1]) - pid.i = float(u[2]) - pid.d = float(u[3]) - self.setPid.publish(pid) - return - - elif u[0] == "y": - self.get_logger().info("Turning PID on") - temp = Int8() - temp.data = 1 - self.statePub.publish(temp) - return - - elif u[0] == "n": - self.get_logger().info("Turning PID off") - temp = Int8() - temp.data = 0 - self.statePub.publish(temp) - return - - elif u[0] == "stop": - volts = SixFloats() - volts.m0 = 0 - volts.m1 = 0 - volts.m2 = 0 - volts.m3 = 0 - volts.m4 = 0 - volts.m5 = 0 - self.setVoltsPub.publish(volts) - return - - -def main(args=None): - rclpy.init(args=args) - node = driveStation() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/drive/drive/electrical_wrapper.py b/src/drive/drive/electrical_wrapper.py deleted file mode 100644 index bd3a8edc..00000000 --- a/src/drive/drive/electrical_wrapper.py +++ /dev/null @@ -1,56 +0,0 @@ -from rclpy.time import Time -from sensor_msgs.msg import BatteryState - - -class ElectricalWrapper: - def __init__(self, parent_node): - """Roboclaw Electrical Data - - Args: - parent_node (rclpy.node.Node): RCL Node - """ - # Node parameters - self.parent_node = parent_node - self.clock = self.parent_node.get_clock() - self.logger = self.parent_node.get_logger() - # Electricity publishers - self.left_elec_pub = self.parent_node.left_elec_pub - self.right_elec_pub = self.parent_node.right_elec_pub - # Electrical data - self.motor_elec = {} - - def publish_elec(self, cur_time: Time, elec_data: dict): - """Publish elec in two battery states messages - - Main Battery Voltage => voltage - Current => current - Logic Battery Voltage => charge - PWM => percentage - Temperature => temperature - - Args: - cur_time (Time): Current time - elec_data: Electrical data from the RoboClaw - """ - - side = "left" - bs = BatteryState() - bs.header.stamp = cur_time.to_msg() - bs.header.frame_id = "base_link" - bs.voltage = elec_data["voltage"] - bs.current = elec_data["current"][side] - bs.percentage = elec_data["pwm"][side] - bs.charge = elec_data["logicbatt"] - bs.temperature = elec_data["temperature"] - self.left_elec_pub.publish(bs) - - side = "right" - bs = BatteryState() - bs.header.stamp = cur_time.to_msg() - bs.header.frame_id = "base_link" - bs.voltage = elec_data["voltage"] - bs.current = elec_data["current"][side] - bs.percentage = elec_data["pwm"][side] - bs.charge = elec_data["logicbatt"] - bs.temperature = elec_data["temperature"] - self.right_elec_pub.publish(bs) diff --git a/src/drive/drive/encoder_wrapper.py b/src/drive/drive/encoder_wrapper.py deleted file mode 100644 index 5ea4753b..00000000 --- a/src/drive/drive/encoder_wrapper.py +++ /dev/null @@ -1,202 +0,0 @@ -import math -from math import cos, pi, sin - -from geometry_msgs.msg import Quaternion, TransformStamped -from nav_msgs.msg import Odometry -from rclpy.time import Time -from std_msgs.msg import Float64 -from tf_transformations import quaternion_from_euler - - -class EncoderWrapper: - def __init__( - self, - ticks_per_meter, - ticks_per_rotation, - base_width, - parent_node, - ): - """Encoder Odometry + Electrical - - Args: - ticks_per_meter (float): Ticks per meter, according to the ROS parameters - ticks_per_rotation (float): Ticks per wheel rotation, according to the ROS parameters - base_width (float): Base width (baseline) of the robot - parent_node (rclpy.node.Node): RCL Node - """ - self.TICKS_PER_METER = ticks_per_meter - self.TICKS_PER_ROTATION = ticks_per_rotation - self.BASE_WIDTH = base_width - # Node parameters - self.parent_node = parent_node - self.clock = self.parent_node.get_clock() - self.logger = self.parent_node.get_logger() - # Odometry, left and right encoders publishers - self.odom_pub = self.parent_node.odom_pub - self.left_encoder_pub = self.parent_node.left_encoder_pub - self.right_encoder_pub = self.parent_node.right_encoder_pub - # Odometry data - self.cur_x = 0 - self.cur_y = 0 - self.cur_theta = 0.0 - self.last_enc_left = 0 - self.last_enc_right = 0 - self.last_enc_time = self.clock.now().nanoseconds - self.vel_theta = 0 - self.left_ang_vel = 0 - self.right_ang_vel = 0 - - @staticmethod - def normalize_angle(angle): - while angle > pi: - angle -= 2.0 * pi - while angle < -pi: - angle += 2.0 * pi - return angle - - def update(self, enc_left, enc_right): - # Hack solution to accomidate for reversing motor 1 is make it negative - left_ticks = enc_left - self.last_enc_left - right_ticks = -(enc_right - self.last_enc_right) - self.last_enc_left = enc_left - self.last_enc_right = enc_right - - dist_left = left_ticks / self.TICKS_PER_METER - dist_right = right_ticks / self.TICKS_PER_METER - dist = (dist_right + dist_left) / 2.0 - - current_time = self.clock.now().nanoseconds - d_time = current_time - self.last_enc_time - self.last_enc_time = current_time - - self.left_ang_vel = ( - 2 * math.pi * left_ticks / (self.TICKS_PER_ROTATION * d_time * 1e-9) - ) - self.right_ang_vel = ( - 2 * math.pi * right_ticks / (self.TICKS_PER_ROTATION * d_time * 1e-9) - ) - - # TODO find better what to determine going straight, this means slight deviation is accounted - if left_ticks == right_ticks: - d_theta = 0.0 - self.cur_x += dist * cos(self.cur_theta) - self.cur_y += dist * sin(self.cur_theta) - else: - # delta theta - d_theta = -(dist_right - dist_left) / self.BASE_WIDTH - r = dist / d_theta - self.cur_x += r * (sin(d_theta + self.cur_theta) - sin(self.cur_theta)) - self.cur_y -= r * (cos(d_theta + self.cur_theta) - cos(self.cur_theta)) - self.cur_theta = self.normalize_angle(self.cur_theta + d_theta) - - if abs(d_time) < 1000: - vel_x = 0.0 - vel_theta = 0.0 - else: - vel_x = dist / (d_time * 1e-9) - vel_theta = d_theta / (d_time * 1e-9) - - self.vel_theta = vel_theta - return vel_x, vel_theta - - def update_n_publish(self, enc_left: float, enc_right: float, publish_time: Time): - """Update odom and elec and publish all data accordingly - - Args: - enc_left: Left Encoder Measurement - enc_right: Right Encoder Measurement - publish_time (Time): Current time - """ - # 2106 per 0.1 seconds is max speed, error in the 16th bit is 32768 - # TODO lets find a better way to deal with this error - if abs(enc_left - self.last_enc_left) > 20000: - self.logger.error( - "Ignoring left encoder jump: cur " - + str(enc_left) - + ", last " - + str(self.last_enc_left) - ) - return - elif abs(enc_right - self.last_enc_right) > 20000: - self.logger.error( - "Ignoring right encoder jump: cur " - + str(enc_right) - + ", last " - + str(self.last_enc_right) - ) - return - - vel_x, vel_theta = self.update(enc_left, enc_right) - self.publish_odom( - self.cur_x, self.cur_y, self.cur_theta, publish_time, vel_x, vel_theta - ) - - def publish_odom( - self, - cur_x: float, - cur_y: float, - cur_theta: float, - cur_time: Time, - vx: float, - vth: float, - ): - """Publish odometry - - Args: - cur_x (float): Current x coordinate - cur_y (float): Current y coordinate - cur_theta (float): Current theta heading - cur_time (Time): Current time - vx (float): Linear speed - forward - vth (float): angular speed => delta theta / time - """ - quat = quaternion_from_euler(0, 0, cur_theta) - - t = TransformStamped() - t.header.stamp = cur_time.to_msg() - t.header.frame_id = "base_link" - t.child_frame_id = "odom" - t.transform.translation.x = cur_x - t.transform.translation.y = cur_y - t.transform.translation.z = 0.0 - q = quaternion_from_euler(0, 0, -cur_theta) - t.transform.rotation.x = q[0] - t.transform.rotation.y = q[1] - t.transform.rotation.z = q[2] - t.transform.rotation.w = q[3] - # br = tf2_ros.TransformBroadcaster(self.parent_node) - # br.sendTransform(t) - - odom = Odometry() - odom.header.stamp = cur_time.to_msg() - odom.header.frame_id = "odom" - - odom.pose.pose.position.x = cur_x - odom.pose.pose.position.y = cur_y - odom.pose.pose.position.z = 0.0 - odom.pose.pose.orientation = Quaternion( - x=quat[0], y=quat[1], z=quat[2], w=quat[3] - ) - - odom.pose.covariance[0] = 0.03 - odom.pose.covariance[7] = 0.03 - odom.pose.covariance[14] = 99999 - odom.pose.covariance[21] = 99999 - odom.pose.covariance[28] = 99999 - odom.pose.covariance[35] = 0.03 - - odom.child_frame_id = "base_link" - odom.twist.twist.linear.x = vx - odom.twist.twist.linear.y = 0.0 - odom.twist.twist.angular.z = vth - odom.twist.covariance = odom.pose.covariance - - left_enc = Float64() - left_enc.data = self.left_ang_vel - right_enc = Float64() - right_enc.data = self.right_ang_vel - - self.odom_pub.publish(odom) - - self.left_encoder_pub.publish(left_enc) - self.right_encoder_pub.publish(right_enc) diff --git a/src/drive/drive/joystick_breakout.py b/src/drive/drive/joystick_breakout.py deleted file mode 100644 index a9943cc9..00000000 --- a/src/drive/drive/joystick_breakout.py +++ /dev/null @@ -1,48 +0,0 @@ -import rclpy -import rclpy.logging -from rclpy.node import Node - -import rclpy.time -from sensor_msgs.msg import Joy -from std_msgs.msg import Bool - - -class joystickBreakout(Node): - def __init__(self): - super().__init__("joystick_breakout") - self.estop = Bool() - self.estop.data = False - self.lastTimestamp = 0.0 - self.pubState = 0 # 0 = drive 1 = arm - - self.setDrive = self.create_publisher(Joy, "/joystick/drive", 1) - self.setArm = self.create_publisher(Joy, "/joystick/arm", 1) - self.setEstop = self.create_publisher(Joy, "/joystick/estop", 1) - self.cmd_joy_subscriber = self.create_subscription( - Joy, "/joy", self.cmd_joy_callback, 10 - ) - - def cmd_joy_callback(self, msg: Joy): - if msg.buttons[8] == 1: - self.pubState = 0 - elif msg.buttons[9] == 1: - self.pubState = 1 - if self.pubState == 0: - self.setDrive.publish(msg) - elif self.pubState == 1: - self.setArm.publish(msg) - else: - msg.axes = [-val for val in msg.axes] - self.setDrive.publish(msg) - - -def main(args=None): - rclpy.init(args=args) - node = joystickBreakout() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/drive/drive/joystick_controller.py b/src/drive/drive/joystick_controller.py deleted file mode 100644 index 8f893a9e..00000000 --- a/src/drive/drive/joystick_controller.py +++ /dev/null @@ -1,182 +0,0 @@ -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import Joy -from geometry_msgs.msg import Twist - - -class Curve: - """ - Class to handle different types of curves for joystick input. - """ - - def __init__(self, curve_type): - """ - Initialize the Curve object. - - :param curve_type: Type of curve to use (e.g., "linear"). - """ - self.curve = lambda x: x - self.set_curve(curve_type) - - def set_curve(self, curve_type): - """ - Set the type of curve to use. - - :param curve_type: Type of curve to use (e.g., "linear"). - """ - if curve_type == "linear": - self.curve = lambda x: x - else: - rclpy.logging.get_logger("joystick_controller").warn( - f"Failed to change curve type. Unknown curve {curve_type}." - ) - rclpy.logging.get_logger("joystick_controller").info( - "Curve options are: linear" - ) - - def __call__(self, x): - """ - Apply the curve to the input value. - - :param x: Input value. - :return: Curved value. - """ - return self.curve(x) - - -class ButtonLayout: - """ - Class to handle the button layout of the joystick. - """ - - def __init__(self, deadman_button, turbo_button, linear_axis, angular_axis): - """ - Initialize the ButtonLayout object. - - :param deadman_button: Index of the deadman button. - :param turbo_button: Index of the turbo button. - :param linear_axis: Index of the linear axis. - :param angular_axis: Index of the angular axis. - """ - self.deadman_button = deadman_button - self.turbo_button = turbo_button - self.linear_axis = linear_axis - self.angular_axis = angular_axis - - def deadman(self, msg): - """ - Check if the deadman button is pressed. - - :param msg: Joy message. - :return: True if the deadman button is pressed, False otherwise. - """ - return self.deadman_button < 0 or msg.buttons[self.deadman_button] == 1 - - def turbo(self, msg): - """ - Check if the turbo button is pressed. - - :param msg: Joy message. - :return: True if the turbo button is pressed, False otherwise. - """ - return self.turbo_button >= 0 and msg.buttons[self.turbo_button] == 1 - - def linear(self, msg): - """ - Get the linear axis value from the joystick message. - - :param msg: Joy message. - :return: Linear axis value. - """ - return msg.axes[self.linear_axis] - - def angular(self, msg): - """ - Get the angular axis value from the joystick message. - - :param msg: Joy message. - :return: Angular axis value. - """ - return msg.axes[self.angular_axis] - - -class JoystickController(Node): - """ - ROS2 Node to control the robot using a joystick. - """ - - def __init__(self): - """ - Initialize the JoystickController node. - """ - super().__init__("joystick_controller") - self.declare_parameter("linear_axis_index", 0) - linear_axis = ( - self.get_parameter("linear_axis_index").get_parameter_value().integer_value - ) - self.declare_parameter("turn_axis_index", 1) - turn_axis = ( - self.get_parameter("turn_axis_index").get_parameter_value().integer_value - ) - self.declare_parameter("turbo_button", -1) - turbo_button = ( - self.get_parameter("turbo_button").get_parameter_value().integer_value - ) - self.declare_parameter("deadman_button", -1) - deadman_button = ( - self.get_parameter("deadman_button").get_parameter_value().integer_value - ) - self.declare_parameter("max_linear_speed", 1.0) - self.max_linear_speed = ( - self.get_parameter("max_linear_speed").get_parameter_value().double_value - ) - self.declare_parameter("max_angular_speed", 1.0) - self.max_angular_speed = ( - self.get_parameter("max_angular_speed").get_parameter_value().double_value - ) - self.declare_parameter("turbo_multiplier", 2.0) - self.turbo_multiplier = ( - self.get_parameter("turbo_multiplier").get_parameter_value().double_value - ) - self.declare_parameter("curve_type", "linear") - curve_type = self.get_parameter("curve_type").get_parameter_value().string_value - - self.layout = ButtonLayout(deadman_button, turbo_button, linear_axis, turn_axis) - self.curve = Curve(curve_type) - - self.twist_pub = self.create_publisher(Twist, "/cmd_vel", 10) - self.joy_sub = self.create_subscription(Joy, "/joy", self.joy_callback, 10) - - def joy_callback(self, msg: Joy): - """ - Callback function to handle joystick messages. - - :param msg: Joy message. - """ - if not self.layout.deadman(msg): - return - - twist = Twist() - twist.linear.x = self.curve(self.layout.linear(msg)) * self.max_linear_speed - twist.angular.z = self.curve(self.layout.angular(msg)) * self.max_angular_speed - - if self.layout.turbo(msg): - twist.linear.x *= self.turbo_multiplier - twist.angular.z *= self.turbo_multiplier - - self.twist_pub.publish(twist) - - -def main(args=None): - """ - Main function to initialize and spin the ROS2 node. - """ - rclpy.init(args=args) - node = JoystickController() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/drive/drive/joystick_drive.py b/src/drive/drive/joystick_drive.py deleted file mode 100644 index 6b6241ac..00000000 --- a/src/drive/drive/joystick_drive.py +++ /dev/null @@ -1,126 +0,0 @@ -from math import radians -import rclpy -import rclpy.logging -from rclpy.node import Node - -import rclpy.time -from std_msgs.msg import Int8, Float32, Bool -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Twist -from sensor_msgs.msg import Joy -from interfaces.msg import LiveTune -from interfaces.msg import SixFloats -from interfaces.msg import PIDmsg - - -def map_range(value, old_min, old_max, new_min, new_max): - old_range = old_max - old_min - new_range = new_max - new_min - scaled_value = (value - old_min) / old_range - mapped_value = new_min + scaled_value * new_range - return mapped_value - - -class joystickDrive(Node): - def __init__(self): - super().__init__("JoystickNode") - self.twist = Twist() - self.estop = Bool() - self.estop.data = False - self.estopTimeout = Bool() - self.estopTimeout.data = False - self.lastTimestamp = 0 - - self.declare_parameter("PID", 0) - self.pidMode = self.get_parameter("PID").get_parameter_value().integer_value - self.declare_parameter("PID_max_speed", 1.0) - self.MAX_PID_SPEED = ( - self.get_parameter("PID_max_speed").get_parameter_value().double_value - ) - self.declare_parameter("PID_max_turn", 1.0) - self.MAX_PID_TURN = ( - self.get_parameter("PID_max_turn").get_parameter_value().double_value - ) - self.declare_parameter("voltage_max_speed", 1.0) - self.MAX_VOLTAGE_SPEED = ( - self.get_parameter("voltage_max_speed").get_parameter_value().double_value - ) - self.declare_parameter("voltage_max_turn", 1.0) - self.MAX_VOLTAGE_TURN = ( - self.get_parameter("voltage_max_turn").get_parameter_value().double_value - ) - self.active = True - - self.setTwistPub = self.create_publisher(Twist, "/drive/cmd_vel", 1) - self.setEstop = self.create_publisher(Bool, "/drive/estop", 1) - self.cmd_move_subscriber = self.create_subscription( - Joy, "/joystick/drive", self.cmd_joy_callback, 10 - ) - self.setEstop.publish(self.estop) # init as not estoped - - freq = 10 - self.rate = self.create_rate(freq) - period = 1 / freq - self.timer = self.create_timer(period, self.joystick_timeout_handler) - - def joystick_timeout_handler(self): - if Node.get_clock(self).now().seconds_nanoseconds()[0] - self.lastTimestamp > 2: - self.estopTimeout.data = True - self.setEstop.publish(self.estopTimeout) - elif Node.get_clock(self).now().seconds_nanoseconds()[ - 0 - ] - self.lastTimestamp <= 2 and (self.estopTimeout.data and not self.estop): - self.estopTimeout.data = False - self.setEstop.publish(self.estopTimeout) - - def cmd_joy_callback(self, msg: Joy): - if msg.buttons[7] == 1 and self.last_msg.buttons[7] == 0: - self.active = not self.active - self.last_msg = msg - - self.lastTimestamp = msg.header.stamp.sec - if msg.buttons[0] == 1: # able to change buttons later on - self.estop.data = True - self.twist.linear.x = 0.0 - self.twist.angular.z = 0.0 - self.setTwistPub.publish(self.twist) - self.setEstop.publish(self.estop) - return - if ( - msg.buttons[1] == 1 - ): # have 2 different buttons to avoid double presses not stopping - self.estop.data = False - self.twist.linear.x = 0.0 - self.twist.angular.z = 0.0 - self.setTwistPub.publish(self.twist) - self.setEstop.publish(self.estop) - return - - if self.pidMode == 1 and self.active: - self.twist.linear.x = map_range( - msg.axes[1], -1, 1, -self.MAX_PID_SPEED, self.MAX_PID_SPEED - ) - self.twist.angular.z = map_range( - msg.axes[0], -1, 1, -self.MAX_PID_TURN, self.MAX_PID_TURN - ) - self.setTwistPub.publish(self.twist) - elif self.active: - self.twist.linear.x = map_range( - msg.axes[1], -1, 1, -self.MAX_VOLTAGE_SPEED, self.MAX_VOLTAGE_SPEED - ) - self.twist.angular.z = map_range( - msg.axes[0], -1, 1, -self.MAX_VOLTAGE_TURN, self.MAX_VOLTAGE_TURN - ) - self.setTwistPub.publish(self.twist) - - -def main(args=None): - rclpy.init(args=args) - node = joystickDrive() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/drive/drive/roboclaw_driver.py b/src/drive/drive/roboclaw_driver.py deleted file mode 100755 index 3e390b63..00000000 --- a/src/drive/drive/roboclaw_driver.py +++ /dev/null @@ -1,1262 +0,0 @@ -import random -import serial -import time -import threading - -_trystimeout = 3 -__author__ = "bwbazemore@uga.edu (Brad Bazemore)" - - -# Command Enums - - -class Cmd: - M1FORWARD = 0 - M1BACKWARD = 1 - SETMINMB = 2 - SETMAXMB = 3 - M2FORWARD = 4 - M2BACKWARD = 5 - M17BIT = 6 - M27BIT = 7 - MIXEDFORWARD = 8 - MIXEDBACKWARD = 9 - MIXEDRIGHT = 10 - MIXEDLEFT = 11 - MIXEDFB = 12 - MIXEDLR = 13 - GETM1ENC = 16 - GETM2ENC = 17 - GETM1SPEED = 18 - GETM2SPEED = 19 - RESETENC = 20 - GETVERSION = 21 - SETM1ENCCOUNT = 22 - SETM2ENCCOUNT = 23 - GETMBATT = 24 - GETLBATT = 25 - SETMINLB = 26 - SETMAXLB = 27 - SETM1PID = 28 - SETM2PID = 29 - GETM1ISPEED = 30 - GETM2ISPEED = 31 - M1DUTY = 32 - M2DUTY = 33 - MIXEDDUTY = 34 - M1SPEED = 35 - M2SPEED = 36 - MIXEDSPEED = 37 - M1SPEEDACCEL = 38 - M2SPEEDACCEL = 39 - MIXEDSPEEDACCEL = 40 - M1SPEEDDIST = 41 - M2SPEEDDIST = 42 - MIXEDSPEEDDIST = 43 - M1SPEEDACCELDIST = 44 - M2SPEEDACCELDIST = 45 - MIXEDSPEEDACCELDIST = 46 - GETBUFFERS = 47 - GETPWMS = 48 - GETCURRENTS = 49 - MIXEDSPEED2ACCEL = 50 - MIXEDSPEED2ACCELDIST = 51 - M1DUTYACCEL = 52 - M2DUTYACCEL = 53 - MIXEDDUTYACCEL = 54 - READM1PID = 55 - READM2PID = 56 - SETMAINVOLTAGES = 57 - SETLOGICVOLTAGES = 58 - GETMINMAXMAINVOLTAGES = 59 - GETMINMAXLOGICVOLTAGES = 60 - SETM1POSPID = 61 - SETM2POSPID = 62 - READM1POSPID = 63 - READM2POSPID = 64 - M1SPEEDACCELDECCELPOS = 65 - M2SPEEDACCELDECCELPOS = 66 - MIXEDSPEEDACCELDECCELPOS = 67 - SETM1DEFAULTACCEL = 68 - SETM2DEFAULTACCEL = 69 - SETPINFUNCTIONS = 74 - GETPINFUNCTIONS = 75 - SETDEADBAND = 76 - GETDEADBAND = 77 - RESTOREDEFAULTS = 80 - GETTEMP = 82 - GETTEMP2 = 83 - GETERROR = 90 - GETENCODERMODE = 91 - SETM1ENCODERMODE = 92 - SETM2ENCODERMODE = 93 - WRITENVM = 94 - READNVM = 95 - SETCONFIG = 98 - GETCONFIG = 99 - SETM1MAXCURRENT = 133 - SETM2MAXCURRENT = 134 - GETM1MAXCURRENT = 135 - GETM2MAXCURRENT = 136 - SETPWMMODE = 148 - GETPWMMODE = 149 - FLAGBOOTLOADER = 255 - - -# Private Functions - - -def crc_clear(): - global _crc - _crc = 0 - return - - -def crc_update(data): - global _crc - _crc ^= data << 8 - for bit in range(0, 8): - if (_crc & 0x8000) == 0x8000: - _crc = (_crc << 1) ^ 0x1021 - else: - _crc <<= 1 - return - - -def _sendcommand(address, command): - crc_clear() - crc_update(address) - port.write(address.to_bytes(1, "big")) - crc_update(command) - port.write(command.to_bytes(1, "big")) - return - - -def _readchecksumword(): - data = port.read(2) - if len(data) == 2: - crc = (data[0] << 8) | data[1] - return 1, crc - return 0, 0 - - -def _readbyte(): - data = port.read(1) - if len(data): - val = ord(data) - crc_update(val) - return 1, val - return 0, 0 - - -def _readword(): - val1 = _readbyte() - if val1[0]: - val2 = _readbyte() - if val2[0]: - return 1, val1[1] << 8 | val2[1] - return 0, 0 - - -def _readlong(): - val1 = _readbyte() - if val1[0]: - val2 = _readbyte() - if val2[0]: - val3 = _readbyte() - if val3[0]: - val4 = _readbyte() - if val4[0]: - return 1, val1[1] << 24 | val2[1] << 16 | val3[1] << 8 | val4[1] - return 0, 0 - - -def _readslong(): - val = _readlong() - if val[0]: - if val[1] & 0x80000000: - return val[0], val[1] - 0x100000000 - return val[0], val[1] - return 0, 0 - - -def _writebyte(val): - crc_update(val & 0xFF) - port.write(val.to_bytes(1, "big")) - - -def _writesbyte(val): - _writebyte(val) - - -def _writeword(val): - _writebyte((val >> 8) & 0xFF) - _writebyte(val & 0xFF) - - -def _writesword(val): - _writeword(val) - - -def _writelong(val): - _writebyte((val >> 24) & 0xFF) - _writebyte((val >> 16) & 0xFF) - _writebyte((val >> 8) & 0xFF) - _writebyte(val & 0xFF) - - -def _writeslong(val): - _writelong(val) - - -def _read1(address, cmd): - global _crc - trys = _trystimeout - while 1: - port.flushInput() - _sendcommand(address, cmd) - val1 = _readbyte() - if val1[0]: - crc = _readchecksumword() - if crc[0]: - if _crc & 0xFFFF != crc[1] & 0xFFFF: - return 0, 0 - return 1, val1[1] - trys -= 1 - if trys == 0: - break - return 0, 0 - - -def _read2(address, cmd): - global _crc - trys = _trystimeout - while 1: - port.flushInput() - _sendcommand(address, cmd) - val1 = _readword() - if val1[0]: - crc = _readchecksumword() - if crc[0]: - if _crc & 0xFFFF != crc[1] & 0xFFFF: - return 0, 0 - return 1, val1[1] - trys -= 1 - if trys == 0: - break - return 0, 0 - - -def _read4(address, cmd): - global _crc - trys = _trystimeout - while 1: - port.flushInput() - _sendcommand(address, cmd) - val1 = _readlong() - if val1[0]: - crc = _readchecksumword() - if crc[0]: - if _crc & 0xFFFF != crc[1] & 0xFFFF: - return 0, 0 - return 1, val1[1] - trys -= 1 - if trys == 0: - break - return 0, 0 - - -def _read4_1(address, cmd): - global _crc - trys = _trystimeout - while 1: - port.flushInput() - _sendcommand(address, cmd) - val1 = _readslong() - if val1[0]: - val2 = _readbyte() - if val2[0]: - crc = _readchecksumword() - if crc[0]: - if _crc & 0xFFFF != crc[1] & 0xFFFF: - return 0, 0 - return 1, val1[1], val2[1] - trys -= 1 - if trys == 0: - break - return 0, 0 - - -def _read_n(address, cmd, args): - global _crc - trys = _trystimeout - while 1: - port.flushInput() - trys -= 1 - if trys == 0: - break - failed = False - _sendcommand(address, cmd) - data = [ - 1, - ] - for i in range(0, args): - val = _readlong() - if val[0] == 0: - failed = True - break - data.append(val[1]) - if failed: - continue - crc = _readchecksumword() - if crc[0]: - if _crc & 0xFFFF == crc[1] & 0xFFFF: - return data - return 0, 0, 0, 0, 0 - - -def _writechecksum(): - global _crc - _writeword(_crc & 0xFFFF) - val = _readbyte() - if val[0]: - return True - return False - - -def _write0(address, cmd): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write1(address, cmd, val): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writebyte(val) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write111(address, cmd, val1, val2): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writebyte(val1) - _writebyte(val2) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write111(address, cmd, val1, val2, val3): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writebyte(val1) - _writebyte(val2) - _writebyte(val3) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write2(address, cmd, val): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writeword(val) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _writeS2(address, cmd, val): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writesword(val) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write22(address, cmd, val1, val2): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writeword(val1) - _writeword(val2) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _writeS22(address, cmd, val1, val2): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writesword(val1) - _writeword(val2) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _writeS2S2(address, cmd, val1, val2): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writesword(val1) - _writesword(val2) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _writeS24(address, cmd, val1, val2): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writesword(val1) - _writelong(val2) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _writeS24S24(address, cmd, val1, val2, val3, val4): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writesword(val1) - _writelong(val2) - _writesword(val3) - _writelong(val4) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write4(address, cmd, val): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writelong(val) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _writeS4(address, cmd, val): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writeslong(val) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write44(address, cmd, val1, val2): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writelong(val1) - _writelong(val2) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write4S4(address, cmd, val1, val2): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writelong(val1) - _writeslong(val2) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _writeS4S4(address, cmd, val1, val2): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writeslong(val1) - _writeslong(val2) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write441(address, cmd, val1, val2, val3): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writelong(val1) - _writelong(val2) - _writebyte(val3) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _writeS441(address, cmd, val1, val2, val3): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writeslong(val1) - _writelong(val2) - _writebyte(val3) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write4S4S4(address, cmd, val1, val2, val3): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writelong(val1) - _writeslong(val2) - _writeslong(val3) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write4S441(address, cmd, val1, val2, val3, val4): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writelong(val1) - _writeslong(val2) - _writelong(val3) - _writebyte(val4) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write4444(address, cmd, val1, val2, val3, val4): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writelong(val1) - _writelong(val2) - _writelong(val3) - _writelong(val4) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write4S44S4(address, cmd, val1, val2, val3, val4): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writelong(val1) - _writeslong(val2) - _writelong(val3) - _writeslong(val4) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write44441(address, cmd, val1, val2, val3, val4, val5): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writelong(val1) - _writelong(val2) - _writelong(val3) - _writelong(val4) - _writebyte(val5) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _writeS44S441(address, cmd, val1, val2, val3, val4, val5): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writeslong(val1) - _writelong(val2) - _writeslong(val3) - _writelong(val4) - _writebyte(val5) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write4S44S441(address, cmd, val1, val2, val3, val4, val5, val6): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writelong(val1) - _writeslong(val2) - _writelong(val3) - _writeslong(val4) - _writelong(val5) - _writebyte(val6) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write4S444S441(address, cmd, val1, val2, val3, val4, val5, val6, val7): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writelong(val1) - _writeslong(val2) - _writelong(val3) - _writelong(val4) - _writeslong(val5) - _writelong(val6) - _writebyte(val7) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write4444444(address, cmd, val1, val2, val3, val4, val5, val6, val7): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writelong(val1) - _writelong(val2) - _writelong(val3) - _writelong(val4) - _writelong(val5) - _writelong(val6) - _writelong(val7) - if _writechecksum(): - return True - trys -= 1 - return False - - -def _write444444441(address, cmd, val1, val2, val3, val4, val5, val6, val7, val8, val9): - trys = _trystimeout - while trys: - _sendcommand(address, cmd) - _writelong(val1) - _writelong(val2) - _writelong(val3) - _writelong(val4) - _writelong(val5) - _writelong(val6) - _writelong(val7) - _writelong(val8) - _writebyte(val9) - if _writechecksum(): - return True - trys -= 1 - return False - - -# User accessible functions - - -def SendRandomData(cnt): - for i in range(0, cnt): - byte = random.getrandbits(8) - port.write(byte.to_bytes(1, "big")) - return - - -def ForwardM1(address, val): - return _write1(address, Cmd.M1FORWARD, val) - - -def BackwardM1(address, val): - return _write1(address, Cmd.M1BACKWARD, val) - - -def SetMinVoltageMainBattery(address, val): - return _write1(address, Cmd.SETMINMB, val) - - -def SetMaxVoltageMainBattery(address, val): - return _write1(address, Cmd.SETMAXMB, val) - - -def ForwardM2(address, val): - return _write1(address, Cmd.M2FORWARD, val) - - -def BackwardM2(address, val): - return _write1(address, Cmd.M2BACKWARD, val) - - -def ForwardBackwardM1(address, val): - return _write1(address, Cmd.M17BIT, val) - - -def ForwardBackwardM2(address, val): - return _write1(address, Cmd.M27BIT, val) - - -def ForwardMixed(address, val): - return _write1(address, Cmd.MIXEDFORWARD, val) - - -def BackwardMixed(address, val): - return _write1(address, Cmd.MIXEDBACKWARD, val) - - -def TurnRightMixed(address, val): - return _write1(address, Cmd.MIXEDRIGHT, val) - - -def TurnLeftMixed(address, val): - return _write1(address, Cmd.MIXEDLEFT, val) - - -def ForwardBackwardMixed(address, val): - return _write1(address, Cmd.MIXEDFB, val) - - -def LeftRightMixed(address, val): - return _write1(address, Cmd.MIXEDLR, val) - - -def ReadEncM1(address): - return _read4_1(address, Cmd.GETM1ENC) - - -def ReadEncM2(address): - return _read4_1(address, Cmd.GETM2ENC) - - -def ReadSpeedM1(address): - return _read4_1(address, Cmd.GETM1SPEED) - - -def ReadSpeedM2(address): - return _read4_1(address, Cmd.GETM2SPEED) - - -def ResetEncoders(address): - return _write0(address, Cmd.RESETENC) - - -def ReadVersion(address): - global _crc - trys = _trystimeout - while 1: - port.flushInput() - _sendcommand(address, Cmd.GETVERSION) - str = "" - passed = True - for i in range(0, 48): - data = port.read(1) - if len(data): - val = ord(data) - crc_update(val) - if val == 0: - break - str += chr(data[0]) - else: - passed = False - break - if passed: - crc = _readchecksumword() - if crc[0]: - if _crc & 0xFFFF == crc[1] & 0xFFFF: - return 1, str - else: - time.sleep(0.01) - trys -= 1 - if trys == 0: - break - return 0, 0 - - -def SetEncM1(address, cnt): - return _write4(address, Cmd.SETM1ENCCOUNT, cnt) - - -def SetEncM2(address, cnt): - return _write4(address, Cmd.SETM2ENCCOUNT, cnt) - - -def ReadMainBatteryVoltage(address): - return _read2(address, Cmd.GETMBATT) - - -def ReadLogicBatteryVoltage( - address, -): - return _read2(address, Cmd.GETLBATT) - - -def SetMinVoltageLogicBattery(address, val): - return _write1(address, Cmd.SETMINLB, val) - - -def SetMaxVoltageLogicBattery(address, val): - return _write1(address, Cmd.SETMAXLB, val) - - -def SetM1VelocityPID(address, p, i, d, qpps): - return _write4444( - address, Cmd.SETM1PID, int(d * 65536), int(p * 65536), int(i * 65536), qpps - ) - - -def SetM2VelocityPID(address, p, i, d, qpps): - return _write4444( - address, Cmd.SETM2PID, int(d * 65536), int(p * 65536), int(i * 65536), qpps - ) - - -def ReadISpeedM1(address): - return _read4_1(address, Cmd.GETM1ISPEED) - - -def ReadISpeedM2(address): - return _read4_1(address, Cmd.GETM2ISPEED) - - -def DutyM1(address, val): - return _writeS2(address, Cmd.M1DUTY, val) - - -def DutyM2(address, val): - return _writeS2(address, Cmd.M2DUTY, val) - - -def DutyM1M2(address, m1, m2): - return _writeS2S2(address, Cmd.MIXEDDUTY, m1, m2) - - -def SpeedM1(address, val): - return _writeS4(address, Cmd.M1SPEED, val) - - -def SpeedM2(address, val): - return _writeS4(address, Cmd.M2SPEED, val) - - -def SpeedM1M2(address, m1, m2): - return _writeS4S4(address, Cmd.MIXEDSPEED, m1, m2) - - -def SpeedAccelM1(address, accel, speed): - return _write4S4(address, Cmd.M1SPEEDACCEL, accel, speed) - - -def SpeedAccelM2(address, accel, speed): - return _write4S4(address, Cmd.M2SPEEDACCEL, accel, speed) - - -def SpeedAccelM1M2(address, accel, speed1, speed2): - return _write4S4S4(address, Cmd.M1SPEEDACCEL, accel, speed1, speed2) - - -def SpeedDistanceM1(address, speed, distance, buffer): - return _writeS441(address, Cmd.M1SPEEDDIST, speed, distance, buffer) - - -def SpeedDistanceM2(address, speed, distance, buffer): - return _writeS441(address, Cmd.M2SPEEDDIST, speed, distance, buffer) - - -def SpeedDistanceM1M2(address, speed1, distance1, speed2, distance2, buffer): - return _writeS44S441( - address, Cmd.MIXEDSPEEDDIST, speed1, distance1, speed2, distance2, buffer - ) - - -def SpeedAccelDistanceM1(address, accel, speed, distance, buffer): - return _write4S441(address, Cmd.M1SPEEDACCELDIST, accel, speed, distance, buffer) - - -def SpeedAccelDistanceM2(address, accel, speed, distance, buffer): - return _write4S441(address, Cmd.M2SPEEDACCELDIST, accel, speed, distance, buffer) - - -def SpeedAccelDistanceM1M2( - address, accel, speed1, distance1, speed2, distance2, buffer -): - return _write4S44S441( - address, - Cmd.MIXEDSPEED2ACCELDIST, - accel, - speed1, - distance1, - speed2, - distance2, - buffer, - ) - - -def ReadBuffers(address): - val = _read2(address, Cmd.GETBUFFERS) - if val[0]: - return 1, val[1] >> 8, val[1] & 0xFF - return 0, 0, 0 - - -def ReadPWMs(address): - val = _read4(address, Cmd.GETPWMS) - if val[0]: - pwm1 = val[1] >> 16 - pwm2 = val[1] & 0xFFFF - if pwm1 & 0x8000: - pwm1 -= 0x10000 - if pwm2 & 0x8000: - pwm2 -= 0x10000 - return 1, pwm1, pwm2 - return 0, 0, 0 - - -def ReadCurrents(address): - val = _read4(address, Cmd.GETCURRENTS) - if val[0]: - cur1 = val[1] >> 16 - cur2 = val[1] & 0xFFFF - if cur1 & 0x8000: - cur1 -= 0x10000 - if cur2 & 0x8000: - cur2 -= 0x10000 - return 1, cur1, cur2 - return 0, 0, 0 - - -def SpeedAccelM1M2_2(address, accel1, speed1, accel2, speed2): - return _write4S44S4(address, Cmd.MIXEDSPEED2ACCEL, accel, speed1, accel2, speed2) - - -def SpeedAccelDistanceM1M2_2( - address, accel1, speed1, distance1, accel2, speed2, distance2, buffer -): - return _write4S444S441( - address, - Cmd.MIXEDSPEED2ACCELDIST, - accel1, - speed1, - distance1, - accel2, - speed2, - distance2, - buffer, - ) - - -def DutyAccelM1(address, accel, duty): - return _writeS24(address, Cmd.M1DUTYACCEL, duty, accel) - - -def DutyAccelM2(address, accel, duty): - return _writeS24(address, Cmd.M2DUTYACCEL, duty, accel) - - -def DutyAccelM1M2(address, accel1, duty1, accel2, duty2): - return _writeS24S24(Cmd.MIXEDDUTYACCEL, duty1, accel1, duty2, accel2) - - -def ReadM1VelocityPID(address): - data = _read_n(address, Cmd.READM1PID, 4) - if data[0]: - data[1] /= 65536.0 - data[2] /= 65536.0 - data[3] /= 65536.0 - return data - return 0, 0, 0, 0, 0 - - -def ReadM2VelocityPID(address): - data = _read_n(address, Cmd.READM2PID, 4) - if data[0]: - data[1] /= 65536.0 - data[2] /= 65536.0 - data[3] /= 65536.0 - return data - return 0, 0, 0, 0, 0 - - -def SetMainVoltages(address, min, max): - return _write22(address, Cmd.SETMAINVOLTAGES, min, max) - - -def SetLogicVoltages(address, min, max): - return _write22(address, Cmd.SETLOGICVOLTAGES, min, max) - - -def ReadMinMaxMainVoltages(address): - val = _read4(address, Cmd.GETMINMAXMAINVOLTAGES) - if val[0]: - min = val[1] >> 16 - max = val[1] & 0xFFFF - return 1, min, max - return 0, 0, 0 - - -def ReadMinMaxLogicVoltages(address): - val = _read4(address, Cmd.GETMINMAXLOGICVOLTAGES) - if val[0]: - min = val[1] >> 16 - max = val[1] & 0xFFFF - return 1, min, max - return 0, 0, 0 - - -def SetM1PositionPID(address, kp, ki, kd, kimax, deadzone, min, max): - return _write4444444( - address, - Cmd.SETM1POSPID, - int(kd * 1024), - int(kp * 1024), - int(ki * 1024), - kimax, - deadzone, - min, - max, - ) - - -def SetM2PositionPID(address, kp, ki, kd, kimax, deadzone, min, max): - return _write4444444( - address, - Cmd.SETM2POSPID, - int(kd * 1024), - int(kp * 1024), - int(ki * 1024), - kimax, - deadzone, - min, - max, - ) - - -def ReadM1PositionPID(address): - data = _read_n(address, Cmd.READM1POSPID, 7) - if data[0]: - data[0] /= 1024.0 - data[1] /= 1024.0 - data[2] /= 1024.0 - return data - return 0, 0, 0, 0, 0, 0, 0, 0 - - -def ReadM2PositionPID(address): - data = _read_n(address, Cmd.READM2POSPID, 7) - if data[0]: - data[0] /= 1024.0 - data[1] /= 1024.0 - data[2] /= 1024.0 - return data - return 0, 0, 0, 0, 0, 0, 0, 0 - - -def SpeedAccelDeccelPositionM1(address, accel, speed, deccel, position, buffer): - return _write44441( - address, Cmd.M1SPEEDACCELDECCELPOS, accel, speed, deccel, position, buffer - ) - - -def SpeedAccelDeccelPositionM2(address, accel, speed, deccel, position, buffer): - return _write44441( - address, Cmd.M2SPEEDACCELDECCELPOS, accel, speed, deccel, position, buffer - ) - - -def SpeedAccelDeccelPositionM1M2( - address, - accel1, - speed1, - deccel1, - position1, - accel2, - speed2, - deccel2, - position2, - buffer, -): - return _write444444441( - address, - Cmd.MIXEDSPEEDACCELDECCELPOS, - accel1, - speed1, - deccel1, - position1, - accel2, - speed2, - deccel2, - position2, - buffer, - ) - - -def SetM1DefaultAccel(address, accel): - return _write4(address, Cmd.SETM1DEFAULTACCEL, accel) - - -def SetM2DefaultAccel(address, accel): - return _write4(address, Cmd.SETM2DEFAULTACCEL, accel) - - -def SetPinFunctions(address, S3mode, S4mode, S5mode): - return _write111(address, Cmd.SETPINFUNCTIONS, S3mode, S4mode, S5mode) - - -def ReadPinFunctions(address): - global _crc - trys = _trystimeout - while 1: - _sendcommand(address, Cmd.GETPINFUNCTIONS) - val1 = _readbyte() - if val1[0]: - val2 = _readbyte() - if val1[0]: - val3 = _readbyte() - if val1[0]: - crc = _readchecksumword() - if crc[0]: - if _crc & 0xFFFF != crc[1] & 0xFFFF: - return 0, 0 - return 1, val1[1], val2[1], val3[1] - trys -= 1 - if trys == 0: - break - return 0, 0 - - -def SetDeadBand(address, min, max): - return _write111(address, Cmd.SETDEADBAND, min, max) - - -def GetDeadBand(address): - val = _read2(address, Cmd.GETDEADBAND) - if val[0]: - return 1, val[1] >> 8, val[1] & 0xFF - return 0, 0, 0 - - -# Warning(TTL Serial): Baudrate will change if not already set to 38400. Communications will be lost -def RestoreDefaults(address): - return _write0(address, Cmd.RESTOREDEFAULTS) - - -def ReadTemp(address): - return _read2(address, Cmd.GETTEMP) - - -def ReadTemp2(address): - return _read2(address, Cmd.GETTEMP2) - - -def ReadError(address): - return _read2(address, Cmd.GETERROR) - - -def ReadEncoderModes(address): - val = _read2(address, Cmd.GETENCODERMODE) - if val[0]: - return 1, val[1] >> 8, val[1] & 0xFF - return 0, 0, 0 - - -def SetM1EncoderMode(address, mode): - return _write1(address, Cmd.SETM1ENCODERMODE, mode) - - -def SetM2EncoderMode(address, mode): - return _write1(address, Cmd.SETM2ENCODERMODE, mode) - - -# saves active settings to NVM -def WriteNVM(address): - return _write4(address, Cmd.WRITENVM, 0xE22EAB7A) - - -# restores settings from NVM -# Warning(TTL Serial): If baudrate changes or the control mode changes communications will be lost -def ReadNVM(address): - return _write0(address, Cmd.READNVM) - - -# Warning(TTL Serial): If control mode is changed from packet serial mode -# when setting config communications will be lost! -# Warning(TTL Serial): If baudrate of packet serial mode is changed communications will be lost! -def SetConfig(address, config): - return _write2(address, Cmd.SETCONFIG, config) - - -def GetConfig(address): - return _read2(address, Cmd.GETCONFIG) - - -def SetM1MaxCurrent(address, max): - return _write44(address, Cmd.SETM1MAXCURRENT, max, 0) - - -def SetM2MaxCurrent(address, max): - return _write44(address, Cmd.SETM2MAXCURRENT, max, 0) - - -def ReadM1MaxCurrent(address): - data = _read_n(address, Cmd.GETM1MAXCURRENT, 2) - if data[0]: - return 1, data[1] - return 0, 0 - - -def ReadM2MaxCurrent(address): - data = _read_n(address, Cmd.GETM2MAXCURRENT, 2) - if data[0]: - return 1, data[1] - return 0, 0 - - -def SetPWMMode(address, mode): - return _write1(address, Cmd.SETPWMMODE, mode) - - -def ReadPWMMode(address): - return _read1(address, Cmd.GETPWMMODE) - - -def Open(comport, rate): - global port - port = serial.Serial(comport, baudrate=rate, timeout=0.1, interCharTimeout=0.01) - return diff --git a/src/drive/drive/roboclaw_node.py b/src/drive/drive/roboclaw_node.py deleted file mode 100755 index 72cdd27d..00000000 --- a/src/drive/drive/roboclaw_node.py +++ /dev/null @@ -1,391 +0,0 @@ -#!/usr/bin/env python -import diagnostic_updater -import rclpy -from geometry_msgs.msg import Twist -from nav_msgs.msg import Odometry -from rclpy.node import Node -from sensor_msgs.msg import BatteryState -from std_msgs.msg import Float64, Bool - -from . import roboclaw_driver as roboclaw -from . import utils as u -from .electrical_wrapper import ElectricalWrapper -from .encoder_wrapper import EncoderWrapper - - -class Movement: - """Movement class - responsible of running the RoboClaw""" - - def __init__( - self, - address, - max_speed, - base_width, - ticks_per_meter, - ticks_per_rotation, - parent_clock, - parent_logger, - ): - self.twist = None - self.address = address - self.MAX_SPEED = max_speed - self.BASE_WIDTH = base_width - self.TICKS_PER_METER = ticks_per_meter - self.TICKS_PER_ROTATION = ticks_per_rotation - self.clock = parent_clock - self.logger = parent_logger - self.last_set_speed_time = self.clock.now().nanoseconds - self.vr_ticks = 0 - self.vl_ticks = 0 - self.stopped = True - - def run(self): # DRIVE LOGIC - if self.stopped is True: - try: - roboclaw.ForwardM1(self.address, 0) - roboclaw.ForwardM2(self.address, 0) - except OSError as e: - self.logger.warn("Stopping error: " + str(e.errno)) - self.logger.debug(e) - return - if self.twist is None: - return - if self.twist.linear.x != 0 or self.twist.angular.z != 0: - self.stopped = False - if self.twist is None or self.stopped is True: - roboclaw.ForwardM1(self.address, 0) - roboclaw.ForwardM2(self.address, 0) - return - - linear_x = self.twist.linear.x - if linear_x > self.MAX_SPEED: # check for messages above speed limit - linear_x = self.MAX_SPEED - if linear_x < -self.MAX_SPEED: - linear_x = -self.MAX_SPEED - - # vr and vl are how fast the velocity on the left and right side is in m/s - vr = linear_x - self.twist.angular.z * self.BASE_WIDTH / 2 # m/s - vl = linear_x + self.twist.angular.z * self.BASE_WIDTH / 2 - self.twist = None - - # ticks convert the speed the wheel needs to go into encoder ticks per second - vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s - vl_ticks = int(vl * self.TICKS_PER_METER) - - self.logger.debug("vr_ticks: " + str(vr_ticks) + "vl_ticks: " + str(vl_ticks)) - - try: - # This is a hack way to keep a poorly tuned PID from making noise at speed 0 - if vr_ticks == 0 and vl_ticks == 0: - roboclaw.ForwardM1(self.address, 0) - roboclaw.ForwardM2(self.address, 0) - ####### PID DRIVE ####### - else: - roboclaw.SpeedM1M2(self.address, -vr_ticks, vl_ticks) - self.logger.info( - "tryng PID vr = " + str(-vr_ticks) + " vl = " + str(vl_ticks) - ) - - ####### VOLTAGE DRIVE ####### - # else: - # dutyCycle1 = int(vr / 12 * 32767) #mainBatteryVoltage * 32767 - # dutyCycle2 = int(vl / 12 * 32767) - # roboclaw.DutyM1M2(self.address, -dutyCycle1, dutyCycle2) - except OSError as e: - self.logger.warn("SpeedM1M2 OSError: " + str(e.errno)) - self.logger.debug(e) - - -class RoboclawNode(Node): - """RoboClaw Node""" - - def __init__(self): - super().__init__("roboclaw_node") - self.ERRORS = u.ROBOCLAW_ERRORS - - freq = 30 - self.rate = self.create_rate(freq) - period = 1 / freq - self.timer = self.create_timer(period, self.run) - - self.get_logger().info("Connecting to roboclaw") - - # initialzie default connection params - self.declare_parameter("dev", "/dev/ttyACM0") - dev_name = self.get_parameter("dev").get_parameter_value().string_value - self.get_logger().info(dev_name) - - self.declare_parameter("baud", 115200) - baud_rate = self.get_parameter("baud").get_parameter_value().integer_value - - self.declare_parameter("address", 128) - self.address = self.get_parameter("address").get_parameter_value().integer_value - if self.address > 0x87 or self.address < 0x80: - self.get_logger().fatal("Address out of range") - self.shutdown("Address out of range") - - # TODO need someway to check if address is correct - try: - roboclaw.Open(dev_name, baud_rate) - except Exception as e: - self.get_logger().fatal("Could not connect to RoboClaw") - self.get_logger().debug(e) - self.shutdown("Could not connect to RoboClaw") - - self.updater = diagnostic_updater.Updater(self) - self.updater.setHardwareID("RoboClaw") - self.updater.add( - diagnostic_updater.FunctionDiagnosticTask("Vitals", self.check_vitals) - ) - try: - version = roboclaw.ReadVersion(self.address) - except Exception as e: - self.get_logger().warn("Problem getting RoboClaw version") - self.get_logger().warn(e) - pass - - if not version[0]: - self.get_logger().warn("Could not get version from RoboClaw") - else: - self.get_logger().debug(repr(version[1])) - - roboclaw.SpeedM1M2(self.address, 0, 0) # set roboclaws to default - roboclaw.ResetEncoders(self.address) - - # Get launch parameters - self.declare_parameter("max_speed", 2.0) - self.MAX_SPEED = ( - self.get_parameter("max_speed").get_parameter_value().double_value - ) - self.declare_parameter("ticks_per_meter", 819) - self.TICKS_PER_METER = ( - self.get_parameter("ticks_per_meter").get_parameter_value().integer_value - ) - self.declare_parameter("ticks_per_rotation", 1024) - self.TICKS_PER_ROTATION = ( - self.get_parameter("ticks_per_rotation").get_parameter_value().integer_value - ) - self.declare_parameter("base_width", 0.9144) - self.BASE_WIDTH = ( - self.get_parameter("base_width").get_parameter_value().double_value - ) - self.declare_parameter("pub_odom", True) - self.PUB_ODOM = self.get_parameter("pub_odom").get_parameter_value().bool_value - self.declare_parameter("pub_elec", True) - self.PUB_ELEC = self.get_parameter("pub_elec").get_parameter_value().bool_value - self.declare_parameter("stop_movement", True) - self.STOP_MOVEMENT = ( - self.get_parameter("stop_movement").get_parameter_value().bool_value - ) - - self.encodm = None - self.electr = None - if self.PUB_ELEC: - self.left_elec_pub = self.create_publisher( - BatteryState, dev_name + "/roboclaw/elec/left", 1 - ) - self.right_elec_pub = self.create_publisher( - BatteryState, dev_name + "/roboclaw/elec/right", 1 - ) - self.electr = ElectricalWrapper(self) - - if self.PUB_ODOM: # publish odometry - self.odom_pub = self.create_publisher( - Odometry, dev_name + "/odom_roboclaw", 1 - ) - self.left_encoder_pub = self.create_publisher( - Float64, dev_name + "/left_encoder_angular_velocity", 1 - ) - self.right_encoder_pub = self.create_publisher( - Float64, dev_name + "/right_encoder_angular_velocity", 1 - ) - self.encodm = EncoderWrapper( - self.TICKS_PER_METER, - self.TICKS_PER_ROTATION, - self.BASE_WIDTH, - self, - ) - self.movement = Movement( # create movement class with parameters - self.address, - self.MAX_SPEED, - self.BASE_WIDTH, - self.TICKS_PER_METER, - self.TICKS_PER_ROTATION, - self.get_clock(), - self.get_logger(), - ) - self.last_set_speed_time = self.get_clock().now().nanoseconds - # subscriptions - self.cmd_vel_sub = self.create_subscription( - Twist, "/drive/cmd_vel", self.cmd_vel_callback, 1 - ) - self.cmd_estop_sub = self.create_subscription( - Bool, "/drive/estop", self.cmd_estop_callback, 1 - ) - self.get_clock().sleep_for(rclpy.duration.Duration(seconds=1.0)) - - def run(self): # Roboclaw run loop - # stop movement if robot doesn't recieve commands for 1 sec - if ( - self.STOP_MOVEMENT - and not self.movement.stopped - and self.get_clock().now().nanoseconds - self.movement.last_set_speed_time - > 10e9 - ): - self.get_logger().info("Did not get command for 1 second, stopping") - try: - roboclaw.ForwardM1(self.address, 0) - roboclaw.ForwardM2(self.address, 0) - except OSError as e: - self.get_logger().error("Could not stop") - self.get_logger().debug(e) - self.movement.stopped = True - - # TODO need find solution to the OSError11 looks like sync problem with serial - # Read encoders - status1, enc1, crc1 = None, None, None - status2, enc2, crc2 = None, None, None - - # log and publish - try: - status1, enc1, crc1 = roboclaw.ReadEncM1(self.address) - except ValueError: - pass - except OSError as e: - self.get_logger().warn("ReadEncM1 OSError: " + str(e.errno)) - self.get_logger().debug(e) - - try: - status2, enc2, crc2 = roboclaw.ReadEncM2(self.address) - except ValueError: - pass - except OSError as e: - self.get_logger().warn("ReadEncM2 OSError: " + str(e.errno)) - self.get_logger().debug(e) - - if self.PUB_ELEC: - try: - elec_data = self.poll_elec() - except ValueError: - pass - except OSError as e: - self.get_logger().warn("Electrical OSError: " + str(e.errno)) - self.get_logger().debug(e) - - has_enc1 = "enc1" in vars() - has_enc2 = "enc2" in vars() - has_encoders = has_enc1 and has_enc2 and enc1 and enc2 - - publish_time = self.get_clock().now() - - if has_encoders: - self.get_logger().debug(" Encoders " + str(enc1) + " " + str(enc2)) - if self.encodm: - # Left motor encoder : M2 / Right motor encoder : M1 - self.encodm.update_n_publish(enc2, enc1, publish_time) - # self.encodm.update_n_publish(enc1, enc2) - self.updater.update() - - if self.electr: - self.electr.publish_elec(publish_time, elec_data) - - # self.get_logger().info("Update done moving if cmd") - - self.movement.run() # run movement loop - - def poll_elec(self) -> dict: - """Read motors electrical data""" - elec_data = {} - - status, *currents = roboclaw.ReadCurrents(self.address) - self.log_elec(status, "currents") - elec_data["current"] = { - sd: curr / 100 for sd, curr in zip(("left", "right"), currents) - } - - status, voltage = roboclaw.ReadMainBatteryVoltage(self.address) - self.log_elec(status, "voltages") - elec_data["voltage"] = voltage / 10 - - status, logicbatt = roboclaw.ReadLogicBatteryVoltage(self.address) - self.log_elec(status, "logic voltages") - elec_data["logicbatt"] = logicbatt / 10 - - status, *pwms = roboclaw.ReadPWMs(self.address) - self.log_elec(status, "PWMs") - elec_data["pwm"] = { - sd: pwm / 327.67 for sd, pwm in zip(("left", "right"), pwms) - } - - status, temperature = roboclaw.ReadTemp(self.address) - self.log_elec(status, "temperature") - elec_data["temperature"] = temperature / 10 - - return elec_data - - def log_elec(self, status: int, name: str): - if status: - # self.get_logger().info(f"Got {name.lower()} data") - pass - else: - self.get_logger().info(f"Missing {name.lower()}") - - def cmd_vel_callback(self, twist): - self.movement.last_set_speed_time = self.get_clock().now().nanoseconds - self.movement.twist = twist - - def cmd_estop_callback(self, stopped): - self.movement.stopped = stopped.data - - # TODO: Need to make this work when more than one error is raised - def check_vitals(self, stat): - try: - status = roboclaw.ReadError(self.address)[1] - except OSError as e: - self.get_logger().warn("Diagnostics OSError: " + str(e.errno)) - self.get_logger().debug(e) - return - state, message = self.ERRORS[status] - stat.summary(state, message) - try: - stat.add( - "Main Batt V:", - str(float(roboclaw.ReadMainBatteryVoltage(self.address)[1] / 10)), - ) - stat.add( - "Logic Batt V:", - str(float(roboclaw.ReadLogicBatteryVoltage(self.address)[1] / 10)), - ) - stat.add("Temp1 C:", str(float(roboclaw.ReadTemp(self.address)[1] / 10))) - stat.add("Temp2 C:", str(float(roboclaw.ReadTemp2(self.address)[1] / 10))) - except OSError as e: - self.get_logger().warn("Diagnostics OSError: " + str(e.errno)) - self.get_logger().debug(e) - return stat - - # TODO: need clean shutdown so motors stop even if new msgs are arriving - def shutdown(self, str_msg): - self.get_logger().info("Shutting down :" + str_msg) - try: - roboclaw.ForwardM1(self.address, 0) - roboclaw.ForwardM2(self.address, 0) - except OSError: - self.get_logger().error("Shutdown did not work trying again") - try: - roboclaw.ForwardM1(self.address, 0) - roboclaw.ForwardM2(self.address, 0) - except OSError as e: - self.get_logger().error("Could not shutdown motors!!!!") - self.get_logger().debug(e) - - -def main(args=None): - rclpy.init(args=args) - roboclaw_node = RoboclawNode() - rclpy.spin(roboclaw_node) - roboclaw_node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/drive/drive/utils.py b/src/drive/drive/utils.py deleted file mode 100644 index b693c822..00000000 --- a/src/drive/drive/utils.py +++ /dev/null @@ -1,36 +0,0 @@ -import diagnostic_msgs - -ROBOCLAW_ERRORS = { - 0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"), - 0x0001: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"), - 0x0002: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"), - 0x0004: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"), - 0x0008: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"), - 0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature2"), - 0x0020: ( - diagnostic_msgs.msg.DiagnosticStatus.ERROR, - "Main batt voltage high", - ), - 0x0040: ( - diagnostic_msgs.msg.DiagnosticStatus.ERROR, - "Logic batt voltage high", - ), - 0x0080: ( - diagnostic_msgs.msg.DiagnosticStatus.ERROR, - "Logic batt voltage low", - ), - 0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 driver fault"), - 0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 driver fault"), - 0x0400: ( - diagnostic_msgs.msg.DiagnosticStatus.WARN, - "Main batt voltage high", - ), - 0x0800: ( - diagnostic_msgs.msg.DiagnosticStatus.WARN, - "Main batt voltage low", - ), - 0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature1"), - 0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN, "Temperature2"), - 0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"), - 0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home"), -} diff --git a/src/drive/launch/flightstick_drive.launch.py b/src/drive/launch/flightstick_drive.launch.py deleted file mode 100644 index 3ed41874..00000000 --- a/src/drive/launch/flightstick_drive.launch.py +++ /dev/null @@ -1,54 +0,0 @@ -import launch -import launch_ros.actions -from launch.substitutions import LaunchConfiguration -from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -import os -from ament_index_python.packages import get_package_share_directory -from launch.conditions import IfCondition - - -# Make launch configeration, defult false, launch talon on rover. Defult True for xbox -def generate_launch_description(): - """Generate launch description with multiple components.""" - pkg_drive = get_package_share_directory("drive") - pkg_arm = get_package_share_directory("arm_srdf") - parameters_file = os.path.join(pkg_drive, "config", "pxn.yaml") - launch_backend = LaunchConfiguration("launch_backend", default="False") - launch_backend_cmd = DeclareLaunchArgument( - "launch_backend", - default_value="False", - description="Launch the motor controller if True", - ) - - backend_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_drive, "launch", "talon.launch.py") - ), - condition=IfCondition(launch_backend), - ) - arm_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_arm, "launch", "talon.launch.py") - ), - condition=IfCondition(launch_backend), - ) - - return launch.LaunchDescription( - [ - launch_backend_cmd, - backend_cmd, - arm_cmd, - launch_ros.actions.Node( - package="joy", executable="joy_node", name="joystick" - ), - launch_ros.actions.Node( - package="joystick_control", - executable="flightstick_control", - name="flightstick_control", - output="screen", - parameters=[parameters_file], - ), - ] - ) diff --git a/src/drive/launch/xbox_drive.launch.py b/src/drive/launch/xbox_drive.launch.py deleted file mode 100644 index 6d303cd2..00000000 --- a/src/drive/launch/xbox_drive.launch.py +++ /dev/null @@ -1,47 +0,0 @@ -import launch -import launch_ros.actions -from launch.substitutions import LaunchConfiguration -from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -import os -from ament_index_python.packages import get_package_share_directory -from launch.conditions import IfCondition - - -def generate_launch_description(): - """Generate launch description with multiple components.""" - pkg_drive = get_package_share_directory("drive") - launch_backend = LaunchConfiguration("launch_backend", default="True") - launch_backend_cmd = DeclareLaunchArgument( - "launch_backend", - default_value="True", - description="Launch the motor controller if True", - ) - - backend_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_drive, "launch", "talon.launch.py") - ), - condition=IfCondition(launch_backend), - ) - - return launch.LaunchDescription( - [ - launch_backend_cmd, - backend_cmd, - launch_ros.actions.Node( - package="joy", executable="joy_node", name="joystick" - ), - launch_ros.actions.Node( - package="drive", - executable="joystick_controller", - name="joystick_controller", - parameters=[ - {"linear_axis_index": 3}, - {"turn_axis_index": 2}, - {"max_linear_speed": 2.0}, - ], - ), - ] - ) diff --git a/src/drive/package.xml b/src/drive/package.xml deleted file mode 100644 index 155469f7..00000000 --- a/src/drive/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - drive - 0.0.0 - TODO: Package description - chris - TODO: License declaration - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - geometry_msgs - nav_msgs - rclpy - std_msgs - tf2 - geometry_msgs - nav_msgs - rclpy - std_msgs - tf2 - ros2launch - - ament_python - rclpy - interfaces - joy - - diff --git a/src/drive/setup.cfg b/src/drive/setup.cfg deleted file mode 100644 index 1b7de367..00000000 --- a/src/drive/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/drive -[install] -install_scripts=$base/lib/drive diff --git a/src/drive/setup.py b/src/drive/setup.py deleted file mode 100644 index c5c1d370..00000000 --- a/src/drive/setup.py +++ /dev/null @@ -1,37 +0,0 @@ -from setuptools import setup -import os -from glob import glob - -package_name = "drive" - -setup( - name=package_name, - version="0.0.0", - packages=[package_name], - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - ( - os.path.join("share", package_name, "launch"), - glob(os.path.join("launch", "*launch.[pxy][yma]*")), - ), - (os.path.join("share", package_name, "config"), glob("config/*")), - ], - install_requires=["setuptools"], - zip_safe=True, - maintainer="chris", - maintainer_email="chris@todo.todo", - description="TODO: Package description", - license="TODO: License declaration", - tests_require=["pytest"], - entry_points={ - "console_scripts": [ - "drive_node = drive.drive_node:main", - "drive_station = drive.drive_station:main", - "joystick_drive = drive.joystick_drive:main", - "roboclaw_node = drive.roboclaw_node:main", - "joystick_breakout = drive.joystick_breakout:main", - "joystick_controller = drive.joystick_controller:main", - ], - }, -) diff --git a/src/drive/test/test_copyright.py b/src/drive/test/test_copyright.py deleted file mode 100644 index f46f861d..00000000 --- a/src/drive/test/test_copyright.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/src/interfaces/CMakeLists.txt b/src/interfaces/CMakeLists.txt index 7cde292b..4bae23ad 100644 --- a/src/interfaces/CMakeLists.txt +++ b/src/interfaces/CMakeLists.txt @@ -19,17 +19,9 @@ find_package(geographic_msgs REQUIRED) find_package(sensor_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - "msg/SixFloats.msg" - "msg/LiveTune.msg" - "msg/PIDmsg.msg" "msg/ArucoMarkers.msg" - "msg/PointArray.msg" - "msg/ArmCmd.msg" "msg/VideoSource.msg" "msg/SystemTelemetry.msg" - "srv/ArmPos.srv" - "srv/ArmCmd.srv" - "msg/GPIOmsg.msg" #ik its goofy the actual gpio package is GPIO "msg/GasSensorReading.msg" "srv/NavToGPSGeopose.srv" "srv/VideoOut.srv" diff --git a/src/interfaces/msg/ArmCmd.msg b/src/interfaces/msg/ArmCmd.msg deleted file mode 100644 index 828a2dd5..00000000 --- a/src/interfaces/msg/ArmCmd.msg +++ /dev/null @@ -1,12 +0,0 @@ -uint8 END_EFF_UNKNOWN=0 -uint8 END_EFF_OPEN=1 -uint8 END_EFF_CLOSE=2 - -geometry_msgs/Pose pose -geometry_msgs/Pose current_pose -float32 speed -bool estop -bool reset -uint8 end_effector -bool query_goal_state -float64[] goal_angles diff --git a/src/interfaces/msg/GPIOmsg.msg b/src/interfaces/msg/GPIOmsg.msg deleted file mode 100644 index d5f71368..00000000 --- a/src/interfaces/msg/GPIOmsg.msg +++ /dev/null @@ -1,2 +0,0 @@ -int64 value -int64 gpio \ No newline at end of file diff --git a/src/interfaces/msg/LiveTune.msg b/src/interfaces/msg/LiveTune.msg deleted file mode 100644 index 8e4feeff..00000000 --- a/src/interfaces/msg/LiveTune.msg +++ /dev/null @@ -1,3 +0,0 @@ -int8 arm_motor_number -string command -float32 value \ No newline at end of file diff --git a/src/interfaces/msg/PIDmsg.msg b/src/interfaces/msg/PIDmsg.msg deleted file mode 100644 index 71fe1f46..00000000 --- a/src/interfaces/msg/PIDmsg.msg +++ /dev/null @@ -1,3 +0,0 @@ -float32 p -float32 i -float32 d \ No newline at end of file diff --git a/src/interfaces/msg/PointArray.msg b/src/interfaces/msg/PointArray.msg deleted file mode 100644 index 1c55e6d8..00000000 --- a/src/interfaces/msg/PointArray.msg +++ /dev/null @@ -1,5 +0,0 @@ -std_msgs/Header header - -geometry_msgs/Point[] points -bool[] is_moving # Targets moving means they are less accurate with alot of noise - diff --git a/src/interfaces/msg/SixFloats.msg b/src/interfaces/msg/SixFloats.msg deleted file mode 100644 index f2a98139..00000000 --- a/src/interfaces/msg/SixFloats.msg +++ /dev/null @@ -1,6 +0,0 @@ -float32 m0 -float32 m1 -float32 m2 -float32 m3 -float32 m4 -float32 m5 \ No newline at end of file diff --git a/src/interfaces/srv/ArmCmd.srv b/src/interfaces/srv/ArmCmd.srv deleted file mode 100644 index 78f66493..00000000 --- a/src/interfaces/srv/ArmCmd.srv +++ /dev/null @@ -1,8 +0,0 @@ -float32 base -float32 diff1 -float32 diff2 -float32 elbow -float32 wristtilt -float32 wristturn ---- -bool success diff --git a/src/interfaces/srv/ArmPos.srv b/src/interfaces/srv/ArmPos.srv deleted file mode 100644 index 5d26f79f..00000000 --- a/src/interfaces/srv/ArmPos.srv +++ /dev/null @@ -1,8 +0,0 @@ -bool stop ---- -float32 base -float32 diff1 -float32 diff2 -float32 elbow -float32 wristtilt -float32 wristturn \ No newline at end of file diff --git a/src/keyboard_publisher/CMakeLists.txt b/src/keyboard_publisher/CMakeLists.txt deleted file mode 100644 index 81c80584..00000000 --- a/src/keyboard_publisher/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(keyboard_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(moveit_ros_planning_interface REQUIRED) -find_package(rclcpp REQUIRED) -find_package(interfaces REQUIRED) - -add_executable(keyboard_publisher src/keyboard_publisher.cpp) -target_include_directories(keyboard_publisher PUBLIC - $ - $) -target_compile_features(keyboard_publisher PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -ament_target_dependencies( - keyboard_publisher - interfaces - "moveit_ros_planning_interface" - "rclcpp" -) - -install(TARGETS keyboard_publisher - 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/keyboard_publisher/package.xml b/src/keyboard_publisher/package.xml deleted file mode 100644 index 1732685e..00000000 --- a/src/keyboard_publisher/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - keyboard_publisher - 0.0.0 - TODO: Package description - will - TODO: License declaration - - ament_cmake - - moveit_ros_planning_interface - rclcpp - interfaces - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/keyboard_publisher/src/keyboard_publisher.cpp b/src/keyboard_publisher/src/keyboard_publisher.cpp deleted file mode 100644 index 37aeb79b..00000000 --- a/src/keyboard_publisher/src/keyboard_publisher.cpp +++ /dev/null @@ -1,95 +0,0 @@ -#include "keyboard_publisher.h" - -using namespace std::chrono_literals; - -KeyboardPublisher::KeyboardPublisher() : Node("minimal_publisher"), count_(0) { - publisher_ = - this->create_publisher("arm_base_commands", 10); - timer_ = this->create_wall_timer( - 500ms, std::bind(&KeyboardPublisher::timer_callback, this)); - std::cout << "Type w, a, s, d to move. Use zxrtfgcv to change orientation. " - "Type 'h' to change step size (default is 10 rviz units). Type " - "'n' to reset. Type 'm' to open/close gripper. Type 'b' to plan " - "to the orange arm in rviz." - << std::endl; -} - -void KeyboardPublisher::timer_callback() { - interfaces::msg::ArmCmd poseCmd; - poseCmd.pose.position.x = 0; - poseCmd.pose.position.y = 0; - poseCmd.pose.position.z = 0; - poseCmd.pose.orientation.x = 0; - poseCmd.pose.orientation.y = 0; - poseCmd.pose.orientation.z = 0; - poseCmd.pose.orientation.w = 0; - poseCmd.speed = defSpeed; - poseCmd.end_effector = interfaces::msg::ArmCmd::END_EFF_UNKNOWN; - poseCmd.estop = false; - poseCmd.reset = false; - poseCmd.query_goal_state = false; - char c; - std::cin >> c; - if (c == 'w') { - poseCmd.pose.position.x = 1; - } else if (c == 's') { - poseCmd.pose.position.x = -1; - } else if (c == 'a') { - poseCmd.pose.position.y = 1; - } else if (c == 'd') { - poseCmd.pose.position.y = -1; - } else if (c == 'z') { - poseCmd.pose.position.z = 1; - } else if (c == 'x') { - poseCmd.pose.position.z = -1; - } else if (c == 'r') { - poseCmd.pose.orientation.x = 1; - } else if (c == 't') { - poseCmd.pose.orientation.x = -1; - } else if (c == 'f') { - poseCmd.pose.orientation.y = 1; - } else if (c == 'g') { - poseCmd.pose.orientation.y = -1; - } else if (c == 'c') { - poseCmd.pose.orientation.z = 1; - } else if (c == 'v') { - poseCmd.pose.orientation.z = -1; - } else if (c == 'h') { - double newSpeed = 0; - std::cin >> newSpeed; - defSpeed = newSpeed; - } else if (c == 'n') { - poseCmd.reset = true; - } else if (c == 'm') { - isOpen = !isOpen; - poseCmd.end_effector = isOpen ? interfaces::msg::ArmCmd::END_EFF_OPEN - : interfaces::msg::ArmCmd::END_EFF_CLOSE; - } else if (c == 'b') { - poseCmd.query_goal_state = true; - poseCmd.goal_angles.resize(6, 0); - double d = 0; - for (int i = 0; i < 6; i++) { - if (!(std::cin >> d)) { - std::cout << "Enter a real number please >:(" << std::endl; - std::cin.clear(); - std::cin.ignore(std::numeric_limits::max(), '\n'); - return; - } - poseCmd.goal_angles[i] = d / 360.0 * 2 * M_PI; - } - for (unsigned long int i = 0; i < poseCmd.goal_angles.size(); i++) { - RCLCPP_INFO(this->get_logger(), "Goal angle [%ld]: %f", i, - poseCmd.goal_angles[i]); - } - } - - publisher_->publish(poseCmd); -} - -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - - rclcpp::shutdown(); - return 0; -} diff --git a/src/keyboard_publisher/src/keyboard_publisher.h b/src/keyboard_publisher/src/keyboard_publisher.h deleted file mode 100644 index 108f90ff..00000000 --- a/src/keyboard_publisher/src/keyboard_publisher.h +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef KEYBOARD_PUBLISHER_H -#define KEYBOARD_PUBLISHER_H - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "interfaces/msg/arm_cmd.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -class KeyboardPublisher : public rclcpp::Node { - public: - KeyboardPublisher(); - - private: - void timer_callback(); - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - size_t count_; - double defSpeed = 10; - bool isOpen = true; -}; - -#endif diff --git a/src/kindr_ros b/src/kindr_ros deleted file mode 160000 index a2930e14..00000000 --- a/src/kindr_ros +++ /dev/null @@ -1 +0,0 @@ -Subproject commit a2930e1427f2b297a88abb7207bcf0195190af97 diff --git a/src/localization/launch/all_nav.launch.py b/src/localization/launch/all_nav.launch.py deleted file mode 100644 index 643c6a66..00000000 --- a/src/localization/launch/all_nav.launch.py +++ /dev/null @@ -1,73 +0,0 @@ -# Copyright (C) 2023 Miguel Ángel González Santamarta - -# This program is free software: you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. - -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. - -# You should have received a copy of the GNU General Public License -# along with this program. If not, see . - - -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource - - -def generate_launch_description(): - - pkg_localization = get_package_share_directory("localization") - pkg_navigation = get_package_share_directory("navigation") - pkg_description = get_package_share_directory("description") - pkg_drive = get_package_share_directory("drive") - pkg_ouster = get_package_share_directory("ouster_ros") - pkg_camera_processing = get_package_share_directory("camera_processing") - - localization_launch_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_localization, "launch", "localization.launch.py") - ) - ) - - ouster_launch_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_ouster, "launch", "driver.launch.py") - ) - ) - - description_launch_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_description, "launch", "robot_state_publisher.launch.py") - ) - ) - - drive_launch_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_drive, "launch", "roverdrivewithoutjoystick.launch.py") - ) - ) - - zed_node_launch_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_camera_processing, "launch", "zed.launch.py") - ), - launch_arguments={"profile": "night"}.items(), - ) - - ld = LaunchDescription() - - # ld.add_action(nav_launch_cmd) - ld.add_action(localization_launch_cmd) - ld.add_action(ouster_launch_cmd) - ld.add_action(description_launch_cmd) - ld.add_action(drive_launch_cmd) - ld.add_action(zed_node_launch_cmd) - - return ld diff --git a/src/moveit_controller/CMakeLists.txt b/src/moveit_controller/CMakeLists.txt deleted file mode 100644 index 6f2c4129..00000000 --- a/src/moveit_controller/CMakeLists.txt +++ /dev/null @@ -1,62 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(moveit_controller) - -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(moveit_ros_planning_interface REQUIRED) -find_package(rclcpp REQUIRED) -find_package(moveit_core REQUIRED) -find_package(moveit_servo REQUIRED) -find_package(interfaces REQUIRED) -find_package(control_toolbox REQUIRED) - -add_executable(moveit_controller src/moveit_controller.cpp) -target_include_directories(moveit_controller PUBLIC - $ - $) -target_compile_features(moveit_controller PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -ament_target_dependencies( - moveit_controller - interfaces - "moveit_ros_planning_interface" - "moveit_core" - "rclcpp" -) - -add_executable(servo_keyboard src/servo_keyboard.cpp) -target_include_directories(servo_keyboard PUBLIC - $ - $) -target_compile_features(servo_keyboard PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -ament_target_dependencies( - servo_keyboard - interfaces - control_toolbox - moveit_core - "moveit_ros_planning_interface" - "moveit_servo" - "rclcpp" -) - -install(TARGETS moveit_controller servo_keyboard - DESTINATION lib/${PROJECT_NAME}) -#install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} -# PATTERN "setup_assistant.launch" EXCLUDE) - -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/moveit_controller/package.xml b/src/moveit_controller/package.xml deleted file mode 100644 index a1bbf0d3..00000000 --- a/src/moveit_controller/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - moveit_controller - 0.0.0 - TODO: Package description - will - TODO: License declaration - - ament_cmake - - moveit_ros_planning_interface - moveit_core - rclcpp - interfaces - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/moveit_controller/src/moveit_controller.cpp b/src/moveit_controller/src/moveit_controller.cpp deleted file mode 100644 index e8e0acc3..00000000 --- a/src/moveit_controller/src/moveit_controller.cpp +++ /dev/null @@ -1,130 +0,0 @@ -#include "moveit_controller.h" - -using moveit::planning_interface::MoveGroupInterface; -using std::placeholders::_1; - -void executeTrajectory(moveit_msgs::msg::RobotTrajectory &traj, - moveit::planning_interface::MoveGroupInterfacePtr mgi) { - RCLCPP_INFO(rclcpp::get_logger("moveit_controller"), "Starting thread"); - mgi->asyncExecute(traj); - RCLCPP_INFO(rclcpp::get_logger("moveit_controller"), "Ending thread"); -} - -bool isEmpty(const geometry_msgs::msg::Pose &p) { - return p.position.x == 0 && p.position.y == 0 && p.position.z == 0 && - p.orientation.x == 0 && p.orientation.y == 0 && p.orientation.z == 0 && - p.orientation.w == 0; -} - -MoveitController::MoveitController(const rclcpp::NodeOptions &options) - : Node("moveit_controller", options), - node_ptr_(std::make_shared("example_moveit")), - executor_ptr_( - std::make_shared()) { - subscription_ = this->create_subscription( - "arm_base_commands", 10, - std::bind(&MoveitController::topic_callback, this, - std::placeholders::_1)); - - move_group_ptr_ = - std::make_shared( - node_ptr_, "rover_arm"); // used to be rover_arm, then rover_arm2 - - executor_ptr_->add_node(node_ptr_); - executor_thread_ = std::thread([this]() { this->executor_ptr_->spin(); }); -} - -void MoveitController::topic_callback(const interfaces::msg::ArmCmd &armMsg) { - geometry_msgs::msg::Pose poseMsg = armMsg.pose; - double stepSize = armMsg.speed; - RCLCPP_INFO(this->get_logger(), "I heard: %f %f %f %f %f %f %f", - poseMsg.position.x, poseMsg.position.y, poseMsg.position.z, - poseMsg.orientation.x, poseMsg.orientation.y, - poseMsg.orientation.z, - poseMsg.orientation.w); //*/ - - move_group_ptr_->stop(); - if (move_it_thread_.joinable()) { - move_it_thread_.join(); - } - if ((isEmpty(poseMsg) && !armMsg.reset && - armMsg.end_effector == interfaces::msg::ArmCmd::END_EFF_UNKNOWN && - !armMsg.query_goal_state) || - armMsg.estop) { - RCLCPP_INFO(this->get_logger(), "Stopping the arm"); - return; - } - - std::vector points; - - move_group_ptr_->setStartStateToCurrentState(); - geometry_msgs::msg::Pose current_pose = - move_group_ptr_->getCurrentPose().pose; - - points.push_back(current_pose); - - // Print the current pose of the end effector - RCLCPP_INFO(this->get_logger(), "Current pose: %f %f %f %f %f %f %f", - current_pose.position.x, current_pose.position.y, - current_pose.position.z, current_pose.orientation.x, - current_pose.orientation.y, current_pose.orientation.z, - current_pose.orientation.w); //*/ - - if (armMsg.end_effector != interfaces::msg::ArmCmd::END_EFF_UNKNOWN) { - // TODO: Add end-effector features - } - if (armMsg.reset == true) // reset to default position - { - geometry_msgs::msg::Pose target_pose; - target_pose.position = default_pose_.position; - move_group_ptr_->setPoseTarget(target_pose); - - // Create a plan to that target pose - moveit::planning_interface::MoveGroupInterface::Plan plan; - bool success = static_cast(move_group_ptr_->plan(plan)); - - // Execute the plan - if (success) { - move_group_ptr_->execute(plan); - } else { - RCLCPP_ERROR(this->get_logger(), "Planing failed!"); - } - } else { - geometry_msgs::msg::Pose new_pose = current_pose; - new_pose.position.x += poseMsg.position.x * stepSize; - new_pose.position.y += poseMsg.position.y * stepSize; - new_pose.position.z += poseMsg.position.z * stepSize; - - if (poseMsg.orientation.x != 0 || poseMsg.orientation.y != 0 || - poseMsg.orientation.z != 0 || - poseMsg.orientation.w != 0) // rotation required - { - tf2::Quaternion q1; - tf2::convert(current_pose.orientation, q1); - tf2::Quaternion q2; - q2.setRPY(poseMsg.orientation.x, poseMsg.orientation.y, - poseMsg.orientation.z); - tf2::Quaternion q3 = q1 * q2; - geometry_msgs::msg::Quaternion q4 = tf2::toMsg(q3); - - new_pose.orientation = q4; - } - points.push_back(new_pose); - move_group_ptr_->computeCartesianPath(points, EEF_STEP, JUMP_THRESHOLD, - trajectory_); - - // launch thread - move_it_thread_ = - std::thread(executeTrajectory, std::ref(trajectory_), move_group_ptr_); - RCLCPP_INFO(this->get_logger(), "Thread created"); - } -} - -int main(int argc, char **argv) { - // Initialize ROS and create the Node - rclcpp::init(argc, argv); - auto node = std::make_shared( - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides( - true)); - rclcpp::spin(node); -} diff --git a/src/moveit_controller/src/moveit_controller.h b/src/moveit_controller/src/moveit_controller.h deleted file mode 100644 index eecf6859..00000000 --- a/src/moveit_controller/src/moveit_controller.h +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef MOVEIT_CONTROLLER_H -#define MOVEIT_CONTROLLER_H - -// TODO: Set to best position to drive in -#define ARM_DEFAULT_X 0.636922 -#define ARM_DEFAULT_Y 0.064768 -#define ARM_DEFAULT_Z 0.678810 - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "interfaces/msg/arm_cmd.hpp" -#include "std_msgs/msg/string.hpp" - -void executeTrajectory(moveit_msgs::msg::RobotTrajectory &traj, - moveit::planning_interface::MoveGroupInterfacePtr mgi); - -bool isEmpty(const geometry_msgs::msg::Pose &p); - -class MoveitController : public rclcpp::Node { - public: - MoveitController(const rclcpp::NodeOptions &options); - - private: - static constexpr double JUMP_THRESHOLD = 0; - static constexpr double EEF_STEP = 0.01; - rclcpp::Subscription::SharedPtr subscription_; - - moveit::planning_interface::MoveGroupInterfacePtr move_group_ptr_; - rclcpp::Node::SharedPtr node_ptr_; - rclcpp::Executor::SharedPtr executor_ptr_; - std::thread executor_thread_; - std::thread move_it_thread_; - moveit_msgs::msg::RobotTrajectory trajectory_; - geometry_msgs::msg::Pose default_pose_; - - void topic_callback(const interfaces::msg::ArmCmd &armMsg); -}; - -#endif diff --git a/src/moveit_controller/src/servo_keyboard.cpp b/src/moveit_controller/src/servo_keyboard.cpp deleted file mode 100644 index 998aeb86..00000000 --- a/src/moveit_controller/src/servo_keyboard.cpp +++ /dev/null @@ -1,213 +0,0 @@ -#include "servo_keyboard.h" - -KeyboardReader::KeyboardReader() : kfd(0) { - // get the console in raw mode - tcgetattr(kfd, &cooked); - struct termios raw; - memcpy(&raw, &cooked, sizeof(struct termios)); - raw.c_lflag &= ~(ICANON | ECHO); - // Setting a new line, then end of file - raw.c_cc[VEOL] = 1; - raw.c_cc[VEOF] = 2; - tcsetattr(kfd, TCSANOW, &raw); -} - -KeyboardReader::~KeyboardReader() { tcsetattr(kfd, TCSANOW, &cooked); } - -void KeyboardReader::readOne(char* c) { - int rc = read(kfd, c, 1); - if (rc < 0) { - throw std::runtime_error("read failed"); - } -} - -KeyboardServo::KeyboardServo() - : frame_to_publish_(BASE_FRAME_ID), joint_vel_cmd_(1.0) { - nh_ = rclcpp::Node::make_shared("servo_keyboard_input"); - - twist_pub_ = nh_->create_publisher( - TWIST_TOPIC, ROS_QUEUE_SIZE); - joint_pub_ = nh_->create_publisher( - JOINT_TOPIC, ROS_QUEUE_SIZE); -} - -void KeyboardServo::spin() { - while (rclcpp::ok()) { - rclcpp::spin_some(nh_); - } -} - -int KeyboardServo::keyLoop() { - char c; - bool publish_twist = false; - bool publish_joint = false; - - std::thread{std::bind(&KeyboardServo::spin, this)}.detach(); - - puts("Reading from keyboard"); - puts("---------------------------"); - puts("Use arrow keys and the '.' and ';' keys to Cartesian jog"); - puts( - "Use 'W' to Cartesian jog in the world frame, and 'E' for the " - "End-Effector frame"); - puts( - "Use 1|2|3|4|5|6|7 keys to joint jog. 'R' to reverse the direction of " - "jogging."); - puts("'Q' to quit."); - - for (;;) { - // get the next event from the keyboard - try { - input.readOne(&c); - } catch (const std::runtime_error&) { - RCLCPP_INFO(nh_->get_logger(), "Read error!"); - return -1; - } - - RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c); - - // // Create the messages we might publish - auto twist_msg = std::make_unique(); - auto joint_msg = std::make_unique(); - - // Use read key-press - switch (c) { - case KEYCODE_LEFT: - RCLCPP_DEBUG(nh_->get_logger(), "LEFT"); - twist_msg->twist.linear.y = -1.0; - publish_twist = true; - break; - case KEYCODE_RIGHT: - RCLCPP_DEBUG(nh_->get_logger(), "RIGHT"); - twist_msg->twist.linear.y = 1.0; - publish_twist = true; - break; - case KEYCODE_UP: - RCLCPP_DEBUG(nh_->get_logger(), "UP"); - twist_msg->twist.linear.x = 1.0; - publish_twist = true; - break; - case KEYCODE_DOWN: - RCLCPP_DEBUG(nh_->get_logger(), "DOWN"); - twist_msg->twist.linear.x = -1.0; - publish_twist = true; - break; - case KEYCODE_PERIOD: - RCLCPP_DEBUG(nh_->get_logger(), "PERIOD"); - twist_msg->twist.linear.z = -1.0; - publish_twist = true; - break; - case KEYCODE_SEMICOLON: - RCLCPP_DEBUG(nh_->get_logger(), "SEMICOLON"); - twist_msg->twist.linear.z = 1.0; - publish_twist = true; - break; - case KEYCODE_A: - RCLCPP_DEBUG(nh_->get_logger(), "A"); - twist_msg->twist.angular.x = 1.0; - publish_twist = true; - break; - case KEYCODE_S: - RCLCPP_DEBUG(nh_->get_logger(), "S"); - twist_msg->twist.angular.x = -1.0; - publish_twist = true; - break; - case KEYCODE_D: - RCLCPP_DEBUG(nh_->get_logger(), "D"); - twist_msg->twist.angular.y = 1.0; - publish_twist = true; - break; - case KEYCODE_F: - RCLCPP_DEBUG(nh_->get_logger(), "F"); - twist_msg->twist.angular.y = -1.0; - publish_twist = true; - break; - case KEYCODE_Z: - RCLCPP_DEBUG(nh_->get_logger(), "Z"); - twist_msg->twist.angular.z = 1.0; - publish_twist = true; - break; - case KEYCODE_X: - RCLCPP_DEBUG(nh_->get_logger(), "X"); - twist_msg->twist.angular.z = -1.0; - publish_twist = true; - break; - case KEYCODE_E: - RCLCPP_DEBUG(nh_->get_logger(), "E"); - frame_to_publish_ = EEF_FRAME_ID; - break; - case KEYCODE_W: - RCLCPP_DEBUG(nh_->get_logger(), "W"); - frame_to_publish_ = BASE_FRAME_ID; - break; - case KEYCODE_1: - RCLCPP_DEBUG(nh_->get_logger(), "1"); - joint_msg->joint_names.push_back("Joint_2"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_2: - RCLCPP_DEBUG(nh_->get_logger(), "2"); - joint_msg->joint_names.push_back("Joint_2"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_3: - RCLCPP_DEBUG(nh_->get_logger(), "3"); - joint_msg->joint_names.push_back("Joint_3"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_4: - RCLCPP_DEBUG(nh_->get_logger(), "4"); - joint_msg->joint_names.push_back("Joint_4"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_5: - RCLCPP_DEBUG(nh_->get_logger(), "5"); - joint_msg->joint_names.push_back("Joint_5"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_6: - RCLCPP_DEBUG(nh_->get_logger(), "6"); - joint_msg->joint_names.push_back("Joint_6"); - joint_msg->velocities.push_back(joint_vel_cmd_); - publish_joint = true; - break; - case KEYCODE_R: - RCLCPP_DEBUG(nh_->get_logger(), "R"); - joint_vel_cmd_ *= -1; - break; - case KEYCODE_Q: - RCLCPP_DEBUG(nh_->get_logger(), "quit"); - return 0; - } - - // If a key requiring a publish was pressed, publish the message now - if (publish_twist) { - twist_msg->header.stamp = nh_->now(); - twist_msg->header.frame_id = frame_to_publish_; - twist_pub_->publish(std::move(twist_msg)); - publish_twist = false; - } else if (publish_joint) { - joint_msg->header.stamp = nh_->now(); - joint_msg->header.frame_id = BASE_FRAME_ID; - joint_pub_->publish(std::move(joint_msg)); - publish_joint = false; - } - } - - return 0; -} - -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - KeyboardServo keyboard_servo; - - int rc = keyboard_servo.keyLoop(); - rclcpp::shutdown(); - - return rc; -} diff --git a/src/moveit_controller/src/servo_keyboard.h b/src/moveit_controller/src/servo_keyboard.h deleted file mode 100644 index ab7a824a..00000000 --- a/src/moveit_controller/src/servo_keyboard.h +++ /dev/null @@ -1,77 +0,0 @@ -#ifndef SERVO_KEYBOARD_H -#define SERVO_KEYBOARD_H - -#include -#include -#include -#include - -#include -#include -#include - -// Define used keys -#define KEYCODE_RIGHT 0x43 -#define KEYCODE_LEFT 0x44 -#define KEYCODE_UP 0x41 -#define KEYCODE_DOWN 0x42 -#define KEYCODE_PERIOD 0x2E -#define KEYCODE_SEMICOLON 0x3B -#define KEYCODE_1 0x31 -#define KEYCODE_2 0x32 -#define KEYCODE_3 0x33 -#define KEYCODE_4 0x34 -#define KEYCODE_5 0x35 -#define KEYCODE_6 0x36 -#define KEYCODE_7 0x37 -#define KEYCODE_Q 0x71 -#define KEYCODE_W 0x77 -#define KEYCODE_E 0x65 -#define KEYCODE_R 0x72 -#define KEYCODE_A 0x61 -#define KEYCODE_S 0x73 -#define KEYCODE_D 0x64 -#define KEYCODE_F 0x66 -#define KEYCODE_Z 0x7A -#define KEYCODE_X 0x78 - -// Some constants used in the Servo Teleop demo -const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds"; -const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds"; -const size_t ROS_QUEUE_SIZE = 10; -const std::string EEF_FRAME_ID = "Link_7"; -const std::string BASE_FRAME_ID = "Link_1"; - -// A class for reading the key inputs from the terminal -class KeyboardReader { - public: - KeyboardReader(); - ~KeyboardReader(); - void readOne(char* c); - - private: - int kfd; - struct termios cooked; -}; - -// Converts key-presses to Twist or Jog commands for Servo, in lieu of a -// controller -class KeyboardServo { - public: - KeyboardServo(); - int keyLoop(); - - private: - void spin(); - - rclcpp::Node::SharedPtr nh_; - - rclcpp::Publisher::SharedPtr twist_pub_; - rclcpp::Publisher::SharedPtr joint_pub_; - - std::string frame_to_publish_; - double joint_vel_cmd_; - KeyboardReader input; -}; - -#endif diff --git a/src/ouster-ros b/src/ouster-ros deleted file mode 160000 index 8c94eab3..00000000 --- a/src/ouster-ros +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8c94eab3832c279498364b88acb11a929fc01fa6 diff --git a/src/science_sensors/science_sensors/stellarnet_driverLibs/__init__.py b/src/science_sensors/science_sensors/stellarnet_driverLibs/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/science_sensors/test/test_flake8.py b/src/science_sensors/test/test_flake8.py deleted file mode 100644 index ee79f31a..00000000 --- a/src/science_sensors/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len( - errors - ) + "\n".join(errors) diff --git a/src/science_sensors/test/test_pep257.py b/src/science_sensors/test/test_pep257.py deleted file mode 100644 index a2c3deb8..00000000 --- a/src/science_sensors/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" diff --git a/src/servo_pkg/servo_pkg/__init__.py b/src/servo_pkg/servo_pkg/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/servo_pkg/test/test_copyright.py b/src/servo_pkg/test/test_copyright.py deleted file mode 100644 index ceffe896..00000000 --- a/src/servo_pkg/test/test_copyright.py +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip( - reason="No copyright header has been placed in the generated source file." -) -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/src/servo_pkg/test/test_flake8.py b/src/servo_pkg/test/test_flake8.py deleted file mode 100644 index ee79f31a..00000000 --- a/src/servo_pkg/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len( - errors - ) + "\n".join(errors) diff --git a/src/servo_pkg/test/test_pep257.py b/src/servo_pkg/test/test_pep257.py deleted file mode 100644 index a2c3deb8..00000000 --- a/src/servo_pkg/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" diff --git a/src/third-party/zed-ros2-wrapper b/src/third-party/zed-ros2-wrapper new file mode 160000 index 00000000..0c168064 --- /dev/null +++ b/src/third-party/zed-ros2-wrapper @@ -0,0 +1 @@ +Subproject commit 0c168064611ceecbd49cd3a84ae865f72970eafc diff --git a/src/ublox b/src/ublox deleted file mode 160000 index 594c999d..00000000 --- a/src/ublox +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 594c999d9bb0e35feac286625cf3ba1a81a7c3fc diff --git a/src/zed-ros2-wrapper b/src/zed-ros2-wrapper deleted file mode 160000 index adaaed80..00000000 --- a/src/zed-ros2-wrapper +++ /dev/null @@ -1 +0,0 @@ -Subproject commit adaaed8092c942f7fd3a697f6ec2da9259775cf3