From 302481ca0bc4a1fa9bfc4093dd2e6995901aedbe Mon Sep 17 00:00:00 2001 From: ConnorN Date: Thu, 14 Aug 2025 21:00:29 -0400 Subject: [PATCH 01/36] dev containers WIP --- .devcontainer/devcontainer.json | 25 +++ .github/workflows/docker-build-science.yml | 33 ---- .gitignore | 8 +- Dockerfile | 23 +++ Dockerfile.science | 95 ----------- README.md | 35 ++++ build.sh | 6 +- build_science.sh | 3 - docker/Dockerfile.base | 156 ++++++++++++++++++ docker/Dockerfile.dev | 57 +++++++ make.sh | 46 ++++++ setup/science/setup_science.sh | 11 +- setup/science/start_science.sh | 14 +- setup/start_rover.sh | 1 + src/bringup/launch/arm.launch.py | 25 +++ src/bringup/launch/core.launch.py | 25 +++ src/bringup/launch/drive.launch.py | 23 +++ src/bringup/launch/localization.launch.py | 30 ++++ src/bringup/launch/navigation.launch.py | 35 ++++ src/bringup/launch/video_streaming.launch.py | 23 +++ src/drive/launch/talon.launch.py | 4 - src/gpio_controller/package.xml | 3 +- src/hardware/package.xml | 1 - src/localization/launch/all_nav.launch.py | 73 -------- .../{gps.launch.py => navsat.launch.py} | 39 ++--- 25 files changed, 552 insertions(+), 242 deletions(-) create mode 100644 .devcontainer/devcontainer.json delete mode 100644 .github/workflows/docker-build-science.yml create mode 100644 Dockerfile delete mode 100644 Dockerfile.science create mode 100644 README.md delete mode 100755 build_science.sh create mode 100644 docker/Dockerfile.base create mode 100644 docker/Dockerfile.dev create mode 100755 make.sh create mode 100644 src/bringup/launch/arm.launch.py create mode 100644 src/bringup/launch/core.launch.py create mode 100644 src/bringup/launch/drive.launch.py create mode 100644 src/bringup/launch/localization.launch.py create mode 100644 src/bringup/launch/navigation.launch.py create mode 100644 src/bringup/launch/video_streaming.launch.py delete mode 100644 src/localization/launch/all_nav.launch.py rename src/localization/launch/{gps.launch.py => navsat.launch.py} (51%) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 00000000..78c5d112 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,25 @@ +{ + "name": "CPRT Dev Container", + + "image": "cprtsoftware/cprt_rover_24-dev:latest", + + "runArgs": [ + "--network=host" + ], + "mounts": [ + "source=${env:SSH_AUTH_SOCK},target=/ssh-agent,type=bind" + ], + "containerEnv": { + "SSH_AUTH_SOCK": "/ssh-agent" + }, + "remoteUser": "vscode", + "containerUser": "vscode", + "remoteEnv": { + "USER": "vscode" + }, + + // Usual extras (optional) + "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/.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/.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/Dockerfile b/Dockerfile new file mode 100644 index 00000000..75a8b2bc --- /dev/null +++ b/Dockerfile @@ -0,0 +1,23 @@ +ARG IMAGE=cprtsoftware/cprt_rover_24-jetson:latest +FROM ${IMAGE} AS base + +FROM base AS builder + +WORKDIR /cprt_rover_24 + +RUN colcon build --continue-on-error --cmake-args=-DCMAKE_BUILD_TYPE=Release --parallel-workers $(nproc) \ + --build-base build-$aarch \ + --install-base install-$(uname -m) + +FROM base AS runtime + +# Create entrypoint script +RUN echo '#!/bin/bash' > /entrypoint.sh && \ + echo 'source /opt/ros/humble/setup.bash' >> /entrypoint.sh && \ + echo 'source /cprt_rover_24/setup.bash' >> /entrypoint.sh && \ + echo 'exec "$@"' >> /entrypoint.sh && \ + chmod +x /entrypoint.sh +COPY --from=builder /cprt_rover_24/install-$(uname -m)/* /cprt_rover_24 + +# Set entrypoint +ENTRYPOINT ["/entrypoint.sh"] \ No newline at end of file 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..049fa69e 100755 --- a/build.sh +++ b/build.sh @@ -24,4 +24,8 @@ done rosdep install --from-paths src -i -r -y -colcon build --symlink-install --continue-on-error --cmake-args=-DCMAKE_BUILD_TYPE=Release --parallel-workers $(nproc) \ No newline at end of file +aarch=$(uname -m) + +colcon build --symlink-install --continue-on-error --cmake-args=-DCMAKE_BUILD_TYPE=Release --parallel-workers $(nproc) \ + --build-base build-$aarch \ + --install-base install-$aarch \ 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/docker/Dockerfile.base b/docker/Dockerfile.base new file mode 100644 index 00000000..b7112e18 --- /dev/null +++ b/docker/Dockerfile.base @@ -0,0 +1,156 @@ +ARG IMAGE=ubuntu:22.04 + +FROM ${IMAGE} AS ros2_humble-base + +ENV DEBIAN_FRONTEND=noninteractive +ENV ROS_DISTRO=humble +ENV ROS_VERSION=2 +ENV ROS_PYTHON_VERSION=3 +ENV ROS_LOCALHOST_ONLY=0 +ENV ROS_DOMAIN_ID=0 + +RUN apt-get update && apt-get install -y locales apt-utils \ + && locale-gen en_US.UTF-8 \ + && update-locale LANG=en_US.UTF-8 LC_ALL=en_US.UTF-8 \ + && rm -rf /var/lib/apt/lists/* + +ENV LANG=en_US.UTF-8 +ENV LANGUAGE=en_US:en +ENV LC_ALL=en_US.UTF-8 + +# Install base packages +RUN apt-get update && apt-get install -y --no-install-recommends \ + curl \ + lsb-release \ + gnupg2 \ + libc6-dev \ + software-properties-common \ + iproute2 \ + busybox && \ + 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=$(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 + +# Install ROS 2 +RUN apt-get update && apt-get upgrade -y && \ + apt-get install -y --no-install-recommends \ + python3-dev \ + python3-pip \ + ros-humble-ros-base && \ + 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 \ + black \ + RPi.GPIO \ + rpi-hardware-pwm \ + Jetson.GPIO + + +FROM ros2_humble-base AS rosdeps + +RUN apt-get update && apt-get upgrade -y && \ + apt-get install -y --no-install-recommends \ + ros-humble-desktop \ + ros-dev-tools \ + python3-rosdep && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* + +# Set project directory +ARG DIR=/cprt_rover_24 +WORKDIR ${DIR} + +# Copy workspace into container +COPY src/**/package.xml src/**/setup.py src/**/CMakeLists.txt ./src + +ARG FILE=/opt/rosdeps.sh + +# Generate list of rosdep dependencies to be installed later +RUN apt-get update && \ + rosdep init && \ + rosdep update && \ + rosdep install -i -r -y --from-paths src -s > ${FILE} + +FROM ${IMAGE} AS gstreamer + +RUN 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 \ + git \ + ninja-build \ + curl + +RUN python3 -m pip install --upgrade pip && \ + pip3 install --user meson + +WORKDIR /gstreamer + +# Install GStreamer and plugins +RUN git clone https://gitlab.freedesktop.org/gstreamer/gstreamer.git && \ + cd gstreamer && \ + 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 + +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 + +RUN 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 && \ + cd gst-plugins-rs && \ + cargo install cargo-c && \ + 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 + +FROM ros2_humble-base AS base +ARG ROSDEP_FILE=/opt/rosdeps.sh +COPY --from=rosdeps ${ROSDEP_FILE} ${ROSDEP_FILE} +COPY --from=gstreamer /target / +COPY setup/enablecan.sh /usr/local/bin/enablecan.sh + +# Setup gstreamer enviorment +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 +ENV GST_PLUGIN_PATH=/opt/gstreamer/lib/gstreamer-1.0:/usr/lib/aarch64-linux-gnu/gstreamer-1.0 + +# Install rosdep dependencies +RUN apt-get update && \ + chmod +x ${ROSDEP_FILE} && \ + ${ROSDEP_FILE} && \ + rm -rf /var/lib/apt/lists/* + +CMD ["/bin/bash"] diff --git a/docker/Dockerfile.dev b/docker/Dockerfile.dev new file mode 100644 index 00000000..3b2a9329 --- /dev/null +++ b/docker/Dockerfile.dev @@ -0,0 +1,57 @@ +ARG IMAGE +FROM ${IMAGE} as ros2_humble-base + +FROM ros2_humble-base as ros2_humble-dev + +RUN apt-get update && apt-get install -y locales apt-utils \ + && locale-gen en_US.UTF-8 \ + && update-locale LANG=en_US.UTF-8 LC_ALL=en_US.UTF-8 \ + && rm -rf /var/lib/apt/lists/* + +ENV LANG=en_US.UTF-8 +ENV LANGUAGE=en_US:en +ENV LC_ALL=en_US.UTF-8 + +# Install base packages +RUN apt-get update && apt-get install -y --no-install-recommends \ + git \ + curl \ + lsb-release \ + gnupg2 \ + libc6-dev \ + x11-apps \ + software-properties-common && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* + +# Install ROS 2 and development tools +RUN apt-get update && apt-get upgrade -y && \ + apt-get install -y --no-install-recommends \ + ros-humble-desktop \ + ros-dev-tools \ + ros-humble-ament-cmake \ + python3-dev \ + python3-pip \ + ninja-build \ + python3-rosdep \ + python3-colcon-common-extensions \ + python3-colcon-ros \ + clang-format && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* + +FROM ros2_humble-dev as final + +# 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 + +RUN useradd -ms /bin/bash -u 1000 vscode \ + && echo "vscode ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers + +# Set entrypoint +ENTRYPOINT ["/entrypoint.sh"] +CMD ["/bin/bash"] diff --git a/make.sh b/make.sh new file mode 100755 index 00000000..f0c92bd7 --- /dev/null +++ b/make.sh @@ -0,0 +1,46 @@ +#!/bin/bash + +REPO=cprtsoftware/cprt_rover_24 +TYPE=jetson # Replace with 'base' as needed +TAG=latest # Replace with the desired commit hash or 'latest' + +BASE_IMAGE_NAME="$REPO-$TYPE:$TAG" +IMAGE_NAME="$REPO-$TYPE-deploy:$TAG" + +cd "$(dirname "$0")" + +source ./docker/common.sh + +files=$(git ls-files -- ':!:(*/)' ':!:(*/**/*)' ':!:(*/**/*/*)') + +# Filter out python files into one variable +python_files=$( + for file in $files; do + if [[ $file == *.py ]]; then + echo "$file" + fi + done +) + +# Launch one instance of `black` for all python files +IFS=$'\n' black $python_files + +# Format C/C++ source & header files +for file in $files; do + if [[ $file == *.cpp || $file == *.hpp || $file == *.c || $file == *.h ]]; then + clang-format -i "$file" + fi +done + +check_docker + +docker run --rm --privileged multiarch/qemu-user-static --reset -p yes + +docker buildx build \ + -t $IMAGE_NAME \ + -f Dockerfile \ + --platform linux/arm64/v8 \ + --build-arg IMAGE=stereolabs/zed:5.0-runtime-l4t-r36.4 \ + --push \ + . + diff --git a/setup/science/setup_science.sh b/setup/science/setup_science.sh index 22a9e0fb..fc799d65 100755 --- a/setup/science/setup_science.sh +++ b/setup/science/setup_science.sh @@ -3,6 +3,15 @@ SCRIPT_DIR="$(dirname "$0")" CURRENT_DIR="$(pwd)" CURRENT_USER=$(whoami) VERSION="science-1.0.0" +AARCH=$(uname -m) +if [ "$AARCH" = "aarch64" ]; then + echo "Detected ARM architecture." + AARCH="arm64" +else + echo "Detected AMD64 architecture." + AARCH="amd64" +fi +echo "Using architecture: $AARCH" # Check if the script is being run from its own directory if [ "$SCRIPT_DIR" == "$CURRENT_DIR" ] || [ "$SCRIPT_DIR" == "." ]; then @@ -54,7 +63,7 @@ fi # Build the Docker image for the project echo "Pulling Docker image..." -docker pull cprtsoftware/cprt_rover_24:${VERSION} +docker pull cprtsoftware/cprt_rover_24-science:$AARCH # Setup science service sudo cp start_science.service /etc/systemd/system/start_science.service diff --git a/setup/science/start_science.sh b/setup/science/start_science.sh index 34e32060..75359c6a 100755 --- a/setup/science/start_science.sh +++ b/setup/science/start_science.sh @@ -1,7 +1,17 @@ #!/bin/bash -IMAGE=cprtsoftware/cprt_rover_24 -TAG=science-latest +AARCH=$(uname -m) +if [ "$AARCH" = "aarch64" ]; then + echo "Detected ARM architecture." + AARCH="arm64" +else + echo "Detected AMD64 architecture." + AARCH="amd64" +fi +echo "Using architecture: $AARCH" + +IMAGE=cprtsoftware/cprt_rover_24-science +TAG=$AARCH JETSON_IP=192.168.0.55 PORT=9000 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/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/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..9b9050eb --- /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", "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/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/drive/launch/talon.launch.py b/src/drive/launch/talon.launch.py index e1dd501a..96b79769 100644 --- a/src/drive/launch/talon.launch.py +++ b/src/drive/launch/talon.launch.py @@ -10,10 +10,6 @@ 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" diff --git a/src/gpio_controller/package.xml b/src/gpio_controller/package.xml index 7cc44904..46f48483 100644 --- a/src/gpio_controller/package.xml +++ b/src/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/hardware/package.xml b/src/hardware/package.xml index e722b579..3d64df89 100644 --- a/src/hardware/package.xml +++ b/src/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/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/localization/launch/gps.launch.py b/src/localization/launch/navsat.launch.py similarity index 51% rename from src/localization/launch/gps.launch.py rename to src/localization/launch/navsat.launch.py index 6412e989..12ec2fb0 100644 --- a/src/localization/launch/gps.launch.py +++ b/src/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, ] ) From 6e42fcd22285da8fba244fdbafa28af39f4097dc Mon Sep 17 00:00:00 2001 From: ConnorN Date: Thu, 14 Aug 2025 23:50:51 -0400 Subject: [PATCH 02/36] Trial #1 --- .github/workflows/build-docker.yml | 73 ++++++++++ Dockerfile | 222 +++++++++++++++++++++++++++-- docker/Dockerfile.base | 156 -------------------- docker/Dockerfile.dev | 57 -------- requirements.txt | 14 ++ 5 files changed, 296 insertions(+), 226 deletions(-) create mode 100644 .github/workflows/build-docker.yml delete mode 100644 docker/Dockerfile.base delete mode 100644 docker/Dockerfile.dev create mode 100644 requirements.txt diff --git a/.github/workflows/build-docker.yml b/.github/workflows/build-docker.yml new file mode 100644 index 00000000..4a430f41 --- /dev/null +++ b/.github/workflows/build-docker.yml @@ -0,0 +1,73 @@ +name: Docker CI/CD + +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + +jobs: + build: + runs-on: ubuntu-latest + strategy: + matrix: + arch: [amd64, arm64] + + steps: + - name: Checkout code + uses: actions/checkout@v4 + + - name: Set up QEMU + uses: docker/setup-qemu-action@v3 + with: + platforms: linux/amd64,linux/arm64 + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + + - name: Cache Docker layers + uses: actions/cache@v3 + with: + path: /tmp/.buildx-cache + key: ${{ runner.os }}-buildx-${{ matrix.arch }}-${{ hashFiles('Dockerfile', 'requirements.txt') }} + restore-keys: | + ${{ runner.os }}-buildx-${{ matrix.arch }}- + + - name: Build base image + run: | + docker buildx build \ + --platform linux/${{ matrix.arch }} \ + --target ros2_humble-base \ + --build-arg TARGETARCH=${{ matrix.arch }} \ + --cache-to type=local,dest=/tmp/.buildx-cache,mode=max \ + --cache-from type=local,src=/tmp/.buildx-cache \ + -t cprtsoftware/ros2_humble-base:latest \ + -t cprtsoftware/ros2_humble-base:${{ github.sha }} \ + . + + - name: Build dev image + run: | + docker buildx build \ + --platform linux/${{ matrix.arch }} \ + --target dev \ + --build-arg TARGETARCH=${{ matrix.arch }} \ + --cache-to type=local,dest=/tmp/.buildx-cache,mode=max \ + --cache-from type=local,src=/tmp/.buildx-cache \ + -t cprtsoftware/ros2_humble-dev:latest \ + -t cprtsoftware/ros2_humble-dev:${{ github.sha }} \ + . + + - name: Log in to Docker Hub + if: github.event_name == 'push' + uses: docker/login-action@v2 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + + - name: Push images + if: github.event_name == 'push' + run: | + docker push cprtsoftware/ros2_humble-base:latest + docker push cprtsoftware/ros2_humble-base:${{ github.sha }} + docker push cprtsoftware/ros2_humble-dev:latest + docker push cprtsoftware/ros2_humble-dev:${{ github.sha }} diff --git a/Dockerfile b/Dockerfile index 75a8b2bc..8ced5cc7 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,23 +1,219 @@ -ARG IMAGE=cprtsoftware/cprt_rover_24-jetson:latest -FROM ${IMAGE} AS base +# syntax=docker/dockerfile:1.5 -FROM base AS builder +############################ +# Pick the Base Image +# This Dockerfile supports both amd64 and jetson architectures. +############################ +ARG UBUNTU_BASE=ubuntu:22.04 +ARG L4T_BASE=nvcr.io/nvidia/l4t-jetpack:r35.3.1 +ARG TARGETARCH -WORKDIR /cprt_rover_24 -RUN colcon build --continue-on-error --cmake-args=-DCMAKE_BUILD_TYPE=Release --parallel-workers $(nproc) \ - --build-base build-$aarch \ - --install-base install-$(uname -m) +# Define arch-specific stages +FROM ${UBUNTU_BASE} AS base_amd64 +FROM ${L4T_BASE} AS base_arm64 -FROM base AS runtime +# Select the right one for the current build target +FROM base_${TARGETARCH} AS base +RUN case "${TARGETARCH}" in amd64|arm64) ;; \ + *) echo "Unsupported TARGETARCH=${TARGETARCH}" && exit 1 ;; esac -# Create entrypoint script + +############################ +# Stage 1: Minimal ROS 2 Base +############################ +FROM base AS ros2_humble-base + +ENV DEBIAN_FRONTEND=noninteractive +ENV ROS_DISTRO=humble +ENV ROS_VERSION=2 +ENV ROS_PYTHON_VERSION=3 +ENV ROS_LOCALHOST_ONLY=0 +ENV ROS_DOMAIN_ID=0 + +# Locales +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && apt-get install -y locales apt-utils \ + && locale-gen en_US.UTF-8 \ + && update-locale LANG=en_US.UTF-8 LC_ALL=en_US.UTF-8 \ + && rm -rf /var/lib/apt/lists/* + +ENV LANG=en_US.UTF-8 +ENV LANGUAGE=en_US:en +ENV LC_ALL=en_US.UTF-8 + +# Base packages +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && apt-get install -y --no-install-recommends \ + curl lsb-release gnupg2 libc6-dev software-properties-common \ + iproute2 busybox && \ + apt-get clean && rm -rf /var/lib/apt/lists/* + +# 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=$(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 + +# Install ROS 2 base +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && apt-get upgrade -y && \ + apt-get install -y --no-install-recommends \ + python3-dev python3-pip ros-humble-ros-base && \ + apt-get clean && rm -rf /var/lib/apt/lists/* + +# Python packages +COPY requirements.txt /tmp/requirements.txt +RUN pip3 install --no-cache-dir -r /tmp/requirements.txt + +############################ +# Stage 2: ROS Deps Scan +############################ +FROM ros2_humble-base AS ros2_humble-rosdeps +ARG DIR=/cprt_rover_24 +WORKDIR ${DIR} + +# Install rosdep and dev tools for scanning +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && apt-get install -y --no-install-recommends \ + python3-rosdep && \ + apt-get clean && rm -rf /var/lib/apt/lists/* + +COPY src/**/package.xml src/**/setup.py src/**/CMakeLists.txt ./src + +ARG FILE=/opt/rosdeps.sh +RUN apt-get update && \ + rosdep init && \ + rosdep update && \ + rosdep install -i -r -y --from-paths src -s > ${FILE} + +############################ +# Stage 3: GStreamer Build +############################ +FROM base AS ros2_humble-gstreamer + +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + 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 \ + git ninja-build curl && \ + apt-get clean + +RUN python3 -m pip install --upgrade pip && \ + pip3 install --user meson + +WORKDIR /gstreamer +RUN git clone https://gitlab.freedesktop.org/gstreamer/gstreamer.git && \ + cd gstreamer && \ + 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 + +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 + +RUN 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 && \ + cd gst-plugins-rs && \ + cargo install cargo-c && \ + 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 4: Minimal Runtime Base +############################ +FROM ros2_humble-base AS runtime +ARG ROSDEP_FILE=/opt/rosdeps.sh +COPY --from=ros2_humble-rosdeps ${ROSDEP_FILE} ${ROSDEP_FILE} +COPY --from=ros2_humble-gstreamer /target / + +# Setup GStreamer environment +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 +ARG TARGETARCH +RUN if [ "$TARGETARCH" = "arm64" ]; then \ + echo 'export GST_PLUGIN_PATH=/opt/gstreamer/lib/gstreamer-1.0:/usr/lib/aarch64-linux-gnu/gstreamer-1.0' >> /etc/profile.d/gst.sh; \ + else \ + echo 'export GST_PLUGIN_PATH=/opt/gstreamer/lib/gstreamer-1.0:/usr/lib/x86_64-linux-gnu/gstreamer-1.0' >> /etc/profile.d/gst.sh; \ + fi + + +# Install rosdep dependencies +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && chmod +x ${ROSDEP_FILE} && ${ROSDEP_FILE} && \ + rm -rf /var/lib/apt/lists/* + +CMD ["/bin/bash"] + +############################ +# Stage 5: Full Dev Environment +############################ +FROM runtime AS dev + +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && apt-get install -y --no-install-recommends \ + git x11-apps ros-humble-desktop ros-dev-tools \ + ros-humble-ament-cmake python3-colcon-common-extensions \ + python3-colcon-ros clang-format && \ + apt-get clean && rm -rf /var/lib/apt/lists/* + +# Entry point for ROS 2 dev RUN echo '#!/bin/bash' > /entrypoint.sh && \ echo 'source /opt/ros/humble/setup.bash' >> /entrypoint.sh && \ - echo 'source /cprt_rover_24/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 -COPY --from=builder /cprt_rover_24/install-$(uname -m)/* /cprt_rover_24 -# Set entrypoint -ENTRYPOINT ["/entrypoint.sh"] \ No newline at end of file +RUN useradd -ms /bin/bash -u 1000 vscode && \ + echo "vscode ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers + +ENTRYPOINT ["/entrypoint.sh"] +CMD ["/bin/bash"] + +############################ +# Stage 6: Build Workspace +############################ +FROM dev AS builder +ARG DIR=/cprt_rover_24 +WORKDIR ${DIR} + +# Copy source code into the builder +COPY src/ ${DIR}/src/ + +# Install any additional dependencies if needed +RUN rosdep install -i -r -y --from-paths src + +# Build the ROS 2 workspace +RUN --mount=type=cache,target=${DIR}/colcon-cache \ + colcon build --symlink-install + +############################ +# Stage 7: Runtime Application +############################ +FROM runtime AS app +ARG DIR=/cprt_rover_24 +WORKDIR ${DIR} + +# Copy built workspace from builder +COPY --from=builder ${DIR}/install ${DIR}/install + +# Set environment for ROS 2 +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"] \ No newline at end of file diff --git a/docker/Dockerfile.base b/docker/Dockerfile.base deleted file mode 100644 index b7112e18..00000000 --- a/docker/Dockerfile.base +++ /dev/null @@ -1,156 +0,0 @@ -ARG IMAGE=ubuntu:22.04 - -FROM ${IMAGE} AS ros2_humble-base - -ENV DEBIAN_FRONTEND=noninteractive -ENV ROS_DISTRO=humble -ENV ROS_VERSION=2 -ENV ROS_PYTHON_VERSION=3 -ENV ROS_LOCALHOST_ONLY=0 -ENV ROS_DOMAIN_ID=0 - -RUN apt-get update && apt-get install -y locales apt-utils \ - && locale-gen en_US.UTF-8 \ - && update-locale LANG=en_US.UTF-8 LC_ALL=en_US.UTF-8 \ - && rm -rf /var/lib/apt/lists/* - -ENV LANG=en_US.UTF-8 -ENV LANGUAGE=en_US:en -ENV LC_ALL=en_US.UTF-8 - -# Install base packages -RUN apt-get update && apt-get install -y --no-install-recommends \ - curl \ - lsb-release \ - gnupg2 \ - libc6-dev \ - software-properties-common \ - iproute2 \ - busybox && \ - 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=$(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 - -# Install ROS 2 -RUN apt-get update && apt-get upgrade -y && \ - apt-get install -y --no-install-recommends \ - python3-dev \ - python3-pip \ - ros-humble-ros-base && \ - 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 \ - black \ - RPi.GPIO \ - rpi-hardware-pwm \ - Jetson.GPIO - - -FROM ros2_humble-base AS rosdeps - -RUN apt-get update && apt-get upgrade -y && \ - apt-get install -y --no-install-recommends \ - ros-humble-desktop \ - ros-dev-tools \ - python3-rosdep && \ - apt-get clean && \ - rm -rf /var/lib/apt/lists/* - -# Set project directory -ARG DIR=/cprt_rover_24 -WORKDIR ${DIR} - -# Copy workspace into container -COPY src/**/package.xml src/**/setup.py src/**/CMakeLists.txt ./src - -ARG FILE=/opt/rosdeps.sh - -# Generate list of rosdep dependencies to be installed later -RUN apt-get update && \ - rosdep init && \ - rosdep update && \ - rosdep install -i -r -y --from-paths src -s > ${FILE} - -FROM ${IMAGE} AS gstreamer - -RUN 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 \ - git \ - ninja-build \ - curl - -RUN python3 -m pip install --upgrade pip && \ - pip3 install --user meson - -WORKDIR /gstreamer - -# Install GStreamer and plugins -RUN git clone https://gitlab.freedesktop.org/gstreamer/gstreamer.git && \ - cd gstreamer && \ - 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 - -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 - -RUN 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 && \ - cd gst-plugins-rs && \ - cargo install cargo-c && \ - 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 - -FROM ros2_humble-base AS base -ARG ROSDEP_FILE=/opt/rosdeps.sh -COPY --from=rosdeps ${ROSDEP_FILE} ${ROSDEP_FILE} -COPY --from=gstreamer /target / -COPY setup/enablecan.sh /usr/local/bin/enablecan.sh - -# Setup gstreamer enviorment -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 -ENV GST_PLUGIN_PATH=/opt/gstreamer/lib/gstreamer-1.0:/usr/lib/aarch64-linux-gnu/gstreamer-1.0 - -# Install rosdep dependencies -RUN apt-get update && \ - chmod +x ${ROSDEP_FILE} && \ - ${ROSDEP_FILE} && \ - rm -rf /var/lib/apt/lists/* - -CMD ["/bin/bash"] diff --git a/docker/Dockerfile.dev b/docker/Dockerfile.dev deleted file mode 100644 index 3b2a9329..00000000 --- a/docker/Dockerfile.dev +++ /dev/null @@ -1,57 +0,0 @@ -ARG IMAGE -FROM ${IMAGE} as ros2_humble-base - -FROM ros2_humble-base as ros2_humble-dev - -RUN apt-get update && apt-get install -y locales apt-utils \ - && locale-gen en_US.UTF-8 \ - && update-locale LANG=en_US.UTF-8 LC_ALL=en_US.UTF-8 \ - && rm -rf /var/lib/apt/lists/* - -ENV LANG=en_US.UTF-8 -ENV LANGUAGE=en_US:en -ENV LC_ALL=en_US.UTF-8 - -# Install base packages -RUN apt-get update && apt-get install -y --no-install-recommends \ - git \ - curl \ - lsb-release \ - gnupg2 \ - libc6-dev \ - x11-apps \ - software-properties-common && \ - apt-get clean && \ - rm -rf /var/lib/apt/lists/* - -# Install ROS 2 and development tools -RUN apt-get update && apt-get upgrade -y && \ - apt-get install -y --no-install-recommends \ - ros-humble-desktop \ - ros-dev-tools \ - ros-humble-ament-cmake \ - python3-dev \ - python3-pip \ - ninja-build \ - python3-rosdep \ - python3-colcon-common-extensions \ - python3-colcon-ros \ - clang-format && \ - apt-get clean && \ - rm -rf /var/lib/apt/lists/* - -FROM ros2_humble-dev as final - -# 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 - -RUN useradd -ms /bin/bash -u 1000 vscode \ - && echo "vscode ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers - -# Set entrypoint -ENTRYPOINT ["/entrypoint.sh"] -CMD ["/bin/bash"] diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 00000000..fc29773b --- /dev/null +++ b/requirements.txt @@ -0,0 +1,14 @@ +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 +black +RPi.GPIO +rpi-hardware-pwm +Jetson.GPIO From ef9e7ca812167a477aacae3458af2d27fc1d32d3 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Fri, 15 Aug 2025 00:18:52 -0400 Subject: [PATCH 03/36] try this load of shit --- .github/workflows/build-docker.yml | 68 +++++++++++++----------------- Dockerfile | 12 ++---- 2 files changed, 34 insertions(+), 46 deletions(-) diff --git a/.github/workflows/build-docker.yml b/.github/workflows/build-docker.yml index 4a430f41..91ef6784 100644 --- a/.github/workflows/build-docker.yml +++ b/.github/workflows/build-docker.yml @@ -1,4 +1,4 @@ -name: Docker CI/CD +name: Docker CI/CD Multi-Arch on: push: @@ -9,9 +9,8 @@ on: jobs: build: runs-on: ubuntu-latest - strategy: - matrix: - arch: [amd64, arm64] + env: + IMAGE_NAME: cprtsoftware/rover steps: - name: Checkout code @@ -25,49 +24,42 @@ jobs: - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 - - name: Cache Docker layers - uses: actions/cache@v3 + - name: Log in to Docker Hub + uses: docker/login-action@v2 with: - path: /tmp/.buildx-cache - key: ${{ runner.os }}-buildx-${{ matrix.arch }}-${{ hashFiles('Dockerfile', 'requirements.txt') }} - restore-keys: | - ${{ runner.os }}-buildx-${{ matrix.arch }}- + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Build base image + - name: Build amd64 image run: | docker buildx build \ - --platform linux/${{ matrix.arch }} \ - --target ros2_humble-base \ - --build-arg TARGETARCH=${{ matrix.arch }} \ - --cache-to type=local,dest=/tmp/.buildx-cache,mode=max \ - --cache-from type=local,src=/tmp/.buildx-cache \ - -t cprtsoftware/ros2_humble-base:latest \ - -t cprtsoftware/ros2_humble-base:${{ github.sha }} \ + --platform linux/amd64 \ + --build-arg TARGETARCH=amd64 \ + -t $IMAGE_NAME:amd64 \ + -t $IMAGE_NAME:${{ github.sha }}-amd64 \ + --load \ . - - name: Build dev image + - name: Build arm64 image run: | docker buildx build \ - --platform linux/${{ matrix.arch }} \ - --target dev \ - --build-arg TARGETARCH=${{ matrix.arch }} \ - --cache-to type=local,dest=/tmp/.buildx-cache,mode=max \ - --cache-from type=local,src=/tmp/.buildx-cache \ - -t cprtsoftware/ros2_humble-dev:latest \ - -t cprtsoftware/ros2_humble-dev:${{ github.sha }} \ + --platform linux/arm64 \ + --build-arg TARGETARCH=arm64 \ + -t $IMAGE_NAME:arm64 \ + -t $IMAGE_NAME:${{ github.sha }}-arm64 \ + --load \ . - - name: Log in to Docker Hub - if: github.event_name == 'push' - uses: docker/login-action@v2 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} + - name: Create multi-arch manifest for latest + run: | + docker manifest create $IMAGE_NAME:latest \ + $IMAGE_NAME:amd64 \ + $IMAGE_NAME:arm64 + docker manifest push $IMAGE_NAME:latest - - name: Push images - if: github.event_name == 'push' + - name: Create multi-arch manifest for commit SHA run: | - docker push cprtsoftware/ros2_humble-base:latest - docker push cprtsoftware/ros2_humble-base:${{ github.sha }} - docker push cprtsoftware/ros2_humble-dev:latest - docker push cprtsoftware/ros2_humble-dev:${{ github.sha }} + docker manifest create $IMAGE_NAME:${{ github.sha }} \ + $IMAGE_NAME:${{ github.sha }}-amd64 \ + $IMAGE_NAME:${{ github.sha }}-arm64 + docker manifest push $IMAGE_NAME:${{ github.sha }} diff --git a/Dockerfile b/Dockerfile index 8ced5cc7..b8a45711 100644 --- a/Dockerfile +++ b/Dockerfile @@ -4,20 +4,16 @@ # Pick the Base Image # This Dockerfile supports both amd64 and jetson architectures. ############################ +ARG TARGETARCH ARG UBUNTU_BASE=ubuntu:22.04 ARG L4T_BASE=nvcr.io/nvidia/l4t-jetpack:r35.3.1 -ARG TARGETARCH - -# Define arch-specific stages +# Base image selection per arch FROM ${UBUNTU_BASE} AS base_amd64 -FROM ${L4T_BASE} AS base_arm64 +FROM ${L4T_BASE} AS base_arm64 -# Select the right one for the current build target +# Select the correct base FROM base_${TARGETARCH} AS base -RUN case "${TARGETARCH}" in amd64|arm64) ;; \ - *) echo "Unsupported TARGETARCH=${TARGETARCH}" && exit 1 ;; esac - ############################ # Stage 1: Minimal ROS 2 Base From c2490df6388ced028820dded4bc98879eb33aee3 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Fri, 15 Aug 2025 00:27:37 -0400 Subject: [PATCH 04/36] more shit --- .github/workflows/lint-and-format.yml | 3 - Dockerfile | 131 +++++++++----------------- 2 files changed, 47 insertions(+), 87 deletions(-) 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/Dockerfile b/Dockerfile index b8a45711..abf54f8b 100644 --- a/Dockerfile +++ b/Dockerfile @@ -8,10 +8,11 @@ ARG TARGETARCH ARG UBUNTU_BASE=ubuntu:22.04 ARG L4T_BASE=nvcr.io/nvidia/l4t-jetpack:r35.3.1 -# Base image selection per arch +############################ +# Stage 0: Base Image per Arch +############################ FROM ${UBUNTU_BASE} AS base_amd64 -FROM ${L4T_BASE} AS base_arm64 - +FROM ${L4T_BASE} AS base_arm64 # Select the correct base FROM base_${TARGETARCH} AS base @@ -22,32 +23,25 @@ FROM base AS ros2_humble-base ENV DEBIAN_FRONTEND=noninteractive ENV ROS_DISTRO=humble -ENV ROS_VERSION=2 -ENV ROS_PYTHON_VERSION=3 -ENV ROS_LOCALHOST_ONLY=0 -ENV ROS_DOMAIN_ID=0 - -# Locales -RUN --mount=type=cache,target=/var/cache/apt \ - --mount=type=cache,target=/var/lib/apt \ +ENV LANG=en_US.UTF-8 +ENV LANGUAGE=en_US:en +ENV LC_ALL=en_US.UTF-8 + +# Cache apt archives only, avoid /var/lib/apt lock issues +RUN --mount=type=cache,target=/var/cache/apt/archives \ apt-get update && apt-get install -y locales apt-utils \ && locale-gen en_US.UTF-8 \ && update-locale LANG=en_US.UTF-8 LC_ALL=en_US.UTF-8 \ && rm -rf /var/lib/apt/lists/* -ENV LANG=en_US.UTF-8 -ENV LANGUAGE=en_US:en -ENV LC_ALL=en_US.UTF-8 - # Base packages -RUN --mount=type=cache,target=/var/cache/apt \ - --mount=type=cache,target=/var/lib/apt \ +RUN --mount=type=cache,target=/var/cache/apt/archives \ apt-get update && apt-get install -y --no-install-recommends \ - curl lsb-release gnupg2 libc6-dev software-properties-common \ - iproute2 busybox && \ + curl lsb-release gnupg2 libc6-dev software-properties-common \ + iproute2 busybox && \ apt-get clean && rm -rf /var/lib/apt/lists/* -# ROS 2 repository +# 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] \ @@ -55,37 +49,29 @@ RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ > /etc/apt/sources.list.d/ros2.list # Install ROS 2 base -RUN --mount=type=cache,target=/var/cache/apt \ - --mount=type=cache,target=/var/lib/apt \ - apt-get update && apt-get upgrade -y && \ - apt-get install -y --no-install-recommends \ - python3-dev python3-pip ros-humble-ros-base && \ +RUN --mount=type=cache,target=/var/cache/apt/archives \ + apt-get update && apt-get install -y --no-install-recommends \ + python3-dev python3-pip ros-humble-ros-base && \ apt-get clean && rm -rf /var/lib/apt/lists/* -# Python packages -COPY requirements.txt /tmp/requirements.txt -RUN pip3 install --no-cache-dir -r /tmp/requirements.txt +# Python dependencies +COPY requirements.txt . +RUN pip3 install --no-cache-dir -r requirements.txt ############################ -# Stage 2: ROS Deps Scan +# Stage 2: ROS deps scan ############################ FROM ros2_humble-base AS ros2_humble-rosdeps ARG DIR=/cprt_rover_24 WORKDIR ${DIR} -# Install rosdep and dev tools for scanning -RUN --mount=type=cache,target=/var/cache/apt \ - --mount=type=cache,target=/var/lib/apt \ - apt-get update && apt-get install -y --no-install-recommends \ - python3-rosdep && \ +RUN --mount=type=cache,target=/var/cache/apt/archives \ + apt-get update && apt-get install -y python3-rosdep && \ apt-get clean && rm -rf /var/lib/apt/lists/* COPY src/**/package.xml src/**/setup.py src/**/CMakeLists.txt ./src - ARG FILE=/opt/rosdeps.sh -RUN apt-get update && \ - rosdep init && \ - rosdep update && \ +RUN rosdep init && rosdep update && \ rosdep install -i -r -y --from-paths src -s > ${FILE} ############################ @@ -93,16 +79,14 @@ RUN apt-get update && \ ############################ FROM base AS ros2_humble-gstreamer -RUN --mount=type=cache,target=/var/cache/apt \ - --mount=type=cache,target=/var/lib/apt \ +RUN --mount=type=cache,target=/var/cache/apt/archives \ 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 \ git ninja-build curl && \ - apt-get clean + apt-get clean && rm -rf /var/lib/apt/lists/* -RUN python3 -m pip install --upgrade pip && \ - pip3 install --user meson +RUN python3 -m pip install --upgrade --user pip meson WORKDIR /gstreamer RUN git clone https://gitlab.freedesktop.org/gstreamer/gstreamer.git && \ @@ -134,55 +118,38 @@ FROM ros2_humble-base AS runtime ARG ROSDEP_FILE=/opt/rosdeps.sh COPY --from=ros2_humble-rosdeps ${ROSDEP_FILE} ${ROSDEP_FILE} COPY --from=ros2_humble-gstreamer /target / - # Setup GStreamer environment -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 -ARG TARGETARCH -RUN if [ "$TARGETARCH" = "arm64" ]; then \ - echo 'export GST_PLUGIN_PATH=/opt/gstreamer/lib/gstreamer-1.0:/usr/lib/aarch64-linux-gnu/gstreamer-1.0' >> /etc/profile.d/gst.sh; \ - else \ - echo 'export GST_PLUGIN_PATH=/opt/gstreamer/lib/gstreamer-1.0:/usr/lib/x86_64-linux-gnu/gstreamer-1.0' >> /etc/profile.d/gst.sh; \ - fi - +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 +RUN --mount=type=cache,target=/var/cache/rosdeps \ + chmod +x ${ROSDEP_FILE} && \ + ${ROSDEP_FILE} -# Install rosdep dependencies -RUN --mount=type=cache,target=/var/cache/apt \ - --mount=type=cache,target=/var/lib/apt \ - apt-get update && chmod +x ${ROSDEP_FILE} && ${ROSDEP_FILE} && \ - rm -rf /var/lib/apt/lists/* CMD ["/bin/bash"] ############################ -# Stage 5: Full Dev Environment +# Stage 5: Dev Environment ############################ FROM runtime AS dev - -RUN --mount=type=cache,target=/var/cache/apt \ - --mount=type=cache,target=/var/lib/apt \ +RUN --mount=type=cache,target=/var/cache/apt/archives \ apt-get update && apt-get install -y --no-install-recommends \ - git x11-apps ros-humble-desktop ros-dev-tools \ - ros-humble-ament-cmake python3-colcon-common-extensions \ - python3-colcon-ros clang-format && \ + git x11-apps ros-humble-desktop ros-dev-tools \ + ros-humble-ament-cmake python3-colcon-common-extensions \ + python3-colcon-ros clang-format && \ apt-get clean && rm -rf /var/lib/apt/lists/* -# Entry point for ROS 2 dev -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 - -RUN useradd -ms /bin/bash -u 1000 vscode && \ - echo "vscode ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers +# Entrypoint +RUN echo '#!/bin/bash\nsource /opt/ros/humble/setup.bash\nif [ -f /cprt_rover_24/install/setup.bash ]; then source /cprt_rover_24/install/setup.bash; fi\nexec "$@"' > /entrypoint.sh \ + && chmod +x /entrypoint.sh +RUN useradd -ms /bin/bash -u 1000 vscode && echo "vscode ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers ENTRYPOINT ["/entrypoint.sh"] CMD ["/bin/bash"] ############################ -# Stage 6: Build Workspace +# Stage 6: Builder ############################ FROM dev AS builder ARG DIR=/cprt_rover_24 @@ -201,15 +168,11 @@ RUN --mount=type=cache,target=${DIR}/colcon-cache \ ############################ # Stage 7: Runtime Application ############################ -FROM runtime AS app +FROM runtime AS rover ARG DIR=/cprt_rover_24 WORKDIR ${DIR} - -# Copy built workspace from builder 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 -# Set environment for ROS 2 -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"] \ No newline at end of file +CMD ["/bin/bash"] From 2a5450db6a0359a82a4f2e93499cb9f18055946b Mon Sep 17 00:00:00 2001 From: ConnorN Date: Fri, 15 Aug 2025 00:34:13 -0400 Subject: [PATCH 05/36] try again (fuck me) --- Dockerfile | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/Dockerfile b/Dockerfile index abf54f8b..b6e1cbfb 100644 --- a/Dockerfile +++ b/Dockerfile @@ -28,19 +28,21 @@ ENV LANGUAGE=en_US:en ENV LC_ALL=en_US.UTF-8 # Cache apt archives only, avoid /var/lib/apt lock issues -RUN --mount=type=cache,target=/var/cache/apt/archives \ - apt-get update && apt-get install -y locales apt-utils \ +RUN --mount=type=cache,target=/var/cache/apt/archives-base \ + apt-get update && apt-get install -y \ + locales \ + apt-utils \ + curl \ + lsb-release \ + gnupg2 \ + libc6-dev \ + software-properties-common \ + iproute2 \ + busybox \ && locale-gen en_US.UTF-8 \ && update-locale LANG=en_US.UTF-8 LC_ALL=en_US.UTF-8 \ && rm -rf /var/lib/apt/lists/* -# Base packages -RUN --mount=type=cache,target=/var/cache/apt/archives \ - apt-get update && apt-get install -y --no-install-recommends \ - curl lsb-release gnupg2 libc6-dev software-properties-common \ - iproute2 busybox && \ - apt-get clean && rm -rf /var/lib/apt/lists/* - # ROS 2 repo RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ -o /usr/share/keyrings/ros-archive-keyring.gpg && \ @@ -49,7 +51,7 @@ RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ > /etc/apt/sources.list.d/ros2.list # Install ROS 2 base -RUN --mount=type=cache,target=/var/cache/apt/archives \ +RUN --mount=type=cache,target=/var/cache/apt/archives-base \ apt-get update && apt-get install -y --no-install-recommends \ python3-dev python3-pip ros-humble-ros-base && \ apt-get clean && rm -rf /var/lib/apt/lists/* @@ -65,7 +67,7 @@ FROM ros2_humble-base AS ros2_humble-rosdeps ARG DIR=/cprt_rover_24 WORKDIR ${DIR} -RUN --mount=type=cache,target=/var/cache/apt/archives \ +RUN --mount=type=cache,target=/var/cache/apt/archives-rosdeps \ apt-get update && apt-get install -y python3-rosdep && \ apt-get clean && rm -rf /var/lib/apt/lists/* @@ -79,7 +81,7 @@ RUN rosdep init && rosdep update && \ ############################ FROM base AS ros2_humble-gstreamer -RUN --mount=type=cache,target=/var/cache/apt/archives \ +RUN --mount=type=cache,target=/var/cache/apt/archives-gst \ 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 \ @@ -133,7 +135,7 @@ CMD ["/bin/bash"] # Stage 5: Dev Environment ############################ FROM runtime AS dev -RUN --mount=type=cache,target=/var/cache/apt/archives \ +RUN --mount=type=cache,target=/var/cache/apt/archives-runtime \ apt-get update && apt-get install -y --no-install-recommends \ git x11-apps ros-humble-desktop ros-dev-tools \ ros-humble-ament-cmake python3-colcon-common-extensions \ From 4788405c94effb3c6bcb1f632d539eb8d16ba0c6 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Fri, 15 Aug 2025 00:40:09 -0400 Subject: [PATCH 06/36] insert curse word here --- Dockerfile | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/Dockerfile b/Dockerfile index b6e1cbfb..10cc6dba 100644 --- a/Dockerfile +++ b/Dockerfile @@ -29,18 +29,23 @@ ENV LC_ALL=en_US.UTF-8 # Cache apt archives only, avoid /var/lib/apt lock issues RUN --mount=type=cache,target=/var/cache/apt/archives-base \ - apt-get update && apt-get install -y \ - locales \ - apt-utils \ - curl \ - lsb-release \ - gnupg2 \ - libc6-dev \ - software-properties-common \ - iproute2 \ - busybox \ + apt-get update && apt-get install -y --no-install-recommends \ + locales \ + apt-utils \ + curl \ + lsb-release \ + gnupg2 \ + libc6-dev \ + software-properties-common \ + iproute2 \ + busybox \ + build-essential \ + python3-dev \ + python3-setuptools \ + python3-wheel \ && locale-gen en_US.UTF-8 \ && update-locale LANG=en_US.UTF-8 LC_ALL=en_US.UTF-8 \ + && apt-get clean \ && rm -rf /var/lib/apt/lists/* # ROS 2 repo From e0678d53f4df31f3652ad6a82346d7848261d2ed Mon Sep 17 00:00:00 2001 From: ConnorN Date: Fri, 15 Aug 2025 01:03:29 -0400 Subject: [PATCH 07/36] FUCK --- .github/workflows/build-docker.yml | 54 +++++++++++++++++++----- Dockerfile | 68 ++++++++++++------------------ 2 files changed, 71 insertions(+), 51 deletions(-) diff --git a/.github/workflows/build-docker.yml b/.github/workflows/build-docker.yml index 91ef6784..5a9b8ebd 100644 --- a/.github/workflows/build-docker.yml +++ b/.github/workflows/build-docker.yml @@ -7,25 +7,27 @@ on: branches: [ main ] jobs: - build: + setup: runs-on: ubuntu-latest - env: + outputs: IMAGE_NAME: cprtsoftware/rover + steps: + - name: Set image name + run: echo "IMAGE_NAME=cprtsoftware/rover" >> $GITHUB_ENV + build-amd64: + runs-on: ubuntu-latest + needs: setup steps: - - name: Checkout code - uses: actions/checkout@v4 + - uses: actions/checkout@v4 - - name: Set up QEMU - uses: docker/setup-qemu-action@v3 + - uses: docker/setup-qemu-action@v3 with: - platforms: linux/amd64,linux/arm64 + platforms: linux/amd64 - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v3 + - uses: docker/setup-buildx-action@v3 - - name: Log in to Docker Hub - uses: docker/login-action@v2 + - uses: docker/login-action@v2 with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} @@ -37,9 +39,28 @@ jobs: --build-arg TARGETARCH=amd64 \ -t $IMAGE_NAME:amd64 \ -t $IMAGE_NAME:${{ github.sha }}-amd64 \ + --cache-from=type=registry,ref=$IMAGE_NAME:amd64-cache \ + --cache-to=type=registry,ref=$IMAGE_NAME:amd64-cache,mode=max \ --load \ . + build-arm64: + runs-on: ubuntu-latest + needs: setup + steps: + - uses: actions/checkout@v4 + + - uses: docker/setup-qemu-action@v3 + with: + platforms: linux/arm64 + + - uses: docker/setup-buildx-action@v3 + + - uses: docker/login-action@v2 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + - name: Build arm64 image run: | docker buildx build \ @@ -47,9 +68,20 @@ jobs: --build-arg TARGETARCH=arm64 \ -t $IMAGE_NAME:arm64 \ -t $IMAGE_NAME:${{ github.sha }}-arm64 \ + --cache-from=type=registry,ref=$IMAGE_NAME:arm64-cache \ + --cache-to=type=registry,ref=$IMAGE_NAME:arm64-cache,mode=max \ --load \ . + push-manifest: + runs-on: ubuntu-latest + needs: [build-amd64, build-arm64] + steps: + - uses: docker/login-action@v2 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + - name: Create multi-arch manifest for latest run: | docker manifest create $IMAGE_NAME:latest \ diff --git a/Dockerfile b/Dockerfile index 10cc6dba..42c59494 100644 --- a/Dockerfile +++ b/Dockerfile @@ -27,26 +27,14 @@ ENV LANG=en_US.UTF-8 ENV LANGUAGE=en_US:en ENV LC_ALL=en_US.UTF-8 -# Cache apt archives only, avoid /var/lib/apt lock issues -RUN --mount=type=cache,target=/var/cache/apt/archives-base \ +# Per-arch APT cache +RUN --mount=type=cache,target=/var/cache/apt/archives-${TARGETARCH} \ apt-get update && apt-get install -y --no-install-recommends \ - locales \ - apt-utils \ - curl \ - lsb-release \ - gnupg2 \ - libc6-dev \ - software-properties-common \ - iproute2 \ - busybox \ - build-essential \ - python3-dev \ - python3-setuptools \ - python3-wheel \ + locales apt-utils curl lsb-release gnupg2 \ + software-properties-common build-essential python3-dev python3-pip \ && locale-gen en_US.UTF-8 \ && update-locale LANG=en_US.UTF-8 LC_ALL=en_US.UTF-8 \ - && apt-get clean \ - && rm -rf /var/lib/apt/lists/* + && apt-get clean && rm -rf /var/lib/apt/lists/* # ROS 2 repo RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ @@ -55,15 +43,15 @@ RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" \ > /etc/apt/sources.list.d/ros2.list -# Install ROS 2 base -RUN --mount=type=cache,target=/var/cache/apt/archives-base \ +RUN --mount=type=cache,target=/var/cache/apt/archives-${TARGETARCH} \ apt-get update && apt-get install -y --no-install-recommends \ - python3-dev python3-pip ros-humble-ros-base && \ - apt-get clean && rm -rf /var/lib/apt/lists/* + ros-humble-ros-base python3-setuptools python3-wheel \ + && apt-get clean && rm -rf /var/lib/apt/lists/* -# Python dependencies +# Python dependencies (per-arch pip cache) COPY requirements.txt . -RUN pip3 install --no-cache-dir -r requirements.txt +RUN --mount=type=cache,target=/root/.cache/pip \ + pip3 install --no-cache-dir -r requirements.txt ############################ # Stage 2: ROS deps scan @@ -72,9 +60,9 @@ FROM ros2_humble-base AS ros2_humble-rosdeps ARG DIR=/cprt_rover_24 WORKDIR ${DIR} -RUN --mount=type=cache,target=/var/cache/apt/archives-rosdeps \ - apt-get update && apt-get install -y python3-rosdep && \ - apt-get clean && rm -rf /var/lib/apt/lists/* +RUN --mount=type=cache,target=/var/cache/apt/archives-${TARGETARCH} \ + apt-get update && apt-get install -y python3-rosdep \ + && apt-get clean && rm -rf /var/lib/apt/lists/* COPY src/**/package.xml src/**/setup.py src/**/CMakeLists.txt ./src ARG FILE=/opt/rosdeps.sh @@ -86,7 +74,7 @@ RUN rosdep init && rosdep update && \ ############################ FROM base AS ros2_humble-gstreamer -RUN --mount=type=cache,target=/var/cache/apt/archives-gst \ +RUN --mount=type=cache,target=/var/cache/apt/archives-${TARGETARCH} \ 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 \ @@ -96,19 +84,23 @@ RUN --mount=type=cache,target=/var/cache/apt/archives-gst \ RUN python3 -m pip install --upgrade --user pip meson WORKDIR /gstreamer -RUN git clone https://gitlab.freedesktop.org/gstreamer/gstreamer.git && \ +RUN --mount=type=cache,target=/root/.cache/gstreamer \ + --mount=type=cache,target=/root/.cache/meson \ + git clone https://gitlab.freedesktop.org/gstreamer/gstreamer.git && \ cd gstreamer && \ 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 +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 -RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y && \ +RUN --mount=type=cache,target=/root/.cargo \ + 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 && \ cd gst-plugins-rs && \ @@ -125,14 +117,12 @@ FROM ros2_humble-base AS runtime ARG ROSDEP_FILE=/opt/rosdeps.sh COPY --from=ros2_humble-rosdeps ${ROSDEP_FILE} ${ROSDEP_FILE} COPY --from=ros2_humble-gstreamer /target / -# Setup GStreamer environment 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 -RUN --mount=type=cache,target=/var/cache/rosdeps \ - chmod +x ${ROSDEP_FILE} && \ - ${ROSDEP_FILE} +RUN --mount=type=cache,target=/var/cache/apt/archives-${TARGETARCH} \ + chmod +x ${ROSDEP_FILE} && ${ROSDEP_FILE} CMD ["/bin/bash"] @@ -140,7 +130,7 @@ CMD ["/bin/bash"] # Stage 5: Dev Environment ############################ FROM runtime AS dev -RUN --mount=type=cache,target=/var/cache/apt/archives-runtime \ +RUN --mount=type=cache,target=/var/cache/apt/archives-${TARGETARCH} \ apt-get update && apt-get install -y --no-install-recommends \ git x11-apps ros-humble-desktop ros-dev-tools \ ros-humble-ament-cmake python3-colcon-common-extensions \ @@ -162,13 +152,11 @@ FROM dev AS builder ARG DIR=/cprt_rover_24 WORKDIR ${DIR} -# Copy source code into the builder COPY src/ ${DIR}/src/ -# Install any additional dependencies if needed RUN rosdep install -i -r -y --from-paths src -# Build the ROS 2 workspace +# Cache colcon builds RUN --mount=type=cache,target=${DIR}/colcon-cache \ colcon build --symlink-install From 0d2a7ce940c1c10b783ff22f761023f891f30788 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Fri, 15 Aug 2025 01:05:40 -0400 Subject: [PATCH 08/36] SHIT --- .github/workflows/build-docker.yml | 40 +++++++++++++++++++++--------- 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/.github/workflows/build-docker.yml b/.github/workflows/build-docker.yml index 5a9b8ebd..1050d6ad 100644 --- a/.github/workflows/build-docker.yml +++ b/.github/workflows/build-docker.yml @@ -6,28 +6,36 @@ on: pull_request: branches: [ main ] +env: + IMAGE_NAME: cprtsoftware/rover + jobs: setup: runs-on: ubuntu-latest outputs: - IMAGE_NAME: cprtsoftware/rover + IMAGE_NAME: ${{ env.IMAGE_NAME }} steps: - - name: Set image name - run: echo "IMAGE_NAME=cprtsoftware/rover" >> $GITHUB_ENV + - name: Checkout repository + uses: actions/checkout@v4 build-amd64: runs-on: ubuntu-latest needs: setup + env: + IMAGE_NAME: ${{ needs.setup.outputs.IMAGE_NAME }} steps: - uses: actions/checkout@v4 - - uses: docker/setup-qemu-action@v3 + - name: Set up QEMU + uses: docker/setup-qemu-action@v3 with: platforms: linux/amd64 - - uses: docker/setup-buildx-action@v3 + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 - - uses: docker/login-action@v2 + - name: Docker login + uses: docker/login-action@v2 with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} @@ -47,16 +55,21 @@ jobs: build-arm64: runs-on: ubuntu-latest needs: setup + env: + IMAGE_NAME: ${{ needs.setup.outputs.IMAGE_NAME }} steps: - uses: actions/checkout@v4 - - uses: docker/setup-qemu-action@v3 + - name: Set up QEMU + uses: docker/setup-qemu-action@v3 with: platforms: linux/arm64 - - uses: docker/setup-buildx-action@v3 + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 - - uses: docker/login-action@v2 + - name: Docker login + uses: docker/login-action@v2 with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} @@ -76,20 +89,23 @@ jobs: push-manifest: runs-on: ubuntu-latest needs: [build-amd64, build-arm64] + env: + IMAGE_NAME: ${{ needs.setup.outputs.IMAGE_NAME }} steps: - - uses: docker/login-action@v2 + - name: Docker login + uses: docker/login-action@v2 with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Create multi-arch manifest for latest + - name: Create and push multi-arch manifest (latest) run: | docker manifest create $IMAGE_NAME:latest \ $IMAGE_NAME:amd64 \ $IMAGE_NAME:arm64 docker manifest push $IMAGE_NAME:latest - - name: Create multi-arch manifest for commit SHA + - name: Create and push multi-arch manifest (commit SHA) run: | docker manifest create $IMAGE_NAME:${{ github.sha }} \ $IMAGE_NAME:${{ github.sha }}-amd64 \ From 5f8899bd9a7fe9475ea0f48b8331d5afa8a1d9a7 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Fri, 15 Aug 2025 01:11:09 -0400 Subject: [PATCH 09/36] ASS --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index 42c59494..086c9b03 100644 --- a/Dockerfile +++ b/Dockerfile @@ -6,7 +6,7 @@ ############################ ARG TARGETARCH ARG UBUNTU_BASE=ubuntu:22.04 -ARG L4T_BASE=nvcr.io/nvidia/l4t-jetpack:r35.3.1 +ARG L4T_BASE=nvcr.io/nvidia/l4t-jetpack:r36.4.0 ############################ # Stage 0: Base Image per Arch From 1cf3ab9c3f37b92dd63411a39b3eb3402bb739f9 Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Fri, 15 Aug 2025 12:30:35 -0400 Subject: [PATCH 10/36] try this again (still hate devops) --- Dockerfile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index 086c9b03..01fe27fc 100644 --- a/Dockerfile +++ b/Dockerfile @@ -154,7 +154,8 @@ WORKDIR ${DIR} COPY src/ ${DIR}/src/ -RUN rosdep install -i -r -y --from-paths src +RUN rosdep init && rosdep update && \ + rosdep install -i -r -y --from-paths src # Cache colcon builds RUN --mount=type=cache,target=${DIR}/colcon-cache \ From 81a973a0302a627cc68f9dd5ff7ef46b4af7b9ca Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Fri, 15 Aug 2025 12:44:13 -0400 Subject: [PATCH 11/36] 5.0 any better? --- .github/workflows/build-docker.yml | 98 +++++++++++++++--------------- 1 file changed, 50 insertions(+), 48 deletions(-) diff --git a/.github/workflows/build-docker.yml b/.github/workflows/build-docker.yml index 1050d6ad..826a075d 100644 --- a/.github/workflows/build-docker.yml +++ b/.github/workflows/build-docker.yml @@ -23,91 +23,93 @@ jobs: needs: setup env: IMAGE_NAME: ${{ needs.setup.outputs.IMAGE_NAME }} + outputs: + digest: ${{ steps.build.outputs.digest }} steps: - uses: actions/checkout@v4 - - name: Set up QEMU - uses: docker/setup-qemu-action@v3 - with: - platforms: linux/amd64 - - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v3 + - uses: docker/setup-buildx-action@v3 - name: Docker login - uses: docker/login-action@v2 + uses: docker/login-action@v3 with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Build amd64 image - run: | - docker buildx build \ - --platform linux/amd64 \ - --build-arg TARGETARCH=amd64 \ - -t $IMAGE_NAME:amd64 \ - -t $IMAGE_NAME:${{ github.sha }}-amd64 \ - --cache-from=type=registry,ref=$IMAGE_NAME:amd64-cache \ - --cache-to=type=registry,ref=$IMAGE_NAME:amd64-cache,mode=max \ - --load \ - . + - name: Build & push amd64 (with registry cache) + id: build + uses: docker/build-push-action@v6 + with: + context: . + file: ./Dockerfile + platforms: linux/amd64 + push: true + 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 + # Expose the pushed image digest for the manifest job + provenance: false build-arm64: runs-on: ubuntu-latest needs: setup env: IMAGE_NAME: ${{ needs.setup.outputs.IMAGE_NAME }} + outputs: + digest: ${{ steps.build.outputs.digest }} steps: - uses: actions/checkout@v4 - - name: Set up QEMU + - name: Set up QEMU (for arm emulation) uses: docker/setup-qemu-action@v3 with: platforms: linux/arm64 - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v3 + - uses: docker/setup-buildx-action@v3 - name: Docker login - uses: docker/login-action@v2 + uses: docker/login-action@v3 with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Build arm64 image - run: | - docker buildx build \ - --platform linux/arm64 \ - --build-arg TARGETARCH=arm64 \ - -t $IMAGE_NAME:arm64 \ - -t $IMAGE_NAME:${{ github.sha }}-arm64 \ - --cache-from=type=registry,ref=$IMAGE_NAME:arm64-cache \ - --cache-to=type=registry,ref=$IMAGE_NAME:arm64-cache,mode=max \ - --load \ - . + - name: Build & push arm64 (with registry cache) + id: build + uses: docker/build-push-action@v6 + with: + context: . + file: ./Dockerfile + platforms: linux/arm64 + push: true + 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 push-manifest: runs-on: ubuntu-latest - needs: [build-amd64, build-arm64] + needs: [build-amd64, build-arm64, setup] env: IMAGE_NAME: ${{ needs.setup.outputs.IMAGE_NAME }} steps: - name: Docker login - uses: docker/login-action@v2 + uses: docker/login-action@v3 with: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Create and push multi-arch manifest (latest) - run: | - docker manifest create $IMAGE_NAME:latest \ - $IMAGE_NAME:amd64 \ - $IMAGE_NAME:arm64 - docker manifest push $IMAGE_NAME:latest - - - name: Create and push multi-arch manifest (commit SHA) + # Build the multi-arch manifest from pushed image digests (recommended) + - name: Create and push multi-arch manifest (latest + commit) run: | - docker manifest create $IMAGE_NAME:${{ github.sha }} \ - $IMAGE_NAME:${{ github.sha }}-amd64 \ - $IMAGE_NAME:${{ github.sha }}-arm64 - docker manifest push $IMAGE_NAME:${{ github.sha }} + AMD64_DIGEST="${{ needs.build-amd64.outputs.digest }}" + ARM64_DIGEST="${{ needs.build-arm64.outputs.digest }}" + + docker buildx imagetools create \ + -t "$IMAGE_NAME:latest" \ + -t "$IMAGE_NAME:${{ github.sha }}" \ + "$IMAGE_NAME@${AMD64_DIGEST}" \ + "$IMAGE_NAME@${ARM64_DIGEST}" \ No newline at end of file From 08ff67aaecd48aa8c92620c6dbbbbef7c040455d Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Fri, 15 Aug 2025 12:49:32 -0400 Subject: [PATCH 12/36] try again --- Dockerfile | 75 ++++++++++++++++++++++++++++++++---------------------- 1 file changed, 45 insertions(+), 30 deletions(-) diff --git a/Dockerfile b/Dockerfile index 01fe27fc..e3292bc5 100644 --- a/Dockerfile +++ b/Dockerfile @@ -2,7 +2,7 @@ ############################ # Pick the Base Image -# This Dockerfile supports both amd64 and jetson architectures. +# This Dockerfile supports both amd64 and Jetson (arm64) architectures. ############################ ARG TARGETARCH ARG UBUNTU_BASE=ubuntu:22.04 @@ -14,6 +14,7 @@ ARG L4T_BASE=nvcr.io/nvidia/l4t-jetpack:r36.4.0 FROM ${UBUNTU_BASE} AS base_amd64 FROM ${L4T_BASE} AS base_arm64 # Select the correct base +ARG TARGETARCH FROM base_${TARGETARCH} AS base ############################ @@ -27,14 +28,14 @@ ENV LANG=en_US.UTF-8 ENV LANGUAGE=en_US:en ENV LC_ALL=en_US.UTF-8 -# Per-arch APT cache -RUN --mount=type=cache,target=/var/cache/apt/archives-${TARGETARCH} \ +# 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} \ apt-get update && apt-get install -y --no-install-recommends \ locales apt-utils curl lsb-release gnupg2 \ software-properties-common build-essential python3-dev python3-pip \ && locale-gen en_US.UTF-8 \ && update-locale LANG=en_US.UTF-8 LC_ALL=en_US.UTF-8 \ - && apt-get clean && rm -rf /var/lib/apt/lists/* + && rm -rf /var/lib/apt/lists/* # ROS 2 repo RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ @@ -43,15 +44,15 @@ RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ 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/archives-${TARGETARCH} \ +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH} \ apt-get update && apt-get install -y --no-install-recommends \ ros-humble-ros-base python3-setuptools python3-wheel \ - && apt-get clean && rm -rf /var/lib/apt/lists/* + && rm -rf /var/lib/apt/lists/* # Python dependencies (per-arch pip cache) COPY requirements.txt . -RUN --mount=type=cache,target=/root/.cache/pip \ - pip3 install --no-cache-dir -r requirements.txt +RUN --mount=type=cache,target=/root/.cache/pip,id=pip-${TARGETARCH},sharing=locked \ + pip3 install -r requirements.txt ############################ # Stage 2: ROS deps scan @@ -60,13 +61,19 @@ FROM ros2_humble-base AS ros2_humble-rosdeps ARG DIR=/cprt_rover_24 WORKDIR ${DIR} -RUN --mount=type=cache,target=/var/cache/apt/archives-${TARGETARCH} \ +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH} \ apt-get update && apt-get install -y python3-rosdep \ - && apt-get clean && rm -rf /var/lib/apt/lists/* + && rm -rf /var/lib/apt/lists/* + +# Cache rosdep/rosdistro metadata across builds +RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ + --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ + rosdep init && rosdep update COPY src/**/package.xml src/**/setup.py src/**/CMakeLists.txt ./src ARG FILE=/opt/rosdeps.sh -RUN rosdep init && rosdep update && \ +RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ + --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ rosdep install -i -r -y --from-paths src -s > ${FILE} ############################ @@ -74,20 +81,19 @@ RUN rosdep init && rosdep update && \ ############################ FROM base AS ros2_humble-gstreamer -RUN --mount=type=cache,target=/var/cache/apt/archives-${TARGETARCH} \ +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH} \ 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 \ - git ninja-build curl && \ - apt-get clean && rm -rf /var/lib/apt/lists/* + git ninja-build curl \ + && rm -rf /var/lib/apt/lists/* RUN python3 -m pip install --upgrade --user pip meson WORKDIR /gstreamer -RUN --mount=type=cache,target=/root/.cache/gstreamer \ - --mount=type=cache,target=/root/.cache/meson \ - git clone https://gitlab.freedesktop.org/gstreamer/gstreamer.git && \ - cd gstreamer && \ +RUN --mount=type=cache,target=/root/.cache/gstreamer,id=gst-${TARGETARCH} \ + --mount=type=cache,target=/root/.cache/meson,id=meson-${TARGETARCH} \ + 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 && \ @@ -99,16 +105,17 @@ 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 -RUN --mount=type=cache,target=/root/.cargo \ +RUN --mount=type=cache,target=/root/.cargo,id=cargo-home-${TARGETARCH} \ + --mount=type=cache,target=/root/.cargo/registry,id=cargo-registry-${TARGETARCH},sharing=locked \ + --mount=type=cache,target=/root/.cargo/git,id=cargo-git-${TARGETARCH},sharing=locked \ 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 && \ - cd gst-plugins-rs && \ + git clone https://gitlab.freedesktop.org/gstreamer/gst-plugins-rs.git . && \ cargo install cargo-c && \ cargo cbuild -p gst-plugin-webrtc --prefix=/opt/gstreamer --release && \ - cargo cbuild -p gst-plugin-rtp --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 + cargo cinstall -p gst-plugin-rtp --prefix=/opt/gstreamer --destdir /target --libdir=lib --release ############################ # Stage 4: Minimal Runtime Base @@ -121,7 +128,8 @@ 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 -RUN --mount=type=cache,target=/var/cache/apt/archives-${TARGETARCH} \ +# Execute rosdep install script with apt cache +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH} \ chmod +x ${ROSDEP_FILE} && ${ROSDEP_FILE} CMD ["/bin/bash"] @@ -130,12 +138,16 @@ CMD ["/bin/bash"] # Stage 5: Dev Environment ############################ FROM runtime AS dev -RUN --mount=type=cache,target=/var/cache/apt/archives-${TARGETARCH} \ +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH} \ apt-get update && apt-get install -y --no-install-recommends \ git x11-apps ros-humble-desktop ros-dev-tools \ ros-humble-ament-cmake python3-colcon-common-extensions \ - python3-colcon-ros clang-format && \ - apt-get clean && rm -rf /var/lib/apt/lists/* + python3-colcon-ros clang-format ccache \ + && rm -rf /var/lib/apt/lists/* + +# Enable compiler caching +ENV CCACHE_DIR=/root/.cache/ccache +ENV PATH="/usr/lib/ccache:${PATH}" # Entrypoint RUN echo '#!/bin/bash\nsource /opt/ros/humble/setup.bash\nif [ -f /cprt_rover_24/install/setup.bash ]; then source /cprt_rover_24/install/setup.bash; fi\nexec "$@"' > /entrypoint.sh \ @@ -154,11 +166,14 @@ WORKDIR ${DIR} COPY src/ ${DIR}/src/ -RUN rosdep init && rosdep update && \ +# Cache rosdep metadata during install +RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ + --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ + rosdep init && rosdep update && \ rosdep install -i -r -y --from-paths src -# Cache colcon builds -RUN --mount=type=cache,target=${DIR}/colcon-cache \ +# Use ccache for compiles and persist its contents across builds +RUN --mount=type=cache,target=/root/.cache/ccache,id=ccache-${TARGETARCH},sharing=locked \ colcon build --symlink-install ############################ From 5ded5866ec075f337fa63c287230f6b984b8598e Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Fri, 15 Aug 2025 12:55:29 -0400 Subject: [PATCH 13/36] another one bites the dust --- .github/workflows/build-docker.yml | 10 +++++++--- Dockerfile | 14 ++------------ 2 files changed, 9 insertions(+), 15 deletions(-) diff --git a/.github/workflows/build-docker.yml b/.github/workflows/build-docker.yml index 826a075d..2e96cdde 100644 --- a/.github/workflows/build-docker.yml +++ b/.github/workflows/build-docker.yml @@ -44,12 +44,14 @@ jobs: file: ./Dockerfile 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 - # Expose the pushed image digest for the manifest job provenance: false build-arm64: @@ -83,6 +85,9 @@ jobs: file: ./Dockerfile 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 @@ -102,7 +107,6 @@ jobs: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - # Build the multi-arch manifest from pushed image digests (recommended) - name: Create and push multi-arch manifest (latest + commit) run: | AMD64_DIGEST="${{ needs.build-amd64.outputs.digest }}" @@ -112,4 +116,4 @@ jobs: -t "$IMAGE_NAME:latest" \ -t "$IMAGE_NAME:${{ github.sha }}" \ "$IMAGE_NAME@${AMD64_DIGEST}" \ - "$IMAGE_NAME@${ARM64_DIGEST}" \ No newline at end of file + "$IMAGE_NAME@${ARM64_DIGEST}" diff --git a/Dockerfile b/Dockerfile index e3292bc5..b5d88490 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,21 +1,11 @@ # syntax=docker/dockerfile:1.5 -############################ -# Pick the Base Image -# This Dockerfile supports both amd64 and Jetson (arm64) architectures. -############################ -ARG TARGETARCH -ARG UBUNTU_BASE=ubuntu:22.04 -ARG L4T_BASE=nvcr.io/nvidia/l4t-jetpack:r36.4.0 - ############################ # Stage 0: Base Image per Arch ############################ -FROM ${UBUNTU_BASE} AS base_amd64 -FROM ${L4T_BASE} AS base_arm64 -# Select the correct base +ARG BASE_IMAGE=ubuntu:22.04 ARG TARGETARCH -FROM base_${TARGETARCH} AS base +FROM ${BASE_IMAGE} AS base ############################ # Stage 1: Minimal ROS 2 Base From ca3ac822247dd31c51e4d80a88e601cf60bd76ef Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Fri, 15 Aug 2025 12:59:01 -0400 Subject: [PATCH 14/36] fix race cond --- Dockerfile | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/Dockerfile b/Dockerfile index b5d88490..b207c523 100644 --- a/Dockerfile +++ b/Dockerfile @@ -19,7 +19,7 @@ ENV LANGUAGE=en_US:en ENV LC_ALL=en_US.UTF-8 # 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} \ +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${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 python3-pip \ @@ -34,7 +34,7 @@ RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ 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} \ +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ apt-get update && apt-get install -y --no-install-recommends \ ros-humble-ros-base python3-setuptools python3-wheel \ && rm -rf /var/lib/apt/lists/* @@ -51,7 +51,7 @@ FROM ros2_humble-base AS ros2_humble-rosdeps ARG DIR=/cprt_rover_24 WORKDIR ${DIR} -RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH} \ +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ apt-get update && apt-get install -y python3-rosdep \ && rm -rf /var/lib/apt/lists/* @@ -71,7 +71,7 @@ RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ ############################ FROM base AS ros2_humble-gstreamer -RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH} \ +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${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 \ @@ -119,7 +119,7 @@ ENV LD_LIBRARY_PATH=/opt/gstreamer/lib:$LD_LIBRARY_PATH ENV PKG_CONFIG_PATH=/opt/gstreamer/lib/pkgconfig:$PKG_CONFIG_PATH # Execute rosdep install script with apt cache -RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH} \ +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ chmod +x ${ROSDEP_FILE} && ${ROSDEP_FILE} CMD ["/bin/bash"] @@ -128,7 +128,7 @@ CMD ["/bin/bash"] # Stage 5: Dev Environment ############################ FROM runtime AS dev -RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH} \ +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ apt-get update && apt-get install -y --no-install-recommends \ git x11-apps ros-humble-desktop ros-dev-tools \ ros-humble-ament-cmake python3-colcon-common-extensions \ From cfe178deab0a97fbec9ca65d805fae7d021f8775 Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Fri, 15 Aug 2025 13:37:02 -0400 Subject: [PATCH 15/36] try again --- Dockerfile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Dockerfile b/Dockerfile index b207c523..7bce4ac1 100644 --- a/Dockerfile +++ b/Dockerfile @@ -120,7 +120,7 @@ ENV PKG_CONFIG_PATH=/opt/gstreamer/lib/pkgconfig:$PKG_CONFIG_PATH # Execute rosdep install script with apt cache RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ - chmod +x ${ROSDEP_FILE} && ${ROSDEP_FILE} + chmod +x ${ROSDEP_FILE} && apt-get update && ${ROSDEP_FILE} CMD ["/bin/bash"] @@ -159,7 +159,7 @@ COPY src/ ${DIR}/src/ # Cache rosdep metadata during install RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ - rosdep init && rosdep update && \ + apt-get update && rosdep init && rosdep update && \ rosdep install -i -r -y --from-paths src # Use ccache for compiles and persist its contents across builds From 61563b6356da6585293ad4437bd42fcc5d916a96 Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Fri, 15 Aug 2025 16:07:32 -0400 Subject: [PATCH 16/36] hello. it's me --- Dockerfile | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/Dockerfile b/Dockerfile index 7bce4ac1..38c3c062 100644 --- a/Dockerfile +++ b/Dockerfile @@ -151,19 +151,23 @@ CMD ["/bin/bash"] # Stage 6: Builder ############################ FROM dev AS builder +SHELL ["/bin/bash", "-o", "pipefail", "-c"] # <-- ensure we have 'source' +ARG TARGETARCH ARG DIR=/cprt_rover_24 WORKDIR ${DIR} COPY src/ ${DIR}/src/ -# Cache rosdep metadata during install +# rosdep with ROS env loaded (uses cached rosdep/rosdistro) RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ - apt-get update && rosdep init && rosdep update && \ + source /opt/ros/humble/setup.bash && \ + rosdep init && rosdep update && \ rosdep install -i -r -y --from-paths src -# Use ccache for compiles and persist its contents across builds +# 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 ############################ From a74abfa7872c15c48860562a43a3f786c01ea7b5 Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Fri, 15 Aug 2025 16:55:09 -0400 Subject: [PATCH 17/36] staying alive --- .github/workflows/build-docker.yml | 81 ++++++++++++++++++++++++++---- Dockerfile | 52 ++++++++++++++----- 2 files changed, 109 insertions(+), 24 deletions(-) diff --git a/.github/workflows/build-docker.yml b/.github/workflows/build-docker.yml index 2e96cdde..253f6874 100644 --- a/.github/workflows/build-docker.yml +++ b/.github/workflows/build-docker.yml @@ -24,7 +24,8 @@ jobs: env: IMAGE_NAME: ${{ needs.setup.outputs.IMAGE_NAME }} outputs: - digest: ${{ steps.build.outputs.digest }} + app_digest: ${{ steps.build_app.outputs.digest }} + dev_digest: ${{ steps.build_dev.outputs.digest }} steps: - uses: actions/checkout@v4 @@ -36,12 +37,14 @@ jobs: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Build & push amd64 (with registry cache) - id: build + # --- 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: | @@ -54,13 +57,34 @@ jobs: 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: - digest: ${{ steps.build.outputs.digest }} + app_digest: ${{ steps.build_app.outputs.digest }} + dev_digest: ${{ steps.build_dev.outputs.digest }} steps: - uses: actions/checkout@v4 @@ -77,12 +101,14 @@ jobs: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Build & push arm64 (with registry cache) - id: build + # --- 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: | @@ -95,6 +121,26 @@ jobs: 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 + push-manifest: runs-on: ubuntu-latest needs: [build-amd64, build-arm64, setup] @@ -107,13 +153,26 @@ jobs: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Create and push multi-arch manifest (latest + commit) + # Create multi-arch manifest for APP image (latest + commit) + - name: Create and push APP manifest run: | - AMD64_DIGEST="${{ needs.build-amd64.outputs.digest }}" - ARM64_DIGEST="${{ needs.build-arm64.outputs.digest }}" + APP_AMD64_DIGEST="${{ needs.build-amd64.outputs.app_digest }}" + APP_ARM64_DIGEST="${{ needs.build-arm64.outputs.app_digest }}" docker buildx imagetools create \ -t "$IMAGE_NAME:latest" \ -t "$IMAGE_NAME:${{ github.sha }}" \ - "$IMAGE_NAME@${AMD64_DIGEST}" \ - "$IMAGE_NAME@${ARM64_DIGEST}" + "$IMAGE_NAME@${APP_AMD64_DIGEST}" \ + "$IMAGE_NAME@${APP_ARM64_DIGEST}" + + # Create multi-arch manifest for DEV image (dev + dev-${sha}) + - name: Create and push DEV manifest + run: | + DEV_AMD64_DIGEST="${{ needs.build-amd64.outputs.dev_digest }}" + DEV_ARM64_DIGEST="${{ needs.build-arm64.outputs.dev_digest }}" + + docker buildx imagetools create \ + -t "$IMAGE_NAME:dev" \ + -t "$IMAGE_NAME:dev-${{ github.sha }}" \ + "$IMAGE_NAME@${DEV_AMD64_DIGEST}" \ + "$IMAGE_NAME@${DEV_ARM64_DIGEST}" diff --git a/Dockerfile b/Dockerfile index 38c3c062..4b8ace2d 100644 --- a/Dockerfile +++ b/Dockerfile @@ -48,23 +48,41 @@ RUN --mount=type=cache,target=/root/.cache/pip,id=pip-${TARGETARCH},sharing=lock # Stage 2: ROS deps scan ############################ FROM ros2_humble-base AS ros2_humble-rosdeps +FROM ros2_humble-base AS ros2_humble-depsplan +SHELL ["/bin/bash", "-o", "pipefail", "-c"] +ARG TARGETARCH ARG DIR=/cprt_rover_24 WORKDIR ${DIR} -RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ - apt-get update && apt-get install -y python3-rosdep \ - && rm -rf /var/lib/apt/lists/* +# rosdep tooling only here +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH}-rosdeps,sharing=locked \ + apt-get update && apt-get install -y python3-rosdep && rm -rf /var/lib/apt/lists/* -# Cache rosdep/rosdistro metadata across builds +# prime rosdep caches once RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ - rosdep init && rosdep update + rosdep init || true && rosdep update +# bring manifests for resolution COPY src/**/package.xml src/**/setup.py src/**/CMakeLists.txt ./src -ARG FILE=/opt/rosdeps.sh + +# Compute, don't install: write apt/pip lists that runtime can consume +# Skip unresolved keys (add more if needed) +ENV ROSDEP_SKIP_KEYS="kindr_ros" RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ - rosdep install -i -r -y --from-paths src -s > ${FILE} + rosdep install -i -r -y --rosdistro humble --from-paths src -s --skip-keys="$ROSDEP_SKIP_KEYS" \ + | tee /opt/rosdep.plan && \ + awk '/apt(-get)? install/ { + for (i=1;i<=NF;i++) { + if ($i !~ /^(sudo|apt(-get)?|install|-y|--no-install-recommends)$/) print $i + } + }' /opt/rosdep.plan | sort -u > /opt/apt-packages.txt && \ + awk '/^pip3? install/ { + for (i=1;i<=NF;i++) { + if ($i !~ /^(sudo|pip3?|install|-r)$/) print $i + } + }' /opt/rosdep.plan | sort -u > /opt/pip-requirements.txt || true ############################ # Stage 3: GStreamer Build @@ -111,18 +129,25 @@ RUN --mount=type=cache,target=/root/.cargo,id=cargo-home-${TARGETARCH} \ # Stage 4: Minimal Runtime Base ############################ FROM ros2_humble-base AS runtime -ARG ROSDEP_FILE=/opt/rosdeps.sh -COPY --from=ros2_humble-rosdeps ${ROSDEP_FILE} ${ROSDEP_FILE} +ARG TARGETARCH +# Copy just the lists produced above +COPY --from=ros2_humble-depsplan /opt/apt-packages.txt /opt/apt-packages.txt +COPY --from=ros2_humble-depsplan /opt/pip-requirements.txt /opt/pip-requirements.txt 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 -# Execute rosdep install script with apt cache -RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ - chmod +x ${ROSDEP_FILE} && apt-get update && ${ROSDEP_FILE} +# Ensure repos are visible; then install the exact packages, no rosdep needed +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH}-runtime,sharing=locked \ + apt-get update && \ + xargs -r -a /opt/apt-packages.txt apt-get install -y && \ + rm -rf /var/lib/apt/lists/* -CMD ["/bin/bash"] +# Optional: if pip requirements were found +RUN --mount=type=cache,target=/root/.cache/pip,id=pip-${TARGETARCH},sharing=locked \ + if [ -s /opt/pip-requirements.txt ]; then pip3 install -r /opt/pip-requirements.txt; fi ############################ # Stage 5: Dev Environment @@ -162,6 +187,7 @@ COPY src/ ${DIR}/src/ RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ source /opt/ros/humble/setup.bash && \ + apt-get update && \ rosdep init && rosdep update && \ rosdep install -i -r -y --from-paths src From d1cdabda2871dfc76039ed81f51d9ec22c919002 Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Fri, 15 Aug 2025 16:57:48 -0400 Subject: [PATCH 18/36] I hate my life --- Dockerfile | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/Dockerfile b/Dockerfile index 4b8ace2d..f1c98ea6 100644 --- a/Dockerfile +++ b/Dockerfile @@ -73,16 +73,16 @@ RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ rosdep install -i -r -y --rosdistro humble --from-paths src -s --skip-keys="$ROSDEP_SKIP_KEYS" \ | tee /opt/rosdep.plan && \ - awk '/apt(-get)? install/ { - for (i=1;i<=NF;i++) { - if ($i !~ /^(sudo|apt(-get)?|install|-y|--no-install-recommends)$/) print $i - } - }' /opt/rosdep.plan | sort -u > /opt/apt-packages.txt && \ - awk '/^pip3? install/ { - for (i=1;i<=NF;i++) { - if ($i !~ /^(sudo|pip3?|install|-r)$/) print $i - } - }' /opt/rosdep.plan | sort -u > /opt/pip-requirements.txt || true + sh -c "awk '/apt(-get)? install/ { + for (i=1;i<=NF;i++) { + if (\$i !~ /^(sudo|apt(-get)?|install|-y|--no-install-recommends)$/) print \$i + } + }' /opt/rosdep.plan | sort -u > /opt/apt-packages.txt" && \ + sh -c "awk '/^pip3? install/ { + for (i=1;i<=NF;i++) { + if (\$i !~ /^(sudo|pip3?|install|-r)$/) print \$i + } + }' /opt/rosdep.plan | sort -u > /opt/pip-requirements.txt" || true ############################ # Stage 3: GStreamer Build From 4abee18c021fab79d7b939850205a641bf6b4ae9 Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Fri, 15 Aug 2025 16:59:15 -0400 Subject: [PATCH 19/36] I am going to jump --- Dockerfile | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/Dockerfile b/Dockerfile index f1c98ea6..53f05d6f 100644 --- a/Dockerfile +++ b/Dockerfile @@ -73,16 +73,11 @@ RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ rosdep install -i -r -y --rosdistro humble --from-paths src -s --skip-keys="$ROSDEP_SKIP_KEYS" \ | tee /opt/rosdep.plan && \ - sh -c "awk '/apt(-get)? install/ { - for (i=1;i<=NF;i++) { - if (\$i !~ /^(sudo|apt(-get)?|install|-y|--no-install-recommends)$/) print \$i - } - }' /opt/rosdep.plan | sort -u > /opt/apt-packages.txt" && \ - sh -c "awk '/^pip3? install/ { - for (i=1;i<=NF;i++) { - if (\$i !~ /^(sudo|pip3?|install|-r)$/) print \$i - } - }' /opt/rosdep.plan | sort -u > /opt/pip-requirements.txt" || true + awk '/apt(-get)? install/ {for (i=1;i<=NF;i++) {if ($i !~ /^(sudo|apt(-get)?|install|-y|--no-install-recommends)$/) print $i}}' /opt/rosdep.plan \ + | sort -u > /opt/apt-packages.txt && \ + awk '/^pip3? install/ {for (i=1;i<=NF;i++) {if ($i !~ /^(sudo|pip3?|install|-r)$/) print $i}}' /opt/rosdep.plan \ + | sort -u > /opt/pip-requirements.txt || true + ############################ # Stage 3: GStreamer Build From 36864bbfa3aa11e0578a0d26c8df726708ea40aa Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Fri, 15 Aug 2025 17:41:51 -0400 Subject: [PATCH 20/36] add kindr --- Dockerfile | 29 +++++++++++++++++++++++++---- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/Dockerfile b/Dockerfile index 53f05d6f..7543b4c8 100644 --- a/Dockerfile +++ b/Dockerfile @@ -121,7 +121,27 @@ RUN --mount=type=cache,target=/root/.cargo,id=cargo-home-${TARGETARCH} \ cargo cinstall -p gst-plugin-rtp --prefix=/opt/gstreamer --destdir /target --libdir=lib --release ############################ -# Stage 4: Minimal Runtime Base +# Stage 4: kindr +############################ +FROM base AS kindr_build +ARG TARGETARCH +WORKDIR /work + +# deps: cmake + toolchain + Eigen +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH}-kindr,sharing=locked \ + apt-get update && apt-get install -y --no-install-recommends \ + git cmake build-essential libeigen3-dev \ + && rm -rf /var/lib/apt/lists/* + +# build & "install" into a staging dir (/target) +RUN git clone --depth=1 https://github.com/ANYbotics/kindr.git src && \ + cmake -S src -B build -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DBUILD_EXAMPLES=OFF && \ + cmake --build build -j"$(nproc)" && \ + DESTDIR=/kindr-out cmake --install build --prefix /usr/local && \ + mkdir -p /target && cp -a /kindr-out/usr/local /target/usr/local + +############################ +# Stage 5: Minimal Runtime Base ############################ FROM ros2_humble-base AS runtime ARG TARGETARCH @@ -129,6 +149,7 @@ ARG TARGETARCH COPY --from=ros2_humble-depsplan /opt/apt-packages.txt /opt/apt-packages.txt COPY --from=ros2_humble-depsplan /opt/pip-requirements.txt /opt/pip-requirements.txt COPY --from=ros2_humble-gstreamer /target / +COPY --from=kindr_build /target/usr/local/ /usr/local/ ENV PATH=/opt/gstreamer/bin:$PATH ENV LD_LIBRARY_PATH=/opt/gstreamer/lib:$LD_LIBRARY_PATH @@ -145,7 +166,7 @@ RUN --mount=type=cache,target=/root/.cache/pip,id=pip-${TARGETARCH},sharing=lock if [ -s /opt/pip-requirements.txt ]; then pip3 install -r /opt/pip-requirements.txt; fi ############################ -# Stage 5: Dev Environment +# Stage 6: Dev Environment ############################ FROM runtime AS dev RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ @@ -168,7 +189,7 @@ ENTRYPOINT ["/entrypoint.sh"] CMD ["/bin/bash"] ############################ -# Stage 6: Builder +# Stage 7: Builder ############################ FROM dev AS builder SHELL ["/bin/bash", "-o", "pipefail", "-c"] # <-- ensure we have 'source' @@ -192,7 +213,7 @@ RUN --mount=type=cache,target=/root/.cache/ccache,id=ccache-${TARGETARCH},sharin colcon build --symlink-install ############################ -# Stage 7: Runtime Application +# Stage 8: Runtime Application ############################ FROM runtime AS rover ARG DIR=/cprt_rover_24 From 68ba88dcc35a076b5ad31b45a832b0e3ba6c654d Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Fri, 15 Aug 2025 17:45:00 -0400 Subject: [PATCH 21/36] kindr attempt 2 --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index 7543b4c8..8e61f655 100644 --- a/Dockerfile +++ b/Dockerfile @@ -123,7 +123,7 @@ RUN --mount=type=cache,target=/root/.cargo,id=cargo-home-${TARGETARCH} \ ############################ # Stage 4: kindr ############################ -FROM base AS kindr_build +FROM ros2_humble-base AS kindr_build ARG TARGETARCH WORKDIR /work From 1dde17bbf11e512a5808283a98c81195ffda823f Mon Sep 17 00:00:00 2001 From: Connor Needham <129120300+ConnorNeed@users.noreply.github.com> Date: Fri, 15 Aug 2025 17:50:36 -0400 Subject: [PATCH 22/36] hit me baby one more time --- .github/workflows/build-docker.yml | 2 ++ Dockerfile | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/build-docker.yml b/.github/workflows/build-docker.yml index 253f6874..7a94a5d8 100644 --- a/.github/workflows/build-docker.yml +++ b/.github/workflows/build-docker.yml @@ -17,6 +17,8 @@ jobs: steps: - name: Checkout repository uses: actions/checkout@v4 + with: + submodules: recursive build-amd64: runs-on: ubuntu-latest diff --git a/Dockerfile b/Dockerfile index 8e61f655..f485036b 100644 --- a/Dockerfile +++ b/Dockerfile @@ -134,7 +134,8 @@ RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH}-kindr,sharing= && rm -rf /var/lib/apt/lists/* # build & "install" into a staging dir (/target) -RUN git clone --depth=1 https://github.com/ANYbotics/kindr.git src && \ +RUN source /opt/ros/humble/setup.bash && \ + git clone --depth=1 https://github.com/ANYbotics/kindr.git src && \ cmake -S src -B build -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DBUILD_EXAMPLES=OFF && \ cmake --build build -j"$(nproc)" && \ DESTDIR=/kindr-out cmake --install build --prefix /usr/local && \ From 290563afc20dddbf1474d25502295503e4f8d4f8 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sat, 23 Aug 2025 10:49:11 -0400 Subject: [PATCH 23/36] WIP --- .gitmodules | 4 -- Dockerfile | 117 ++++++++++++++++++------------------------ local_docker_build.sh | 52 +++++++++++++++++++ make.sh | 46 ----------------- 4 files changed, 101 insertions(+), 118 deletions(-) create mode 100755 local_docker_build.sh delete mode 100755 make.sh diff --git a/.gitmodules b/.gitmodules index f4f2007f..3fd721f8 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,7 +1,3 @@ -[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 diff --git a/Dockerfile b/Dockerfile index f485036b..41be9a7a 100644 --- a/Dockerfile +++ b/Dockerfile @@ -36,7 +36,7 @@ RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ apt-get update && apt-get install -y --no-install-recommends \ - ros-humble-ros-base python3-setuptools python3-wheel \ + ros-humble-ros-base python3-setuptools python3-wheel python3-rosdep \ && rm -rf /var/lib/apt/lists/* # Python dependencies (per-arch pip cache) @@ -44,58 +44,49 @@ COPY requirements.txt . RUN --mount=type=cache,target=/root/.cache/pip,id=pip-${TARGETARCH},sharing=locked \ pip3 install -r requirements.txt + ############################ -# Stage 2: ROS deps scan +# Stage 2: Rust Setup ############################ -FROM ros2_humble-base AS ros2_humble-rosdeps -FROM ros2_humble-base AS ros2_humble-depsplan -SHELL ["/bin/bash", "-o", "pipefail", "-c"] -ARG TARGETARCH -ARG DIR=/cprt_rover_24 -WORKDIR ${DIR} +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"] -# rosdep tooling only here -RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH}-rosdeps,sharing=locked \ - apt-get update && apt-get install -y python3-rosdep && rm -rf /var/lib/apt/lists/* -# prime rosdep caches once -RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ - --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ - rosdep init || true && rosdep update - -# bring manifests for resolution -COPY src/**/package.xml src/**/setup.py src/**/CMakeLists.txt ./src - -# Compute, don't install: write apt/pip lists that runtime can consume -# Skip unresolved keys (add more if needed) -ENV ROSDEP_SKIP_KEYS="kindr_ros" -RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ - --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ - rosdep install -i -r -y --rosdistro humble --from-paths src -s --skip-keys="$ROSDEP_SKIP_KEYS" \ - | tee /opt/rosdep.plan && \ - awk '/apt(-get)? install/ {for (i=1;i<=NF;i++) {if ($i !~ /^(sudo|apt(-get)?|install|-y|--no-install-recommends)$/) print $i}}' /opt/rosdep.plan \ - | sort -u > /opt/apt-packages.txt && \ - awk '/^pip3? install/ {for (i=1;i<=NF;i++) {if ($i !~ /^(sudo|pip3?|install|-r)$/) print $i}}' /opt/rosdep.plan \ - | sort -u > /opt/pip-requirements.txt || true +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 base AS ros2_humble-gstreamer +FROM rust-builder AS ros2_humble-gstreamer +ARG TARGETARCH + RUN --mount=type=cache,target=/var/cache/apt,id=apt-${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 \ - git ninja-build curl \ + build-essential git ninja-build curl ccache \ && rm -rf /var/lib/apt/lists/* +# 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 && \ @@ -107,14 +98,9 @@ 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 --mount=type=cache,target=/root/.cargo,id=cargo-home-${TARGETARCH} \ - --mount=type=cache,target=/root/.cargo/registry,id=cargo-registry-${TARGETARCH},sharing=locked \ - --mount=type=cache,target=/root/.cargo/git,id=cargo-git-${TARGETARCH},sharing=locked \ - 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 . && \ - cargo install cargo-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 && \ @@ -127,6 +113,9 @@ FROM ros2_humble-base AS kindr_build ARG TARGETARCH WORKDIR /work +# use bash so we can source setup.bash +SHELL ["/bin/bash", "-o", "pipefail", "-c"] + # deps: cmake + toolchain + Eigen RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH}-kindr,sharing=locked \ apt-get update && apt-get install -y --no-install-recommends \ @@ -136,38 +125,35 @@ RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH}-kindr,sharing= # build & "install" into a staging dir (/target) RUN source /opt/ros/humble/setup.bash && \ git clone --depth=1 https://github.com/ANYbotics/kindr.git src && \ - cmake -S src -B build -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DBUILD_EXAMPLES=OFF && \ + cmake -S src -B build \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_TESTS=OFF \ + -DBUILD_EXAMPLES=OFF && \ cmake --build build -j"$(nproc)" && \ - DESTDIR=/kindr-out cmake --install build --prefix /usr/local && \ - mkdir -p /target && cp -a /kindr-out/usr/local /target/usr/local + cmake --install build --prefix /target/usr +RUN mv /target/usr/include/kindr/kindr/* /target/usr/include/kindr/ && rmdir /target/usr/include/kindr/kindr ############################ -# Stage 5: Minimal Runtime Base +# Stage 6: Minimal Runtime Base ############################ FROM ros2_humble-base AS runtime -ARG TARGETARCH -# Copy just the lists produced above -COPY --from=ros2_humble-depsplan /opt/apt-packages.txt /opt/apt-packages.txt -COPY --from=ros2_humble-depsplan /opt/pip-requirements.txt /opt/pip-requirements.txt COPY --from=ros2_humble-gstreamer /target / -COPY --from=kindr_build /target/usr/local/ /usr/local/ +COPY --from=kindr_build /target/usr/ /usr/ 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 -# Ensure repos are visible; then install the exact packages, no rosdep needed -RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH}-runtime,sharing=locked \ +# rosdep with ROS env loaded (uses cached rosdep/rosdistro) +RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ + --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ + source /opt/ros/humble/setup.bash && \ apt-get update && \ - xargs -r -a /opt/apt-packages.txt apt-get install -y && \ - rm -rf /var/lib/apt/lists/* - -# Optional: if pip requirements were found -RUN --mount=type=cache,target=/root/.cache/pip,id=pip-${TARGETARCH},sharing=locked \ - if [ -s /opt/pip-requirements.txt ]; then pip3 install -r /opt/pip-requirements.txt; fi + rosdep init && rosdep update && \ + rosdep install -i -r -y --from-paths src -c /var/cache/rosdep ############################ -# Stage 6: Dev Environment +# Stage 7: Dev Environment ############################ FROM runtime AS dev RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ @@ -190,31 +176,26 @@ ENTRYPOINT ["/entrypoint.sh"] CMD ["/bin/bash"] ############################ -# Stage 7: Builder +# Stage 8: Builder ############################ FROM dev AS builder -SHELL ["/bin/bash", "-o", "pipefail", "-c"] # <-- ensure we have 'source' +SHELL ["/bin/bash", "-o", "pipefail", "-c"] ARG TARGETARCH ARG DIR=/cprt_rover_24 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 COPY src/ ${DIR}/src/ -# rosdep with ROS env loaded (uses cached rosdep/rosdistro) -RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ - --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ - source /opt/ros/humble/setup.bash && \ - apt-get update && \ - rosdep init && rosdep update && \ - rosdep install -i -r -y --from-paths 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 ############################ -# Stage 8: Runtime Application +# Stage 9: Runtime Application ############################ FROM runtime AS rover ARG DIR=/cprt_rover_24 diff --git a/local_docker_build.sh b/local_docker_build.sh new file mode 100755 index 00000000..50b095ba --- /dev/null +++ b/local_docker_build.sh @@ -0,0 +1,52 @@ +#!/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:5.0-runtime-cuda12.8-ubuntu22.04" + ;; + aarch64) + TARGETARCH=arm64 + BASE_IMAGE="stereolabs/zed:5.0-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} \ +# -t ${IMAGE_NAME}:${GIT_SHA}-${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/make.sh b/make.sh deleted file mode 100755 index f0c92bd7..00000000 --- a/make.sh +++ /dev/null @@ -1,46 +0,0 @@ -#!/bin/bash - -REPO=cprtsoftware/cprt_rover_24 -TYPE=jetson # Replace with 'base' as needed -TAG=latest # Replace with the desired commit hash or 'latest' - -BASE_IMAGE_NAME="$REPO-$TYPE:$TAG" -IMAGE_NAME="$REPO-$TYPE-deploy:$TAG" - -cd "$(dirname "$0")" - -source ./docker/common.sh - -files=$(git ls-files -- ':!:(*/)' ':!:(*/**/*)' ':!:(*/**/*/*)') - -# Filter out python files into one variable -python_files=$( - for file in $files; do - if [[ $file == *.py ]]; then - echo "$file" - fi - done -) - -# Launch one instance of `black` for all python files -IFS=$'\n' black $python_files - -# Format C/C++ source & header files -for file in $files; do - if [[ $file == *.cpp || $file == *.hpp || $file == *.c || $file == *.h ]]; then - clang-format -i "$file" - fi -done - -check_docker - -docker run --rm --privileged multiarch/qemu-user-static --reset -p yes - -docker buildx build \ - -t $IMAGE_NAME \ - -f Dockerfile \ - --platform linux/arm64/v8 \ - --build-arg IMAGE=stereolabs/zed:5.0-runtime-l4t-r36.4 \ - --push \ - . - From f14509b04d821f0081db15f1491c08df4c9a388d Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sat, 23 Aug 2025 14:06:39 -0400 Subject: [PATCH 24/36] I hurt --- Dockerfile | 48 ++++++++++++++++++++++++++++++------------- local_docker_build.sh | 24 +++++++++++----------- 2 files changed, 46 insertions(+), 26 deletions(-) diff --git a/Dockerfile b/Dockerfile index 41be9a7a..58cfb7e2 100644 --- a/Dockerfile +++ b/Dockerfile @@ -17,16 +17,21 @@ 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 \ apt-get update && apt-get install -y --no-install-recommends \ locales apt-utils curl lsb-release gnupg2 \ - software-properties-common build-essential python3-dev python3-pip \ + software-properties-common build-essential \ + python3-dev tzdata python3-pip \ && locale-gen en_US.UTF-8 \ && update-locale LANG=en_US.UTF-8 LC_ALL=en_US.UTF-8 \ && rm -rf /var/lib/apt/lists/* +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 && \ @@ -134,7 +139,18 @@ RUN source /opt/ros/humble/setup.bash && \ RUN mv /target/usr/include/kindr/kindr/* /target/usr/include/kindr/ && rmdir /target/usr/include/kindr/kindr ############################ -# Stage 6: Minimal Runtime Base +# 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 / @@ -143,49 +159,53 @@ COPY --from=kindr_build /target/usr/ /usr/ 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 /cprt_rover_24/src + +COPY --from=package_xml_collector /collected /tmp/src/ +WORKDIR /tmp # rosdep with ROS env loaded (uses cached rosdep/rosdistro) -RUN --mount=type=cache,target=/var/cache/rosdistro,id=rosdistro \ - --mount=type=cache,target=/var/cache/rosdep,id=rosdep \ +RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ source /opt/ros/humble/setup.bash && \ apt-get update && \ rosdep init && rosdep update && \ - rosdep install -i -r -y --from-paths src -c /var/cache/rosdep + rosdep install -i -r -y --from-paths src ############################ -# Stage 7: Dev Environment +# Stage 8: Dev Environment ############################ FROM runtime AS dev RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ apt-get update && apt-get install -y --no-install-recommends \ git x11-apps ros-humble-desktop ros-dev-tools \ ros-humble-ament-cmake python3-colcon-common-extensions \ - python3-colcon-ros clang-format ccache \ + python3-colcon-ros clang-format ccache nvidia-cuda-toolkit \ && rm -rf /var/lib/apt/lists/* # Enable compiler caching ENV CCACHE_DIR=/root/.cache/ccache ENV PATH="/usr/lib/ccache:${PATH}" -# Entrypoint -RUN echo '#!/bin/bash\nsource /opt/ros/humble/setup.bash\nif [ -f /cprt_rover_24/install/setup.bash ]; then source /cprt_rover_24/install/setup.bash; fi\nexec "$@"' > /entrypoint.sh \ - && chmod +x /entrypoint.sh - RUN useradd -ms /bin/bash -u 1000 vscode && echo "vscode ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers -ENTRYPOINT ["/entrypoint.sh"] +WORKDIR / CMD ["/bin/bash"] ############################ -# Stage 8: Builder +# Stage 9: Builder ############################ FROM dev 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 COPY src/ ${DIR}/src/ @@ -195,7 +215,7 @@ RUN --mount=type=cache,target=/root/.cache/ccache,id=ccache-${TARGETARCH},sharin colcon build --symlink-install ############################ -# Stage 9: Runtime Application +# Stage 10: Runtime Application ############################ FROM runtime AS rover ARG DIR=/cprt_rover_24 diff --git a/local_docker_build.sh b/local_docker_build.sh index 50b095ba..1d8f037c 100755 --- a/local_docker_build.sh +++ b/local_docker_build.sh @@ -38,15 +38,15 @@ docker buildx build \ --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} \ -# -t ${IMAGE_NAME}:${GIT_SHA}-${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} \ + -t ${IMAGE_NAME}:${GIT_SHA}-${TARGETARCH} \ + --cache-from=type=registry,ref=${IMAGE_NAME}:${TARGETARCH}-cache \ + --cache-to=type=registry,ref=${IMAGE_NAME}:${TARGETARCH}-cache,mode=max \ + --load \ + . From 7ccb0fd6891910658434fe0542c5971d50e07c34 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Sat, 23 Aug 2025 14:16:43 -0400 Subject: [PATCH 25/36] damn daniel --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index 58cfb7e2..5a5ed037 100644 --- a/Dockerfile +++ b/Dockerfile @@ -180,7 +180,7 @@ RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked apt-get update && apt-get install -y --no-install-recommends \ git x11-apps ros-humble-desktop ros-dev-tools \ ros-humble-ament-cmake python3-colcon-common-extensions \ - python3-colcon-ros clang-format ccache nvidia-cuda-toolkit \ + python3-colcon-ros clang-format ccache cuda-toolkit-12 \ && rm -rf /var/lib/apt/lists/* # Enable compiler caching From 0c601d53e277f2e46a1245f86111f409d7046248 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Mon, 25 Aug 2025 15:20:29 +0000 Subject: [PATCH 26/36] it builds --- Dockerfile | 50 ++++++++++++++------------------------------------ 1 file changed, 14 insertions(+), 36 deletions(-) diff --git a/Dockerfile b/Dockerfile index 5a5ed037..4db95e75 100644 --- a/Dockerfile +++ b/Dockerfile @@ -24,7 +24,7 @@ RUN --mount=type=cache,target=/var/cache/apt,id=apt-${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 \ + 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 \ && rm -rf /var/lib/apt/lists/* @@ -41,7 +41,8 @@ RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ apt-get update && apt-get install -y --no-install-recommends \ - ros-humble-ros-base python3-setuptools python3-wheel python3-rosdep \ + ros-humble-ros-base python3-setuptools python3-wheel \ + libeigen3-dev python3-rosdep \ && rm -rf /var/lib/apt/lists/* # Python dependencies (per-arch pip cache) @@ -86,7 +87,7 @@ ENV CC="ccache gcc" ENV CXX="ccache g++" -RUN python3 -m pip install --upgrade --user pip meson +RUN python3 -m pip install --upgrade pip meson WORKDIR /gstreamer RUN --mount=type=cache,target=/root/.cache/gstreamer,id=gst-${TARGETARCH} \ @@ -111,33 +112,6 @@ RUN git clone https://gitlab.freedesktop.org/gstreamer/gst-plugins-rs.git . && \ 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 4: kindr -############################ -FROM ros2_humble-base AS kindr_build -ARG TARGETARCH -WORKDIR /work - -# use bash so we can source setup.bash -SHELL ["/bin/bash", "-o", "pipefail", "-c"] - -# deps: cmake + toolchain + Eigen -RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH}-kindr,sharing=locked \ - apt-get update && apt-get install -y --no-install-recommends \ - git cmake build-essential libeigen3-dev \ - && rm -rf /var/lib/apt/lists/* - -# build & "install" into a staging dir (/target) -RUN source /opt/ros/humble/setup.bash && \ - git clone --depth=1 https://github.com/ANYbotics/kindr.git src && \ - cmake -S src -B build \ - -DCMAKE_BUILD_TYPE=Release \ - -DBUILD_TESTS=OFF \ - -DBUILD_EXAMPLES=OFF && \ - cmake --build build -j"$(nproc)" && \ - cmake --install build --prefix /target/usr -RUN mv /target/usr/include/kindr/kindr/* /target/usr/include/kindr/ && rmdir /target/usr/include/kindr/kindr - ############################ # Stage 6: Collect package.xml files ############################ @@ -154,16 +128,14 @@ RUN find . -name 'package.xml' -exec mkdir -p /collected/{} \; \ ############################ FROM ros2_humble-base AS runtime COPY --from=ros2_humble-gstreamer /target / -COPY --from=kindr_build /target/usr/ /usr/ 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 /cprt_rover_24/src - -COPY --from=package_xml_collector /collected /tmp/src/ -WORKDIR /tmp +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 \ @@ -172,6 +144,12 @@ RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked 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 ############################ @@ -212,7 +190,7 @@ 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 + colcon build --symlink-install --continue-on-error ############################ # Stage 10: Runtime Application From a9f2c8571b81532fd0c0da73ecf01fdf454166bb Mon Sep 17 00:00:00 2001 From: ConnorN Date: Mon, 25 Aug 2025 19:05:20 +0000 Subject: [PATCH 27/36] x86 seems good --- .devcontainer/amd64/devcontainer.json | 39 ++++++++++++++++++++++ .devcontainer/devcontainer.json | 25 -------------- .devcontainer/jetson/devcontainer.json | 43 +++++++++++++++++++++++++ .devcontainer/windows/devcontainer.json | 43 +++++++++++++++++++++++++ .gitmodules | 1 + Dockerfile | 14 ++++++-- local_docker_build.sh | 4 +-- 7 files changed, 139 insertions(+), 30 deletions(-) create mode 100644 .devcontainer/amd64/devcontainer.json delete mode 100644 .devcontainer/devcontainer.json create mode 100644 .devcontainer/jetson/devcontainer.json create mode 100644 .devcontainer/windows/devcontainer.json diff --git a/.devcontainer/amd64/devcontainer.json b/.devcontainer/amd64/devcontainer.json new file mode 100644 index 00000000..07a1a6e2 --- /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/devcontainer.json b/.devcontainer/devcontainer.json deleted file mode 100644 index 78c5d112..00000000 --- a/.devcontainer/devcontainer.json +++ /dev/null @@ -1,25 +0,0 @@ -{ - "name": "CPRT Dev Container", - - "image": "cprtsoftware/cprt_rover_24-dev:latest", - - "runArgs": [ - "--network=host" - ], - "mounts": [ - "source=${env:SSH_AUTH_SOCK},target=/ssh-agent,type=bind" - ], - "containerEnv": { - "SSH_AUTH_SOCK": "/ssh-agent" - }, - "remoteUser": "vscode", - "containerUser": "vscode", - "remoteEnv": { - "USER": "vscode" - }, - - // Usual extras (optional) - "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/jetson/devcontainer.json b/.devcontainer/jetson/devcontainer.json new file mode 100644 index 00000000..5a6a6b7d --- /dev/null +++ b/.devcontainer/jetson/devcontainer.json @@ -0,0 +1,43 @@ +{ + "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", + "source=${env:HOME}/.ssh/config,target=/home/vscode/.ssh/config,type=bind,consistency=cached", + "source=${env:HOME}/.ssh/known_hosts,target=/home/vscode/.ssh/known_hosts,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/.gitmodules b/.gitmodules index 3fd721f8..d867ef09 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,7 @@ [submodule "src/zed-ros2-wrapper"] path = src/zed-ros2-wrapper url = https://github.com/stereolabs/zed-ros2-wrapper.git + branch = Humble-v4.2.5 [submodule "ublox"] path = src/ublox url = https://github.com/KumarRobotics/ublox.git diff --git a/Dockerfile b/Dockerfile index 4db95e75..74f79f15 100644 --- a/Dockerfile +++ b/Dockerfile @@ -87,7 +87,7 @@ ENV CC="ccache gcc" ENV CXX="ccache g++" -RUN python3 -m pip install --upgrade pip meson +RUN python3 -m pip install --upgrade --user pip meson WORKDIR /gstreamer RUN --mount=type=cache,target=/root/.cache/gstreamer,id=gst-${TARGETARCH} \ @@ -162,17 +162,17 @@ RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked && rm -rf /var/lib/apt/lists/* # Enable compiler caching -ENV CCACHE_DIR=/root/.cache/ccache ENV PATH="/usr/lib/ccache:${PATH}" 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 dev AS builder +FROM runtime AS builder SHELL ["/bin/bash", "-o", "pipefail", "-c"] ARG TARGETARCH ARG DIR=/cprt_rover_24 @@ -184,6 +184,14 @@ 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 \ + 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 \ + && rm -rf /var/lib/apt/lists/* + COPY src/ ${DIR}/src/ diff --git a/local_docker_build.sh b/local_docker_build.sh index 1d8f037c..8f2ed08d 100755 --- a/local_docker_build.sh +++ b/local_docker_build.sh @@ -9,11 +9,11 @@ ARCH=$(uname -m) case "$ARCH" in x86_64) TARGETARCH=amd64 - BASE_IMAGE="stereolabs/zed:5.0-runtime-cuda12.8-ubuntu22.04" + BASE_IMAGE="stereolabs/zed:4.2-runtime-cuda12.1-ubuntu22.04" ;; aarch64) TARGETARCH=arm64 - BASE_IMAGE="stereolabs/zed:5.0-runtime-l4t-r36.4" + BASE_IMAGE="stereolabs/zed:4.2-runtime-l4t-r36.4" ;; *) echo "Unsupported architecture: $ARCH" From 9d011954044d4d28f01fe6156baaef401fbbb2d1 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Mon, 25 Aug 2025 19:06:18 +0000 Subject: [PATCH 28/36] dev container fixes --- .devcontainer/amd64/devcontainer.json | 2 +- .devcontainer/jetson/devcontainer.json | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/.devcontainer/amd64/devcontainer.json b/.devcontainer/amd64/devcontainer.json index 07a1a6e2..7086eba5 100644 --- a/.devcontainer/amd64/devcontainer.json +++ b/.devcontainer/amd64/devcontainer.json @@ -13,7 +13,7 @@ ], "mounts": [ "source=${env:SSH_AUTH_SOCK},target=/ssh-agent,type=bind", - "source=${env:HOME}/.gitconfig,target=/home/vscode/.gitconfig,type=bind,consistency=cached", + "source=${env:HOME}/.gitconfig,target=/home/vscode/.gitconfig,type=bind,consistency=cached" ], "containerEnv": { "SSH_AUTH_SOCK": "/ssh-agent", diff --git a/.devcontainer/jetson/devcontainer.json b/.devcontainer/jetson/devcontainer.json index 5a6a6b7d..b943ac71 100644 --- a/.devcontainer/jetson/devcontainer.json +++ b/.devcontainer/jetson/devcontainer.json @@ -15,9 +15,7 @@ ], "mounts": [ "source=${env:SSH_AUTH_SOCK},target=/ssh-agent,type=bind", - "source=${env:HOME}/.gitconfig,target=/home/vscode/.gitconfig,type=bind,consistency=cached", - "source=${env:HOME}/.ssh/config,target=/home/vscode/.ssh/config,type=bind,consistency=cached", - "source=${env:HOME}/.ssh/known_hosts,target=/home/vscode/.ssh/known_hosts,type=bind,consistency=cached" + "source=${env:HOME}/.gitconfig,target=/home/vscode/.gitconfig,type=bind,consistency=cached" ], "containerEnv": { "SSH_AUTH_SOCK": "/ssh-agent", From 54a0611074a515407c6c579c1f006bb79d5269e7 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Mon, 25 Aug 2025 19:26:31 +0000 Subject: [PATCH 29/36] missing deps --- Dockerfile | 5 ++--- requirements.txt | 3 ++- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Dockerfile b/Dockerfile index 74f79f15..ed620a7a 100644 --- a/Dockerfile +++ b/Dockerfile @@ -156,13 +156,12 @@ RUN git clone https://github.com/ANYbotics/kindr.git && \ FROM runtime AS dev RUN --mount=type=cache,target=/var/cache/apt,id=apt-${TARGETARCH},sharing=locked \ apt-get update && apt-get install -y --no-install-recommends \ - git x11-apps ros-humble-desktop ros-dev-tools \ + git x11-apps ros-humble-desktop ros-dev-tools black pylint \ ros-humble-ament-cmake python3-colcon-common-extensions \ - python3-colcon-ros clang-format ccache cuda-toolkit-12 \ + python3-colcon-ros clang-format cuda-toolkit-12 \ && rm -rf /var/lib/apt/lists/* # Enable compiler caching -ENV PATH="/usr/lib/ccache:${PATH}" 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 diff --git a/requirements.txt b/requirements.txt index fc29773b..67fe3065 100644 --- a/requirements.txt +++ b/requirements.txt @@ -7,8 +7,9 @@ adafruit-circuitpython-bme280 adafruit-circuitpython-ens160 adafruit-circuitpython-busdevice adafruit-circuitpython-register +adafruit-circuitpython-neopixel pyubx2 -black RPi.GPIO rpi-hardware-pwm Jetson.GPIO +python-can From 58f0e03dd8a9317a0194d66b5c09dd45730cba77 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Tue, 26 Aug 2025 09:32:53 +0000 Subject: [PATCH 30/36] rm --- src/kindr_ros | 1 - 1 file changed, 1 deletion(-) delete mode 160000 src/kindr_ros 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 From 456b2c26c6e37259c441b47308641cb0e807ce9f Mon Sep 17 00:00:00 2001 From: ConnorN Date: Tue, 26 Aug 2025 10:06:02 +0000 Subject: [PATCH 31/36] add git modules --- .gitmodules | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.gitmodules b/.gitmodules index d867ef09..296ef968 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,12 +1,12 @@ -[submodule "src/zed-ros2-wrapper"] - path = src/zed-ros2-wrapper +[submodule "src/third-party/zed-ros2-wrapper"] + path = src/third-party/zed-ros2-wrapper url = https://github.com/stereolabs/zed-ros2-wrapper.git branch = Humble-v4.2.5 -[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/thrid-party/ouster-ros"] + path = src/third-party/ouster-ros url = https://github.com/ouster-lidar/ouster-ros.git branch = ros2 From 66b3629bd02441d62b92a15f7575424f3a071d27 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Tue, 26 Aug 2025 10:07:08 +0000 Subject: [PATCH 32/36] refactor --- .github/workflows/build-docker.yml | 38 +- build.sh | 4 +- setup/cprt_bash_aliases.sh | 420 ------ setup/cprt_bash_rc.sh | 29 - setup/enablecan.sh | 4 - setup/fastdds_config.xml | 17 - setup/firstTimeInstall.sh | 18 +- setup/install_gstreamer.sh | 75 - setup/install_nav_deps.sh | 22 - setup/install_ros_humble.sh | 64 - setup/science/setup_science.sh | 79 -- setup/science/start_science.service | 11 - setup/science/start_science.sh | 42 - setup/tmux_config.conf | 40 - setup/turnOnRaman.sh | 30 - src/{ => Arm}/arm_srdf/.setup_assistant | 0 src/{ => Arm}/arm_srdf/CMakeLists.txt | 0 src/{ => Arm}/arm_srdf/config/arm_config.yaml | 0 .../config/arm_urdf.ros2_control.xacro | 0 src/{ => Arm}/arm_srdf/config/arm_urdf.srdf | 0 .../arm_srdf/config/arm_urdf.urdf.xacro | 0 .../arm_srdf/config/initial_positions.yaml | 0 .../arm_srdf/config/joint_limits.yaml | 0 src/{ => Arm}/arm_srdf/config/kinematics.yaml | 0 src/{ => Arm}/arm_srdf/config/moveit.rviz | 0 .../arm_srdf/config/moveit_controllers.yaml | 0 .../config/pilz_cartesian_limits.yaml | 0 .../arm_srdf/config/ros2_controllers.yaml | 0 src/{ => Arm}/arm_srdf/launch/demo.launch.py | 0 .../arm_srdf/launch/move_group.launch.py | 0 .../arm_srdf/launch/moveit_rviz.launch.py | 0 src/{ => Arm}/arm_srdf/launch/rsp.launch.py | 0 src/{ => Arm}/arm_srdf/launch/servo.launch.py | 0 .../arm_srdf/launch/setup_assistant.launch.py | 0 .../launch/spawn_controllers.launch.py | 0 .../launch/static_virtual_joint_tfs.launch.py | 0 src/{ => Arm}/arm_srdf/launch/talon.launch.py | 0 src/{ => Arm}/arm_srdf/launch/test.launch.py | 0 .../arm_srdf/launch/warehouse_db.launch.py | 0 src/{ => Arm}/arm_srdf/package.xml | 0 src/{ => Arm}/arm_urdf/arm_urdf/__init__.py | 0 src/{ => Arm}/arm_urdf/launch/launch.py | 0 .../arm_urdf/meshes/collision/Link_1.STL | Bin .../arm_urdf/meshes/collision/Link_2.STL | Bin .../arm_urdf/meshes/collision/Link_3.STL | Bin .../arm_urdf/meshes/collision/Link_4.STL | Bin .../arm_urdf/meshes/collision/Link_5.STL | Bin .../arm_urdf/meshes/collision/Link_6.STL | Bin .../arm_urdf/meshes/collision/Link_7.STL | Bin .../arm_urdf/meshes/visual/Link_1.STL | Bin .../arm_urdf/meshes/visual/Link_2.STL | Bin .../arm_urdf/meshes/visual/Link_3.STL | Bin .../arm_urdf/meshes/visual/Link_4.STL | Bin .../arm_urdf/meshes/visual/Link_5.STL | Bin .../arm_urdf/meshes/visual/Link_6.STL | Bin .../arm_urdf/meshes/visual/Link_7.STL | Bin src/{ => Arm}/arm_urdf/package.xml | 0 src/{ => Arm}/arm_urdf/resource/arm_urdf | 0 src/{ => Arm}/arm_urdf/setup.cfg | 0 src/{ => Arm}/arm_urdf/setup.py | 0 src/{ => Arm}/arm_urdf/test/test_copyright.py | 0 src/{ => Arm}/arm_urdf/test/test_flake8.py | 0 src/{ => Arm}/arm_urdf/test/test_pep257.py | 0 src/{ => Arm}/arm_urdf/urdf/arm_urdf.urdf | 0 src/{bringup => Bringup}/bringup/__init__.py | 0 src/{bringup => Bringup}/launch/arm.launch.py | 0 .../launch/arm_tasks.launch.py | 2 +- .../launch/basestation.launch.py | 0 .../launch/basic_drive.launch.py | 2 +- .../launch/core.launch.py | 0 .../launch/drive.launch.py | 2 +- .../launch/localization.launch.py | 0 .../launch/navigation.launch.py | 0 .../launch/science.launch.py | 2 +- .../launch/video_streaming.launch.py | 0 src/{bringup => Bringup}/package.xml | 0 src/{bringup => Bringup}/resource/bringup | 0 src/{bringup => Bringup}/setup.cfg | 0 src/{bringup => Bringup}/setup.py | 0 .../test/test_copyright.py | 0 src/{bringup => Bringup}/test/test_flake8.py | 0 src/{bringup => Bringup}/test/test_pep257.py | 0 .../gpio_controller}/LICENSE | 0 .../gpio_controller}/__init__.py | 0 .../gpio_controller/gpio_controller/lights.py | 0 .../gpio_controller/package.xml | 0 .../gpio_controller/resource/gpio_controller | 0 .../gpio_controller/setup.cfg | 0 src/{ => HW-Devices}/gpio_controller/setup.py | 0 .../gpio_controller}/test/test_copyright.py | 0 .../gpio_controller}/test/test_flake8.py | 0 .../gpio_controller}/test/test_pep257.py | 0 src/{ => HW-Devices}/hardware/CMakeLists.txt | 0 .../hardware/include/TalonSRXWrapper.hpp | 0 .../hardware/include/rover_arm.hpp | 0 src/{ => HW-Devices}/hardware/package.xml | 0 .../hardware/ros2_control_rover_arm.xml | 0 .../hardware/src/TalonSRXWrapper.cpp | 0 .../hardware/src/rover_arm.cpp | 0 .../ros_phoenix_humble/.gitignore | 0 .../ros_phoenix_humble/CMakeLists.txt | 0 .../ros_phoenix_humble/LICENSE | 0 .../ros_phoenix_humble/README.md | 0 .../include/ros_phoenix/base_node.hpp | 0 .../include/ros_phoenix/phoenix_bridge.hpp | 0 .../ros_phoenix/phoenix_components.hpp | 0 .../include/ros_phoenix/phoenix_manager.hpp | 0 .../include/ros_phoenix/phoenix_node.hpp | 0 .../include/ros_phoenix/phoenix_nodes.hpp | 0 .../include/ros_phoenix/phoenix_system.hpp | 0 .../ros_phoenix_humble/launch/demo.launch.py | 0 .../ros_phoenix_humble/msg/MotorControl.msg | 0 .../ros_phoenix_humble/msg/MotorStatus.msg | 0 .../ros_phoenix_humble/package.xml | 0 .../phoenix_bridge_example.ros2_control.xacro | 0 .../phoenix_system_example.ros2_control.xacro | 0 .../ros_phoenix_hardware.xml | 0 .../ros_phoenix_humble/src/base_node.cpp | 0 .../ros_phoenix_humble/src/phoenix_bridge.cpp | 0 .../src/phoenix_components.cpp | 0 .../src/phoenix_container.cpp | 0 .../src/phoenix_manager.cpp | 0 .../ros_phoenix_humble/src/phoenix_nodes.cpp | 0 .../ros_phoenix_humble/src/phoenix_system.cpp | 0 .../science_sensors/README.md | 0 .../launch/can_module_reader.launch.py | 0 .../launch/gas_sensor.launch.py | 0 .../science_sensors/launch/gpio.launch.py | 0 .../launch/panoramic.launch.py | 0 .../science_sensors/launch/talon.launch.py | 0 .../science_sensors/package.xml | 0 .../science_sensors/resource/science_sensors | 0 .../science_sensors}/__init__.py | 0 .../science_sensors/can_module_sensor.py | 0 .../science_sensors/gas_sensor.py | 0 .../science_sensors/microscope_control.py | 0 .../science_sensors/panoramic.py | 0 .../science_sensors/pi_gpio_controller.py | 0 .../science_sensors/pi_gpio_reader.py | 0 .../science_sensors/science_sensors/plot.py | 0 .../science_sensors/science_sensors/raman.py | 0 .../science_sensors/stellarnet_demo.py | 0 .../stellarnet_driverLibs}/__init__.py | 0 .../stellarnet_driver3.cp27-win32.pyd | Bin .../stellarnet_driver3.cp27-win_amd64.pyd | Bin .../stellarnet_driver3.cp310-win32.pyd | Bin .../stellarnet_driver3.cp310-win_amd64.pyd | Bin .../stellarnet_driver3.cp311-win_amd64.pyd | Bin .../stellarnet_driver3.cp312-win_amd64.pyd | Bin .../stellarnet_driver3.cp35-win32.pyd | Bin .../stellarnet_driver3.cp35-win_amd64.pyd | Bin .../stellarnet_driver3.cp36-win32.pyd | Bin .../stellarnet_driver3.cp36-win_amd64.pyd | Bin .../stellarnet_driver3.cp37-win32.pyd | Bin .../stellarnet_driver3.cp37-win_amd64.pyd | Bin .../stellarnet_driver3.cp38-win32.pyd | Bin .../stellarnet_driver3.cp38-win_amd64.pyd | Bin .../stellarnet_driver3.cp39-win32.pyd | Bin .../stellarnet_driver3.cp39-win_amd64.pyd | Bin ...t_driver3.cpython-310-aarch64-linux-gnu.so | Bin ...driver3.cpython-310-arm-linux-gnueabihf.so | Bin .../stellarnet_driver3.cpython-310-darwin.so | Bin ...et_driver3.cpython-310-x86_64-linux-gnu.so | Bin ...t_driver3.cpython-311-aarch64-linux-gnu.so | Bin ...driver3.cpython-311-arm-linux-gnueabihf.so | Bin .../stellarnet_driver3.cpython-311-darwin.so | Bin ...et_driver3.cpython-311-x86_64-linux-gnu.so | Bin ...t_driver3.cpython-312-aarch64-linux-gnu.so | Bin .../stellarnet_driver3.cpython-312-darwin.so | Bin ...et_driver3.cpython-312-x86_64-linux-gnu.so | Bin ...et_driver3.cpython-35m-x86_64-linux-gnu.so | Bin ...t_driver3.cpython-36m-aarch64-linux-gnu.so | Bin ...et_driver3.cpython-36m-x86_64-linux-gnu.so | Bin ...t_driver3.cpython-37m-aarch64-linux-gnu.so | Bin ...driver3.cpython-37m-arm-linux-gnueabihf.so | Bin .../stellarnet_driver3.cpython-37m-darwin.so | Bin ...et_driver3.cpython-37m-x86_64-linux-gnu.so | Bin ...et_driver3.cpython-38-aarch64-linux-gnu.so | Bin ..._driver3.cpython-38-arm-linux-gnueabihf.so | Bin .../stellarnet_driver3.cpython-38-darwin.so | Bin ...net_driver3.cpython-38-x86_64-linux-gnu.so | Bin ...et_driver3.cpython-39-aarch64-linux-gnu.so | Bin .../stellarnet_driver3.cpython-39-darwin.so | Bin ...net_driver3.cpython-39-x86_64-linux-gnu.so | Bin .../windows_only/InstallDriver.exe | Bin .../StellarNet_Python_USB_Driver.cat | Bin .../StellarNet_Python_USB_Driver.inf | Bin .../windows_only/dpinst32.exe | Bin .../windows_only/dpinst64.exe | Bin .../science_sensors/setup.cfg | 0 src/{ => HW-Devices}/science_sensors/setup.py | 0 .../science_sensors}/test/test_copyright.py | 0 .../science_sensors}/test/test_flake8.py | 0 .../science_sensors}/test/test_pep257.py | 0 src/{ => HW-Devices}/servo_pkg/README.md | 0 .../servo_pkg/config/pi_controller.yaml | 0 .../servo_pkg/config/usb_controller.yaml | 0 .../launch/i2c_servo_launch.launch.py | 0 .../launch/pi_servo_launch.launch.py | 0 .../launch/usb_servo_launch.launch.py | 0 src/{ => HW-Devices}/servo_pkg/package.xml | 0 .../servo_pkg/resource/servo_pkg | 0 .../servo_pkg/servo_pkg/USB_Servo.py | 0 .../servo_pkg/servo_pkg}/__init__.py | 0 .../servo_pkg/servo_pkg/i2c_Servo.py | 0 .../servo_pkg/servo_pkg/maestro.py | 0 .../servo_pkg/servo_pkg/pi_Servo.py | 0 .../servo_pkg/servo_pkg/servo_client.py | 0 src/{ => HW-Devices}/servo_pkg/setup.cfg | 0 src/{ => HW-Devices}/servo_pkg/setup.py | 0 .../servo_pkg}/test/test_copyright.py | 0 .../servo_pkg}/test/test_flake8.py | 0 .../servo_pkg}/test/test_pep257.py | 0 .../drive_cpp/CMakeLists.txt | 7 +- .../drive_cpp}/config/pxn.yaml | 0 .../drive_cpp}/config/talon_drive_common.yaml | 0 .../drive_cpp}/config/talon_drive_unique.yaml | 0 .../drive_cpp}/config/thrustmaster.yaml | 0 .../drive_cpp/TalonDriveController.hpp | 0 .../include/drive_cpp/WheelControl.hpp | 0 .../drive_cpp}/launch/talon.launch.py | 4 +- .../drive_cpp/package.xml | 0 .../drive_cpp/src/TalonDriveController.cpp | 0 .../drive_cpp/src/WheelControl.cpp | 0 .../drive_cpp/src/main.cpp | 0 .../talon_drive_controller_plugin.xml | 0 .../joystick_control/CMakeLists.txt | 0 .../include/joystick_control/ArmDummyMode.hpp | 0 .../include/joystick_control/ArmHelpers.hpp | 0 .../include/joystick_control/ArmIKMode.hpp | 0 .../joystick_control/ArmManualMode.hpp | 0 .../include/joystick_control/DriveMode.hpp | 0 .../joystick_control/FlightstickControl.hpp | 0 .../include/joystick_control/Mode.hpp | 0 .../include/joystick_control/ScienceMode.hpp | 0 .../launch/controller.launch.py | 2 +- .../joystick_control/package.xml | 0 .../joystick_control/src/ArmDummyMode.cpp | 0 .../joystick_control/src/ArmHelpers.cpp | 0 .../joystick_control/src/ArmIKMode.cpp | 0 .../joystick_control/src/ArmManualMode.cpp | 0 .../joystick_control/src/DriveMode.cpp | 0 .../src/FlightstickControl.cpp | 0 .../joystick_control/src/ScienceMode.cpp | 0 .../camera_controller/cam_control.py | 103 -- .../launch/camera_controller.launch.py | 27 - src/camera_controller/package.xml | 20 - src/camera_controller/setup.cfg | 4 - src/camera_controller/setup.py | 31 - src/drive/drive/drive_station.py | 94 -- src/drive/drive/electrical_wrapper.py | 56 - src/drive/drive/encoder_wrapper.py | 202 --- src/drive/drive/joystick_breakout.py | 48 - src/drive/drive/joystick_controller.py | 182 --- src/drive/drive/joystick_drive.py | 126 -- src/drive/drive/roboclaw_driver.py | 1262 ----------------- src/drive/drive/roboclaw_node.py | 391 ----- src/drive/drive/utils.py | 36 - src/drive/launch/flightstick_drive.launch.py | 54 - src/drive/launch/xbox_drive.launch.py | 47 - src/drive/package.xml | 32 - src/drive/setup.cfg | 4 - src/drive/setup.py | 37 - src/drive/test/test_copyright.py | 23 - src/interfaces/CMakeLists.txt | 8 - src/interfaces/msg/ArmCmd.msg | 12 - src/interfaces/msg/GPIOmsg.msg | 2 - src/interfaces/msg/LiveTune.msg | 3 - src/interfaces/msg/PIDmsg.msg | 3 - src/interfaces/msg/PointArray.msg | 5 - src/interfaces/msg/SixFloats.msg | 6 - src/interfaces/srv/ArmCmd.srv | 8 - src/interfaces/srv/ArmPos.srv | 8 - src/keyboard_publisher/CMakeLists.txt | 41 - src/keyboard_publisher/package.xml | 22 - .../src/keyboard_publisher.cpp | 95 -- .../src/keyboard_publisher.h | 32 - src/moveit_controller/CMakeLists.txt | 62 - src/moveit_controller/package.xml | 23 - .../src/moveit_controller.cpp | 130 -- src/moveit_controller/src/moveit_controller.h | 47 - src/moveit_controller/src/servo_keyboard.cpp | 213 --- src/moveit_controller/src/servo_keyboard.h | 77 - .../cprt_costmap_plugins/CMakeLists.txt | 0 .../cprt_costmap_plugins/costmap_plugins.xml | 0 .../cprt_costmap_plugins/gridmap_layer.hpp | 0 .../cprt_costmap_plugins/package.xml | 0 .../src/gridmap_layer.cpp | 0 .../cprt_gridmap_filters/CMakeLists.txt | 0 .../cprt_gridmap_filters}/LICENSE | 0 .../cprt_gridmap_filters/filter_plugins.xml | 0 .../include/PreserveCostInflationFilter.hpp | 0 .../cprt_gridmap_filters/package.xml | 0 .../scripts/calculate_decay_rate.py | 0 .../scripts/visualize_decay_rate.py | 0 .../src/PreserveCostInflationFilter.cpp | 0 .../cprt_planner_plugins/CMakeLists.txt | 0 .../cprt_planner_plugins}/LICENSE | 0 .../global_planner_plugin.xml | 0 .../event_horizon_planner.hpp | 0 .../cprt_planner_plugins/package.xml | 0 .../src/event_horizon_planner.cpp | 0 .../elevation_mapping/CMakeLists.txt | 0 src/{ => nav}/elevation_mapping/README.md | 0 .../config/elevation_maps/long_range.yaml | 0 .../config/elevation_maps/remove_object.yaml | 0 .../elevation_maps/simple_demo_map.yaml | 0 .../config/elevation_maps/starleth_map.yaml | 0 .../config/elevation_maps/zed2i_map.yaml | 0 .../elevation_mapping/config/file_path.yaml | 0 .../postprocessor_pipeline.yaml | 0 .../traversability_pipeline.yaml | 0 .../config/robots/ground_truth_demo.yaml | 0 .../config/robots/remove_object.yaml | 0 .../config/robots/simple_demo_robot.yaml | 0 .../config/robots/starleth.yaml | 0 .../config/robots/waffle_robot.yaml | 0 .../config/robots/zed2i_robot.yaml | 0 .../elevation_mapping/config/rosconsole.conf | 0 .../config/sensor_processors/RSLidar-32.yaml | 0 .../config/sensor_processors/aslam.yaml | 0 .../config/sensor_processors/fotonic_g45.yaml | 0 .../sensor_processors/hokuyo_utm-30lx.yaml | 0 .../kinect_nguyen_et_al.yaml | 0 .../config/sensor_processors/perfect.yaml | 0 ...nse_carmine_109_short_range_datasheet.yaml | 0 ..._carmine_109_short_range_uncalibrated.yaml | 0 .../sensor_processors/realsense_ZR300.yaml | 0 .../sensor_processors/realsense_d435.yaml | 0 .../sensor_processors/velodyne_HDL-32E.yaml | 0 .../sensor_processors/zed2i_processor.yaml | 0 .../config/visualization/fused.yaml | 0 .../config/visualization/raw.yaml | 0 .../config/zed_configs/common_stereo.yaml | 0 .../config/zed_configs/zed2i.yaml | 0 .../elevation_mapping/docker/.gitignore | 0 .../docker/Dockerfile.elevation_mapping | 0 .../docker/Dockerfile.ros2_humble | 0 .../elevation_mapping/docker/Dockerfile.user | 0 .../elevation_mapping/docker/build_docker.sh | 0 .../docker/container_scripts/.dockerignore | 0 .../container_scripts/workspace-entrypoint.sh | 0 .../middleware_profiles/rtps_udp_profile.xml | 0 .../docker/rosdep/extra_rosdeps.yaml | 0 .../elevation_mapping/docker/run_docker.sh | 0 .../docker/sourced_scripts/bash_functions.sh | 0 .../elevation_mapping/docker_instructions.md | 0 .../elevation_mapping/ElevationMap.hpp | 0 .../ElevationMapFunctors.hpp | 0 .../elevation_mapping/ElevationMapping.hpp | 0 .../PointXYZRGBConfidenceRatio.hpp | 0 .../RobotMotionMapUpdater.hpp | 0 .../ThreadSafeDataWrapper.hpp | 0 ...mpiricalCumulativeDistributionFunction.hpp | 0 .../elevation_mapping_node.hpp | 0 .../elevation_mapping/input_sources/Input.hpp | 0 .../input_sources/InputSourceManager.hpp | 0 .../PostprocessingPipelineFunctor.hpp | 0 .../postprocessing/PostprocessingWorker.hpp | 0 .../postprocessing/PostprocessorPool.hpp | 0 .../LaserSensorProcessor.hpp | 0 .../PerfectSensorProcessor.hpp | 0 .../sensor_processors/SensorProcessorBase.hpp | 0 .../StereoSensorProcessor.hpp | 0 .../StructuredLightSensorProcessor.hpp | 0 .../launch/elevationMapping_launch.py | 0 .../elevation_mapping/launch/rviz.launch.py | 0 .../launch/visualization.launch.py | 0 .../elevation_mapping/launch/zed2i.launch.py | 0 src/{ => nav}/elevation_mapping/package.xml | 0 .../elevation_mapping/rviz2/custom_rviz2.rviz | 0 .../elevation_mapping/rviz2/map.rviz | 0 .../elevation_mapping/rviz2/viz.rviz | 0 .../elevation_mapping/rviz2/zed2i.rviz | 0 .../elevation_mapping/sample_data/maze.ply | Bin .../elevation_mapping/src/ElevationMap.cpp | 0 .../src/ElevationMapping.cpp | 0 .../src/PointXYZRGBConfidenceRatio.cpp | 0 .../src/RobotMotionMapUpdater.cpp | 0 .../src/elevation_mapping_node.cpp | 0 .../src/input_sources/Input.cpp | 0 .../src/input_sources/InputSourceManager.cpp | 0 .../PostprocessingPipelineFunctor.cpp | 0 .../postprocessing/PostprocessingWorker.cpp | 0 .../src/postprocessing/PostprocessorPool.cpp | 0 .../LaserSensorProcessor.cpp | 0 .../PerfectSensorProcessor.cpp | 0 .../sensor_processors/SensorProcessorBase.cpp | 0 .../StereoSensorProcessor.cpp | 0 .../StructuredLightSensorProcessor.cpp | 0 src/{ => nav}/elevation_mapping/src/test.cpp | 0 .../elevation_mapping/src/test_service.cpp | 0 .../test/ElevationMapTest.cpp | 0 ...icalCumulativeDistributionFunctionTest.cpp | 0 .../test/elevation_mapping.test | 0 .../test/input_sources/InputSourcesTest.cpp | 0 .../config/TestConfigurations.yaml | 0 .../input_sources/config/rosconsole.config | 0 .../test/input_sources/input_sources.test | 0 .../test/input_sources/test_input_sources.cpp | 0 .../test/postprocessing/PostprocessorTest.cpp | 0 .../test/postprocessing/main.cpp | 0 .../test/postprocessing/postprocessor.test | 0 .../postprocessor_pipeline.yaml | 0 .../test/test_elevation_mapping.cpp | 0 src/{ => nav}/gps/config/Heading_config.ubx | Bin src/{ => nav}/gps/config/Rover_config.ubx | Bin src/{ => nav}/gps/config/gps.yaml | 0 src/{ => nav}/gps/gps/__init__.py | 0 src/{ => nav}/gps/gps/heading_pub_node.py | 0 src/{ => nav}/gps/gps/rtcm_pub_node.py | 0 src/{ => nav}/gps/gps/ubx_io_manager.py | 0 .../gps/launch/basestation.launch.py | 0 src/{ => nav}/gps/launch/heading.launch.py | 0 src/{ => nav}/gps/launch/rover.launch.py | 0 src/{ => nav}/gps/package.xml | 0 src/{ => nav}/gps/resource/gps | 0 src/{ => nav}/gps/setup.cfg | 0 src/{ => nav}/gps/setup.py | 0 src/{ => nav}/gps/test/test_copyright.py | 0 .../gps}/test/test_flake8.py | 0 .../gps}/test/test_pep257.py | 0 src/nav/kindr_ros/.gitignore | 36 + src/nav/kindr_ros/LICENSE | 29 + src/nav/kindr_ros/README.md | 43 + src/nav/kindr_ros/kindr_msgs/CMakeLists.txt | 46 + .../kindr_msgs/msg/VectorAtPosition.msg | 22 + src/nav/kindr_ros/kindr_msgs/package.xml | 34 + src/nav/kindr_ros/kindr_ros/CMakeLists.txt | 100 ++ .../RosGeometryMsgPhysicalQuantities.hpp | 108 ++ .../include/kindr_ros/RosGeometryMsgPose.hpp | 109 ++ .../kindr_ros/RosGeometryMsgRotation.hpp | 68 + .../include/kindr_ros/RosGeometryMsgTwist.hpp | 67 + .../kindr_ros/include/kindr_ros/RosTfPose.hpp | 110 ++ .../kindr_ros/include/kindr_ros/kindr_ros.hpp | 44 + src/nav/kindr_ros/kindr_ros/package.xml | 32 + .../kindr_ros/kindr_ros/test/CMakeLists.txt | 13 + .../RosGeometryMsgPhysicalQuantitiesTest.cpp | 132 ++ .../kindr_ros/test/RosGeometryMsgPoseTest.cpp | 129 ++ .../test/RosGeometryMsgRotationTest.cpp | 68 + .../kindr_ros/test/RosTfPoseTest.cpp | 318 +++++ .../kindr_ros/test/TfConventionTest.cpp | 155 ++ .../kindr_ros/kindr_ros/test/test_main.cpp | 36 + .../kindr_rviz_plugins/CMakeLists.txt | 122 ++ .../kindr_rviz_plugins/COLCON_IGNORE} | 0 .../icons/classes/VectorAtPosition.png | Bin 0 -> 585 bytes .../VectorAtPositionDisplay.hpp | 109 ++ .../VectorAtPositionVisual.hpp | 111 ++ .../kindr_ros/kindr_rviz_plugins/package.xml | 26 + .../kindr_rviz_plugins/plugin_description.xml | 9 + .../src/VectorAtPositionDisplay.cpp | 224 +++ .../src/VectorAtPositionVisual.cpp | 194 +++ .../kindr_rviz_plugins/test/EmptyTests.cpp | 5 + .../CMakeLists.txt | 120 ++ .../COLCON_IGNORE} | 0 .../icons/classes/MultiDOFJointTrajectory.png | Bin 0 -> 273 bytes .../MultiDOFJointTrajectoryDisplay.hpp | 159 +++ ...OFJointTrajectoryPointConnectionVisual.hpp | 47 + .../MultiDOFJointTrajectoryPointVisual.hpp | 135 ++ .../package.xml | 26 + .../plugin_description.xml | 9 + .../src/MultiDOFJointTrajectoryDisplay.cpp | 652 +++++++++ ...OFJointTrajectoryPointConnectionVisual.cpp | 69 + .../MultiDOFJointTrajectoryPointVisual.cpp | 438 ++++++ .../test/EmptyTests.cpp | 5 + .../localization/config/driver_params.yaml | 0 .../config/ekf/gps_ekf_global.yaml | 0 .../config/ekf/gps_ekf_local.yaml | 0 src/{ => nav}/localization/config/navsat.yaml | 0 .../localization/config/rtabmap.yaml | 0 .../config/rviz.views/basicNavMap.rviz | 0 src/{ => nav}/localization/config/slam.yaml | 0 .../localization/config/urdf/spike.urdf.xml | 0 .../localization/launch/description.launch.py | 0 .../localization/launch/ekf.launch.py | 0 .../launch/localization.launch.py | 0 .../localization/launch/navsat.launch.py | 0 .../localization/launch/rtabmap.launch.py | 0 .../localization/localization/__init__.py | 0 .../localization/localization/imu_filter.py | 0 .../localization/lidar_mask_tool.py | 0 .../localization/republish_odometry.py | 0 src/{ => nav}/localization/package.xml | 0 .../localization/resource/localization | 0 src/{ => nav}/localization/setup.cfg | 0 src/{ => nav}/localization/setup.py | 0 .../old/drumheller_traversal_guess.yaml | 0 .../old/edc_crosswalk_to_res.yaml | 0 .../old/edcfront_northeast.yaml | 0 .../old/edcfront_ramp_to_edcparkinglot.yaml | 0 .../old/edcfront_southwest.yaml | 0 .../old/edcfront_trees_grass.yaml | 0 .../known_gps_coords/old/useful_locations.md | 0 .../known_gps_coords/send_gps_command.py | 0 .../launch/aruco_gps_commander.py | 0 .../launch/gps_commander.launch.py | 0 .../incremental_gps_commander.launch.py | 0 .../nav_commanders/nav_commanders/__init__.py | 0 .../nav_commanders/gps_utils.py | 0 .../incremental_gps_commander.py | 0 .../nav_commanders/nav_to_gps_aruco.py | 0 .../nav_commanders/nav_to_gps_coords.py | 0 src/{ => nav}/nav_commanders/package.xml | 0 .../nav_commanders/resource/nav_commanders | 0 src/{ => nav}/nav_commanders/setup.cfg | 0 src/{ => nav}/nav_commanders/setup.py | 0 .../nav_commanders/test/test_copyright.py | 0 .../nav_commanders}/test/test_flake8.py | 0 .../nav_commanders}/test/test_pep257.py | 0 src/{ => nav}/navigation/CMakeLists.txt | 0 .../behavior_trees/behaviorTree.xml | 0 .../behaviorTree_NavPathingTestTree.xml | 0 .../behavior_trees/behaviorTree_original.xml | 0 .../behavior_trees/bt_dynamic_replanning.xml | 0 .../navigation/behavior_trees/spike_bt.xml | 0 .../ouster_OS0_32U_sensor_processor.yaml | 0 .../ouster_elevation_mapping.yaml | 0 .../postprocessing_traversability.yaml | 0 .../config/elevation_mapping/spike_robot.yaml | 0 .../zed2i_elevation_mapping.yaml | 0 .../zed2i_sensor_processor.yaml | 0 .../config/ouster/driver_params.yaml | 0 .../config/zed/zed_override_params.yaml | 0 .../navigation/launch/bringup.launch.py | 0 .../navigation/launch/nav2.launch.py | 0 .../navigation/launch/navigation.launch.py | 0 .../navigation/launch/ouster.launch.py | 0 .../launch/ouster_composable.launch.py | 0 .../launch/traversability_gridmap.launch.py | 0 src/{ => nav}/navigation/launch/zed.launch.py | 0 .../ouster_mask_combined_shifted.png | Bin .../lidar_masks/ouster_mask_front.png | Bin .../lidar_masks/ouster_mask_left.png | Bin .../lidar_masks/ouster_mask_rear.png | Bin .../lidar_masks/ouster_mask_right.png | Bin src/{ => nav}/navigation/package.xml | 0 src/{ => nav}/navigation/params/nav2.yaml | 0 src/{ => nav}/navigation/rviz/nav2_cprt.rviz | 0 .../rviz/nav2_offical_default_config.rviz | 0 .../rviz/traversibility_gridmap.rviz | 0 src/ouster-ros | 1 - .../stellarnet_driverLibs/__init__.py | 0 src/science_sensors/test/test_flake8.py | 25 - src/science_sensors/test/test_pep257.py | 23 - src/servo_pkg/servo_pkg/__init__.py | 0 src/servo_pkg/test/test_copyright.py | 27 - src/servo_pkg/test/test_flake8.py | 25 - src/servo_pkg/test/test_pep257.py | 23 - .../system-telemetry-cpp/CMakeLists.txt | 0 .../system-telemetry-cpp/LICENSE | 0 .../system-telemetry-cpp/cpu_collector.hpp | 0 .../system-telemetry-cpp/gpu_collector.hpp | 0 .../system-telemetry-cpp/memory_collector.hpp | 0 .../system_telemetry_publisher.hpp | 0 .../telemetry_collector.hpp | 0 .../system-telemetry-cpp/package.xml | 0 .../src/cpu_collector.cpp | 0 .../src/gpu_collector.cpp | 0 .../src/memory_collector.cpp | 0 .../src/system_telemetry_publisher.cpp | 0 src/ublox | 1 - .../camera_streaming/CMakeLists.txt | 0 .../camera_streaming/config/webrtc.yaml | 0 .../gst-plugins/CMakeLists.txt | 0 .../gst-plugins/gstarucomarker.cpp | 0 .../gst-plugins/gstarucomarker.hpp | 0 .../camera_streaming/hooks/gst_env_hook.sh | 0 .../include/camera_streaming/webrtc_node.h | 0 .../camera_streaming/launch/webRTC.launch.py | 0 src/{ => video}/camera_streaming/package.xml | 0 .../camera_streaming/src/webrtc_node.cpp | 0 .../camera_streaming_qt/.gitignore | 0 .../camera_streaming_qt/CMakeLists.txt | 0 .../camera_streaming_qt/camera_client.h | 0 .../include/camera_streaming_qt/mainwindow.h | 0 .../tests/test_camera_node.h | 0 .../widgets/capture_image_widget.h | 0 .../camera_streaming_qt/widgets/main_widget.h | 0 .../widgets/preset_widget.h | 0 .../widgets/source_widget.h | 0 .../camera_streaming_qt/package.xml | 0 .../camera_streaming_qt/src/camera_client.cpp | 0 .../camera_streaming_qt/src/mainwindow.cpp | 0 .../src/tests/test_camera_node.cpp | 0 .../src/widgets/capture_image_widget.cpp | 0 .../src/widgets/main_widget.cpp | 0 .../src/widgets/preset_widget.cpp | 0 .../src/widgets/source_widget.cpp | 0 .../computer_vision}/__init__.py | 0 .../computer_vision/zed_aruco_detector.py | 0 .../launch/aruco_detector_zed.launch.py | 0 src/{ => video}/computer_vision/package.xml | 0 .../computer_vision/resource/computer_vision} | 0 src/{ => video}/computer_vision/setup.cfg | 0 src/{ => video}/computer_vision/setup.py | 0 .../computer_vision}/test/test_copyright.py | 0 .../computer_vision}/test/test_flake8.py | 0 .../computer_vision}/test/test_pep257.py | 0 src/zed-ros2-wrapper | 1 - 599 files changed, 4175 insertions(+), 4600 deletions(-) delete mode 100755 setup/cprt_bash_aliases.sh delete mode 100755 setup/cprt_bash_rc.sh delete mode 100644 setup/fastdds_config.xml delete mode 100755 setup/install_gstreamer.sh delete mode 100755 setup/install_nav_deps.sh delete mode 100755 setup/install_ros_humble.sh delete mode 100755 setup/science/setup_science.sh delete mode 100644 setup/science/start_science.service delete mode 100755 setup/science/start_science.sh delete mode 100644 setup/tmux_config.conf delete mode 100755 setup/turnOnRaman.sh rename src/{ => Arm}/arm_srdf/.setup_assistant (100%) rename src/{ => Arm}/arm_srdf/CMakeLists.txt (100%) rename src/{ => Arm}/arm_srdf/config/arm_config.yaml (100%) rename src/{ => Arm}/arm_srdf/config/arm_urdf.ros2_control.xacro (100%) rename src/{ => Arm}/arm_srdf/config/arm_urdf.srdf (100%) rename src/{ => Arm}/arm_srdf/config/arm_urdf.urdf.xacro (100%) rename src/{ => Arm}/arm_srdf/config/initial_positions.yaml (100%) rename src/{ => Arm}/arm_srdf/config/joint_limits.yaml (100%) rename src/{ => Arm}/arm_srdf/config/kinematics.yaml (100%) rename src/{ => Arm}/arm_srdf/config/moveit.rviz (100%) rename src/{ => Arm}/arm_srdf/config/moveit_controllers.yaml (100%) rename src/{ => Arm}/arm_srdf/config/pilz_cartesian_limits.yaml (100%) rename src/{ => Arm}/arm_srdf/config/ros2_controllers.yaml (100%) rename src/{ => Arm}/arm_srdf/launch/demo.launch.py (100%) rename src/{ => Arm}/arm_srdf/launch/move_group.launch.py (100%) rename src/{ => Arm}/arm_srdf/launch/moveit_rviz.launch.py (100%) rename src/{ => Arm}/arm_srdf/launch/rsp.launch.py (100%) rename src/{ => Arm}/arm_srdf/launch/servo.launch.py (100%) rename src/{ => Arm}/arm_srdf/launch/setup_assistant.launch.py (100%) rename src/{ => Arm}/arm_srdf/launch/spawn_controllers.launch.py (100%) rename src/{ => Arm}/arm_srdf/launch/static_virtual_joint_tfs.launch.py (100%) rename src/{ => Arm}/arm_srdf/launch/talon.launch.py (100%) rename src/{ => Arm}/arm_srdf/launch/test.launch.py (100%) rename src/{ => Arm}/arm_srdf/launch/warehouse_db.launch.py (100%) rename src/{ => Arm}/arm_srdf/package.xml (100%) rename src/{ => Arm}/arm_urdf/arm_urdf/__init__.py (100%) rename src/{ => Arm}/arm_urdf/launch/launch.py (100%) rename src/{ => Arm}/arm_urdf/meshes/collision/Link_1.STL (100%) rename src/{ => Arm}/arm_urdf/meshes/collision/Link_2.STL (100%) rename src/{ => Arm}/arm_urdf/meshes/collision/Link_3.STL (100%) rename src/{ => Arm}/arm_urdf/meshes/collision/Link_4.STL (100%) rename src/{ => Arm}/arm_urdf/meshes/collision/Link_5.STL (100%) rename src/{ => Arm}/arm_urdf/meshes/collision/Link_6.STL (100%) rename src/{ => Arm}/arm_urdf/meshes/collision/Link_7.STL (100%) rename src/{ => Arm}/arm_urdf/meshes/visual/Link_1.STL (100%) rename src/{ => Arm}/arm_urdf/meshes/visual/Link_2.STL (100%) rename src/{ => Arm}/arm_urdf/meshes/visual/Link_3.STL (100%) rename src/{ => Arm}/arm_urdf/meshes/visual/Link_4.STL (100%) rename src/{ => Arm}/arm_urdf/meshes/visual/Link_5.STL (100%) rename src/{ => Arm}/arm_urdf/meshes/visual/Link_6.STL (100%) rename src/{ => Arm}/arm_urdf/meshes/visual/Link_7.STL (100%) rename src/{ => Arm}/arm_urdf/package.xml (100%) rename src/{ => Arm}/arm_urdf/resource/arm_urdf (100%) rename src/{ => Arm}/arm_urdf/setup.cfg (100%) rename src/{ => Arm}/arm_urdf/setup.py (100%) rename src/{ => Arm}/arm_urdf/test/test_copyright.py (100%) rename src/{ => Arm}/arm_urdf/test/test_flake8.py (100%) rename src/{ => Arm}/arm_urdf/test/test_pep257.py (100%) rename src/{ => Arm}/arm_urdf/urdf/arm_urdf.urdf (100%) rename src/{bringup => Bringup}/bringup/__init__.py (100%) rename src/{bringup => Bringup}/launch/arm.launch.py (100%) rename src/{bringup => Bringup}/launch/arm_tasks.launch.py (96%) rename src/{bringup => Bringup}/launch/basestation.launch.py (100%) rename src/{bringup => Bringup}/launch/basic_drive.launch.py (95%) rename src/{bringup => Bringup}/launch/core.launch.py (100%) rename src/{bringup => Bringup}/launch/drive.launch.py (94%) rename src/{bringup => Bringup}/launch/localization.launch.py (100%) rename src/{bringup => Bringup}/launch/navigation.launch.py (100%) rename src/{bringup => Bringup}/launch/science.launch.py (96%) rename src/{bringup => Bringup}/launch/video_streaming.launch.py (100%) rename src/{bringup => Bringup}/package.xml (100%) rename src/{bringup => Bringup}/resource/bringup (100%) rename src/{bringup => Bringup}/setup.cfg (100%) rename src/{bringup => Bringup}/setup.py (100%) rename src/{bringup => Bringup}/test/test_copyright.py (100%) rename src/{bringup => Bringup}/test/test_flake8.py (100%) rename src/{bringup => Bringup}/test/test_pep257.py (100%) rename src/{cprt_gridmap_filters => HW-Devices/gpio_controller}/LICENSE (100%) rename src/{camera_controller/camera_controller => HW-Devices/gpio_controller/gpio_controller}/__init__.py (100%) rename src/{ => HW-Devices}/gpio_controller/gpio_controller/lights.py (100%) rename src/{ => HW-Devices}/gpio_controller/package.xml (100%) rename src/{ => HW-Devices}/gpio_controller/resource/gpio_controller (100%) rename src/{ => HW-Devices}/gpio_controller/setup.cfg (100%) rename src/{ => HW-Devices}/gpio_controller/setup.py (100%) rename src/{camera_controller => HW-Devices/gpio_controller}/test/test_copyright.py (100%) rename src/{camera_controller => HW-Devices/gpio_controller}/test/test_flake8.py (100%) rename src/{camera_controller => HW-Devices/gpio_controller}/test/test_pep257.py (100%) rename src/{ => HW-Devices}/hardware/CMakeLists.txt (100%) rename src/{ => HW-Devices}/hardware/include/TalonSRXWrapper.hpp (100%) rename src/{ => HW-Devices}/hardware/include/rover_arm.hpp (100%) rename src/{ => HW-Devices}/hardware/package.xml (100%) rename src/{ => HW-Devices}/hardware/ros2_control_rover_arm.xml (100%) rename src/{ => HW-Devices}/hardware/src/TalonSRXWrapper.cpp (100%) rename src/{ => HW-Devices}/hardware/src/rover_arm.cpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/.gitignore (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/CMakeLists.txt (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/LICENSE (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/README.md (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/include/ros_phoenix/base_node.hpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/include/ros_phoenix/phoenix_bridge.hpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/include/ros_phoenix/phoenix_components.hpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/include/ros_phoenix/phoenix_manager.hpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/include/ros_phoenix/phoenix_node.hpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/include/ros_phoenix/phoenix_nodes.hpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/include/ros_phoenix/phoenix_system.hpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/launch/demo.launch.py (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/msg/MotorControl.msg (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/msg/MotorStatus.msg (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/package.xml (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/ros2_control/phoenix_bridge_example.ros2_control.xacro (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/ros2_control/phoenix_system_example.ros2_control.xacro (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/ros_phoenix_hardware.xml (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/src/base_node.cpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/src/phoenix_bridge.cpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/src/phoenix_components.cpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/src/phoenix_container.cpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/src/phoenix_manager.cpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/src/phoenix_nodes.cpp (100%) rename src/{ => HW-Devices}/ros_phoenix_humble/src/phoenix_system.cpp (100%) rename src/{ => HW-Devices}/science_sensors/README.md (100%) rename src/{ => HW-Devices}/science_sensors/launch/can_module_reader.launch.py (100%) rename src/{ => HW-Devices}/science_sensors/launch/gas_sensor.launch.py (100%) rename src/{ => HW-Devices}/science_sensors/launch/gpio.launch.py (100%) rename src/{ => HW-Devices}/science_sensors/launch/panoramic.launch.py (100%) rename src/{ => HW-Devices}/science_sensors/launch/talon.launch.py (100%) rename src/{ => HW-Devices}/science_sensors/package.xml (100%) rename src/{ => HW-Devices}/science_sensors/resource/science_sensors (100%) rename src/{computer_vision/computer_vision => HW-Devices/science_sensors/science_sensors}/__init__.py (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/can_module_sensor.py (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/gas_sensor.py (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/microscope_control.py (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/panoramic.py (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/pi_gpio_controller.py (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/pi_gpio_reader.py (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/plot.py (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/raman.py (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_demo.py (100%) rename src/{drive/drive => HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs}/__init__.py (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win32.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp27-win_amd64.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win32.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp310-win_amd64.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp311-win_amd64.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp312-win_amd64.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win32.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp35-win_amd64.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win32.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp36-win_amd64.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win32.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp37-win_amd64.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win32.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp38-win_amd64.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win32.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cp39-win_amd64.pyd (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-aarch64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-arm-linux-gnueabihf.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-darwin.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-310-x86_64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-aarch64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-arm-linux-gnueabihf.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-darwin.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-311-x86_64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-aarch64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-darwin.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-312-x86_64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-35m-x86_64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-aarch64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-36m-x86_64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-aarch64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-arm-linux-gnueabihf.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-darwin.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-37m-x86_64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-aarch64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-arm-linux-gnueabihf.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-darwin.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-38-x86_64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-aarch64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-darwin.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/stellarnet_driver3.cpython-39-x86_64-linux-gnu.so (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/InstallDriver.exe (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.cat (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/StellarNet_Python_USB_Driver.inf (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst32.exe (100%) rename src/{ => HW-Devices}/science_sensors/science_sensors/stellarnet_driverLibs/windows_only/dpinst64.exe (100%) rename src/{ => HW-Devices}/science_sensors/setup.cfg (100%) rename src/{ => HW-Devices}/science_sensors/setup.py (100%) rename src/{computer_vision => HW-Devices/science_sensors}/test/test_copyright.py (100%) rename src/{computer_vision => HW-Devices/science_sensors}/test/test_flake8.py (100%) rename src/{computer_vision => HW-Devices/science_sensors}/test/test_pep257.py (100%) rename src/{ => HW-Devices}/servo_pkg/README.md (100%) rename src/{ => HW-Devices}/servo_pkg/config/pi_controller.yaml (100%) rename src/{ => HW-Devices}/servo_pkg/config/usb_controller.yaml (100%) rename src/{ => HW-Devices}/servo_pkg/launch/i2c_servo_launch.launch.py (100%) rename src/{ => HW-Devices}/servo_pkg/launch/pi_servo_launch.launch.py (100%) rename src/{ => HW-Devices}/servo_pkg/launch/usb_servo_launch.launch.py (100%) rename src/{ => HW-Devices}/servo_pkg/package.xml (100%) rename src/{ => HW-Devices}/servo_pkg/resource/servo_pkg (100%) rename src/{ => HW-Devices}/servo_pkg/servo_pkg/USB_Servo.py (100%) rename src/{gpio_controller/gpio_controller => HW-Devices/servo_pkg/servo_pkg}/__init__.py (100%) rename src/{ => HW-Devices}/servo_pkg/servo_pkg/i2c_Servo.py (100%) rename src/{ => HW-Devices}/servo_pkg/servo_pkg/maestro.py (100%) rename src/{ => HW-Devices}/servo_pkg/servo_pkg/pi_Servo.py (100%) rename src/{ => HW-Devices}/servo_pkg/servo_pkg/servo_client.py (100%) rename src/{ => HW-Devices}/servo_pkg/setup.cfg (100%) rename src/{ => HW-Devices}/servo_pkg/setup.py (100%) rename src/{gpio_controller => HW-Devices/servo_pkg}/test/test_copyright.py (100%) rename src/{drive => HW-Devices/servo_pkg}/test/test_flake8.py (100%) rename src/{drive => HW-Devices/servo_pkg}/test/test_pep257.py (100%) rename src/{ => Teleop-Control}/drive_cpp/CMakeLists.txt (93%) rename src/{drive => Teleop-Control/drive_cpp}/config/pxn.yaml (100%) rename src/{drive => Teleop-Control/drive_cpp}/config/talon_drive_common.yaml (100%) rename src/{drive => Teleop-Control/drive_cpp}/config/talon_drive_unique.yaml (100%) rename src/{drive => Teleop-Control/drive_cpp}/config/thrustmaster.yaml (100%) rename src/{ => Teleop-Control}/drive_cpp/include/drive_cpp/TalonDriveController.hpp (100%) rename src/{ => Teleop-Control}/drive_cpp/include/drive_cpp/WheelControl.hpp (100%) rename src/{drive => Teleop-Control/drive_cpp}/launch/talon.launch.py (93%) rename src/{ => Teleop-Control}/drive_cpp/package.xml (100%) rename src/{ => Teleop-Control}/drive_cpp/src/TalonDriveController.cpp (100%) rename src/{ => Teleop-Control}/drive_cpp/src/WheelControl.cpp (100%) rename src/{ => Teleop-Control}/drive_cpp/src/main.cpp (100%) rename src/{ => Teleop-Control}/drive_cpp/talon_drive_controller_plugin.xml (100%) rename src/{ => Teleop-Control}/joystick_control/CMakeLists.txt (100%) rename src/{ => Teleop-Control}/joystick_control/include/joystick_control/ArmDummyMode.hpp (100%) rename src/{ => Teleop-Control}/joystick_control/include/joystick_control/ArmHelpers.hpp (100%) rename src/{ => Teleop-Control}/joystick_control/include/joystick_control/ArmIKMode.hpp (100%) rename src/{ => Teleop-Control}/joystick_control/include/joystick_control/ArmManualMode.hpp (100%) rename src/{ => Teleop-Control}/joystick_control/include/joystick_control/DriveMode.hpp (100%) rename src/{ => Teleop-Control}/joystick_control/include/joystick_control/FlightstickControl.hpp (100%) rename src/{ => Teleop-Control}/joystick_control/include/joystick_control/Mode.hpp (100%) rename src/{ => Teleop-Control}/joystick_control/include/joystick_control/ScienceMode.hpp (100%) rename src/{ => Teleop-Control}/joystick_control/launch/controller.launch.py (94%) rename src/{ => Teleop-Control}/joystick_control/package.xml (100%) rename src/{ => Teleop-Control}/joystick_control/src/ArmDummyMode.cpp (100%) rename src/{ => Teleop-Control}/joystick_control/src/ArmHelpers.cpp (100%) rename src/{ => Teleop-Control}/joystick_control/src/ArmIKMode.cpp (100%) rename src/{ => Teleop-Control}/joystick_control/src/ArmManualMode.cpp (100%) rename src/{ => Teleop-Control}/joystick_control/src/DriveMode.cpp (100%) rename src/{ => Teleop-Control}/joystick_control/src/FlightstickControl.cpp (100%) rename src/{ => Teleop-Control}/joystick_control/src/ScienceMode.cpp (100%) delete mode 100755 src/camera_controller/camera_controller/cam_control.py delete mode 100644 src/camera_controller/launch/camera_controller.launch.py delete mode 100644 src/camera_controller/package.xml delete mode 100644 src/camera_controller/setup.cfg delete mode 100644 src/camera_controller/setup.py delete mode 100644 src/drive/drive/drive_station.py delete mode 100644 src/drive/drive/electrical_wrapper.py delete mode 100644 src/drive/drive/encoder_wrapper.py delete mode 100644 src/drive/drive/joystick_breakout.py delete mode 100644 src/drive/drive/joystick_controller.py delete mode 100644 src/drive/drive/joystick_drive.py delete mode 100755 src/drive/drive/roboclaw_driver.py delete mode 100755 src/drive/drive/roboclaw_node.py delete mode 100644 src/drive/drive/utils.py delete mode 100644 src/drive/launch/flightstick_drive.launch.py delete mode 100644 src/drive/launch/xbox_drive.launch.py delete mode 100644 src/drive/package.xml delete mode 100644 src/drive/setup.cfg delete mode 100644 src/drive/setup.py delete mode 100644 src/drive/test/test_copyright.py delete mode 100644 src/interfaces/msg/ArmCmd.msg delete mode 100644 src/interfaces/msg/GPIOmsg.msg delete mode 100644 src/interfaces/msg/LiveTune.msg delete mode 100644 src/interfaces/msg/PIDmsg.msg delete mode 100644 src/interfaces/msg/PointArray.msg delete mode 100644 src/interfaces/msg/SixFloats.msg delete mode 100644 src/interfaces/srv/ArmCmd.srv delete mode 100644 src/interfaces/srv/ArmPos.srv delete mode 100644 src/keyboard_publisher/CMakeLists.txt delete mode 100644 src/keyboard_publisher/package.xml delete mode 100644 src/keyboard_publisher/src/keyboard_publisher.cpp delete mode 100644 src/keyboard_publisher/src/keyboard_publisher.h delete mode 100644 src/moveit_controller/CMakeLists.txt delete mode 100644 src/moveit_controller/package.xml delete mode 100644 src/moveit_controller/src/moveit_controller.cpp delete mode 100644 src/moveit_controller/src/moveit_controller.h delete mode 100644 src/moveit_controller/src/servo_keyboard.cpp delete mode 100644 src/moveit_controller/src/servo_keyboard.h rename src/{ => nav}/cprt_costmap_plugins/CMakeLists.txt (100%) rename src/{ => nav}/cprt_costmap_plugins/costmap_plugins.xml (100%) rename src/{ => nav}/cprt_costmap_plugins/include/cprt_costmap_plugins/gridmap_layer.hpp (100%) rename src/{ => nav}/cprt_costmap_plugins/package.xml (100%) rename src/{ => nav}/cprt_costmap_plugins/src/gridmap_layer.cpp (100%) rename src/{ => nav}/cprt_gridmap_filters/CMakeLists.txt (100%) rename src/{cprt_planner_plugins => nav/cprt_gridmap_filters}/LICENSE (100%) rename src/{ => nav}/cprt_gridmap_filters/filter_plugins.xml (100%) rename src/{ => nav}/cprt_gridmap_filters/include/PreserveCostInflationFilter.hpp (100%) rename src/{ => nav}/cprt_gridmap_filters/package.xml (100%) rename src/{ => nav}/cprt_gridmap_filters/scripts/calculate_decay_rate.py (100%) rename src/{ => nav}/cprt_gridmap_filters/scripts/visualize_decay_rate.py (100%) rename src/{ => nav}/cprt_gridmap_filters/src/PreserveCostInflationFilter.cpp (100%) rename src/{ => nav}/cprt_planner_plugins/CMakeLists.txt (100%) rename src/{gpio_controller => nav/cprt_planner_plugins}/LICENSE (100%) rename src/{ => nav}/cprt_planner_plugins/global_planner_plugin.xml (100%) rename src/{ => nav}/cprt_planner_plugins/include/cprt_planner_plugins/event_horizon_planner.hpp (100%) rename src/{ => nav}/cprt_planner_plugins/package.xml (100%) rename src/{ => nav}/cprt_planner_plugins/src/event_horizon_planner.cpp (100%) rename src/{ => nav}/elevation_mapping/CMakeLists.txt (100%) rename src/{ => nav}/elevation_mapping/README.md (100%) rename src/{ => nav}/elevation_mapping/config/elevation_maps/long_range.yaml (100%) rename src/{ => nav}/elevation_mapping/config/elevation_maps/remove_object.yaml (100%) rename src/{ => nav}/elevation_mapping/config/elevation_maps/simple_demo_map.yaml (100%) rename src/{ => nav}/elevation_mapping/config/elevation_maps/starleth_map.yaml (100%) rename src/{ => nav}/elevation_mapping/config/elevation_maps/zed2i_map.yaml (100%) rename src/{ => nav}/elevation_mapping/config/file_path.yaml (100%) rename src/{ => nav}/elevation_mapping/config/postprocessing/postprocessor_pipeline.yaml (100%) rename src/{ => nav}/elevation_mapping/config/postprocessing/traversability_pipeline.yaml (100%) rename src/{ => nav}/elevation_mapping/config/robots/ground_truth_demo.yaml (100%) rename src/{ => nav}/elevation_mapping/config/robots/remove_object.yaml (100%) rename src/{ => nav}/elevation_mapping/config/robots/simple_demo_robot.yaml (100%) rename src/{ => nav}/elevation_mapping/config/robots/starleth.yaml (100%) rename src/{ => nav}/elevation_mapping/config/robots/waffle_robot.yaml (100%) rename src/{ => nav}/elevation_mapping/config/robots/zed2i_robot.yaml (100%) rename src/{ => nav}/elevation_mapping/config/rosconsole.conf (100%) rename src/{ => nav}/elevation_mapping/config/sensor_processors/RSLidar-32.yaml (100%) rename src/{ => nav}/elevation_mapping/config/sensor_processors/aslam.yaml (100%) rename src/{ => nav}/elevation_mapping/config/sensor_processors/fotonic_g45.yaml (100%) rename src/{ => nav}/elevation_mapping/config/sensor_processors/hokuyo_utm-30lx.yaml (100%) rename src/{ => nav}/elevation_mapping/config/sensor_processors/kinect_nguyen_et_al.yaml (100%) rename src/{ => nav}/elevation_mapping/config/sensor_processors/perfect.yaml (100%) rename src/{ => nav}/elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_datasheet.yaml (100%) rename src/{ => nav}/elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_uncalibrated.yaml (100%) rename src/{ => nav}/elevation_mapping/config/sensor_processors/realsense_ZR300.yaml (100%) rename src/{ => nav}/elevation_mapping/config/sensor_processors/realsense_d435.yaml (100%) rename src/{ => nav}/elevation_mapping/config/sensor_processors/velodyne_HDL-32E.yaml (100%) rename src/{ => nav}/elevation_mapping/config/sensor_processors/zed2i_processor.yaml (100%) rename src/{ => nav}/elevation_mapping/config/visualization/fused.yaml (100%) rename src/{ => nav}/elevation_mapping/config/visualization/raw.yaml (100%) rename src/{ => nav}/elevation_mapping/config/zed_configs/common_stereo.yaml (100%) rename src/{ => nav}/elevation_mapping/config/zed_configs/zed2i.yaml (100%) rename src/{ => nav}/elevation_mapping/docker/.gitignore (100%) rename src/{ => nav}/elevation_mapping/docker/Dockerfile.elevation_mapping (100%) rename src/{ => nav}/elevation_mapping/docker/Dockerfile.ros2_humble (100%) rename src/{ => nav}/elevation_mapping/docker/Dockerfile.user (100%) rename src/{ => nav}/elevation_mapping/docker/build_docker.sh (100%) rename src/{ => nav}/elevation_mapping/docker/container_scripts/.dockerignore (100%) rename src/{ => nav}/elevation_mapping/docker/container_scripts/workspace-entrypoint.sh (100%) rename src/{ => nav}/elevation_mapping/docker/middleware_profiles/rtps_udp_profile.xml (100%) rename src/{ => nav}/elevation_mapping/docker/rosdep/extra_rosdeps.yaml (100%) rename src/{ => nav}/elevation_mapping/docker/run_docker.sh (100%) rename src/{ => nav}/elevation_mapping/docker/sourced_scripts/bash_functions.sh (100%) rename src/{ => nav}/elevation_mapping/docker_instructions.md (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/ElevationMap.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/ElevationMapFunctors.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/PointXYZRGBConfidenceRatio.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/RobotMotionMapUpdater.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/ThreadSafeDataWrapper.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/elevation_mapping_node.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/input_sources/Input.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/input_sources/InputSourceManager.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingPipelineFunctor.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingWorker.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessorPool.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/sensor_processors/LaserSensorProcessor.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/sensor_processors/StereoSensorProcessor.hpp (100%) rename src/{ => nav}/elevation_mapping/include/elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp (100%) rename src/{ => nav}/elevation_mapping/launch/elevationMapping_launch.py (100%) rename src/{ => nav}/elevation_mapping/launch/rviz.launch.py (100%) rename src/{ => nav}/elevation_mapping/launch/visualization.launch.py (100%) rename src/{ => nav}/elevation_mapping/launch/zed2i.launch.py (100%) rename src/{ => nav}/elevation_mapping/package.xml (100%) rename src/{ => nav}/elevation_mapping/rviz2/custom_rviz2.rviz (100%) rename src/{ => nav}/elevation_mapping/rviz2/map.rviz (100%) rename src/{ => nav}/elevation_mapping/rviz2/viz.rviz (100%) rename src/{ => nav}/elevation_mapping/rviz2/zed2i.rviz (100%) rename src/{ => nav}/elevation_mapping/sample_data/maze.ply (100%) rename src/{ => nav}/elevation_mapping/src/ElevationMap.cpp (100%) rename src/{ => nav}/elevation_mapping/src/ElevationMapping.cpp (100%) rename src/{ => nav}/elevation_mapping/src/PointXYZRGBConfidenceRatio.cpp (100%) rename src/{ => nav}/elevation_mapping/src/RobotMotionMapUpdater.cpp (100%) rename src/{ => nav}/elevation_mapping/src/elevation_mapping_node.cpp (100%) rename src/{ => nav}/elevation_mapping/src/input_sources/Input.cpp (100%) rename src/{ => nav}/elevation_mapping/src/input_sources/InputSourceManager.cpp (100%) rename src/{ => nav}/elevation_mapping/src/postprocessing/PostprocessingPipelineFunctor.cpp (100%) rename src/{ => nav}/elevation_mapping/src/postprocessing/PostprocessingWorker.cpp (100%) rename src/{ => nav}/elevation_mapping/src/postprocessing/PostprocessorPool.cpp (100%) rename src/{ => nav}/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp (100%) rename src/{ => nav}/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp (100%) rename src/{ => nav}/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp (100%) rename src/{ => nav}/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp (100%) rename src/{ => nav}/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp (100%) rename src/{ => nav}/elevation_mapping/src/test.cpp (100%) rename src/{ => nav}/elevation_mapping/src/test_service.cpp (100%) rename src/{ => nav}/elevation_mapping/test/ElevationMapTest.cpp (100%) rename src/{ => nav}/elevation_mapping/test/WeightedEmpiricalCumulativeDistributionFunctionTest.cpp (100%) rename src/{ => nav}/elevation_mapping/test/elevation_mapping.test (100%) rename src/{ => nav}/elevation_mapping/test/input_sources/InputSourcesTest.cpp (100%) rename src/{ => nav}/elevation_mapping/test/input_sources/config/TestConfigurations.yaml (100%) rename src/{ => nav}/elevation_mapping/test/input_sources/config/rosconsole.config (100%) rename src/{ => nav}/elevation_mapping/test/input_sources/input_sources.test (100%) rename src/{ => nav}/elevation_mapping/test/input_sources/test_input_sources.cpp (100%) rename src/{ => nav}/elevation_mapping/test/postprocessing/PostprocessorTest.cpp (100%) rename src/{ => nav}/elevation_mapping/test/postprocessing/main.cpp (100%) rename src/{ => nav}/elevation_mapping/test/postprocessing/postprocessor.test (100%) rename src/{ => nav}/elevation_mapping/test/postprocessing/postprocessor_pipeline.yaml (100%) rename src/{ => nav}/elevation_mapping/test/test_elevation_mapping.cpp (100%) rename src/{ => nav}/gps/config/Heading_config.ubx (100%) rename src/{ => nav}/gps/config/Rover_config.ubx (100%) rename src/{ => nav}/gps/config/gps.yaml (100%) rename src/{ => nav}/gps/gps/__init__.py (100%) rename src/{ => nav}/gps/gps/heading_pub_node.py (100%) rename src/{ => nav}/gps/gps/rtcm_pub_node.py (100%) rename src/{ => nav}/gps/gps/ubx_io_manager.py (100%) rename src/{ => nav}/gps/launch/basestation.launch.py (100%) rename src/{ => nav}/gps/launch/heading.launch.py (100%) rename src/{ => nav}/gps/launch/rover.launch.py (100%) rename src/{ => nav}/gps/package.xml (100%) rename src/{ => nav}/gps/resource/gps (100%) rename src/{ => nav}/gps/setup.cfg (100%) rename src/{ => nav}/gps/setup.py (100%) rename src/{ => nav}/gps/test/test_copyright.py (100%) rename src/{gpio_controller => nav/gps}/test/test_flake8.py (100%) rename src/{gpio_controller => nav/gps}/test/test_pep257.py (100%) create mode 100644 src/nav/kindr_ros/.gitignore create mode 100644 src/nav/kindr_ros/LICENSE create mode 100644 src/nav/kindr_ros/README.md create mode 100644 src/nav/kindr_ros/kindr_msgs/CMakeLists.txt create mode 100644 src/nav/kindr_ros/kindr_msgs/msg/VectorAtPosition.msg create mode 100644 src/nav/kindr_ros/kindr_msgs/package.xml create mode 100644 src/nav/kindr_ros/kindr_ros/CMakeLists.txt create mode 100644 src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPhysicalQuantities.hpp create mode 100644 src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPose.hpp create mode 100644 src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgRotation.hpp create mode 100644 src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgTwist.hpp create mode 100644 src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosTfPose.hpp create mode 100644 src/nav/kindr_ros/kindr_ros/include/kindr_ros/kindr_ros.hpp create mode 100644 src/nav/kindr_ros/kindr_ros/package.xml create mode 100644 src/nav/kindr_ros/kindr_ros/test/CMakeLists.txt create mode 100644 src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgPhysicalQuantitiesTest.cpp create mode 100644 src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgPoseTest.cpp create mode 100644 src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgRotationTest.cpp create mode 100644 src/nav/kindr_ros/kindr_ros/test/RosTfPoseTest.cpp create mode 100644 src/nav/kindr_ros/kindr_ros/test/TfConventionTest.cpp create mode 100644 src/nav/kindr_ros/kindr_ros/test/test_main.cpp create mode 100644 src/nav/kindr_ros/kindr_rviz_plugins/CMakeLists.txt rename src/{camera_controller/resource/camera_controller => nav/kindr_ros/kindr_rviz_plugins/COLCON_IGNORE} (100%) create mode 100644 src/nav/kindr_ros/kindr_rviz_plugins/icons/classes/VectorAtPosition.png create mode 100644 src/nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp create mode 100644 src/nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionVisual.hpp create mode 100644 src/nav/kindr_ros/kindr_rviz_plugins/package.xml create mode 100644 src/nav/kindr_ros/kindr_rviz_plugins/plugin_description.xml create mode 100644 src/nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp create mode 100644 src/nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp create mode 100644 src/nav/kindr_ros/kindr_rviz_plugins/test/EmptyTests.cpp create mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/CMakeLists.txt rename src/{computer_vision/resource/computer_vision => nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/COLCON_IGNORE} (100%) create mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/icons/classes/MultiDOFJointTrajectory.png create mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp create mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp create mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp create mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/package.xml create mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/plugin_description.xml create mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp create mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp create mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp create mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/test/EmptyTests.cpp rename src/{ => nav}/localization/config/driver_params.yaml (100%) rename src/{ => nav}/localization/config/ekf/gps_ekf_global.yaml (100%) rename src/{ => nav}/localization/config/ekf/gps_ekf_local.yaml (100%) rename src/{ => nav}/localization/config/navsat.yaml (100%) rename src/{ => nav}/localization/config/rtabmap.yaml (100%) rename src/{ => nav}/localization/config/rviz.views/basicNavMap.rviz (100%) rename src/{ => nav}/localization/config/slam.yaml (100%) rename src/{ => nav}/localization/config/urdf/spike.urdf.xml (100%) rename src/{ => nav}/localization/launch/description.launch.py (100%) rename src/{ => nav}/localization/launch/ekf.launch.py (100%) rename src/{ => nav}/localization/launch/localization.launch.py (100%) rename src/{ => nav}/localization/launch/navsat.launch.py (100%) rename src/{ => nav}/localization/launch/rtabmap.launch.py (100%) rename src/{ => nav}/localization/localization/__init__.py (100%) rename src/{ => nav}/localization/localization/imu_filter.py (100%) rename src/{ => nav}/localization/localization/lidar_mask_tool.py (100%) rename src/{ => nav}/localization/localization/republish_odometry.py (100%) rename src/{ => nav}/localization/package.xml (100%) rename src/{ => nav}/localization/resource/localization (100%) rename src/{ => nav}/localization/setup.cfg (100%) rename src/{ => nav}/localization/setup.py (100%) rename src/{ => nav}/nav_commanders/known_gps_coords/old/drumheller_traversal_guess.yaml (100%) rename src/{ => nav}/nav_commanders/known_gps_coords/old/edc_crosswalk_to_res.yaml (100%) rename src/{ => nav}/nav_commanders/known_gps_coords/old/edcfront_northeast.yaml (100%) rename src/{ => nav}/nav_commanders/known_gps_coords/old/edcfront_ramp_to_edcparkinglot.yaml (100%) rename src/{ => nav}/nav_commanders/known_gps_coords/old/edcfront_southwest.yaml (100%) rename src/{ => nav}/nav_commanders/known_gps_coords/old/edcfront_trees_grass.yaml (100%) rename src/{ => nav}/nav_commanders/known_gps_coords/old/useful_locations.md (100%) rename src/{ => nav}/nav_commanders/known_gps_coords/send_gps_command.py (100%) rename src/{ => nav}/nav_commanders/launch/aruco_gps_commander.py (100%) rename src/{ => nav}/nav_commanders/launch/gps_commander.launch.py (100%) rename src/{ => nav}/nav_commanders/launch/incremental_gps_commander.launch.py (100%) rename src/{ => nav}/nav_commanders/nav_commanders/__init__.py (100%) rename src/{ => nav}/nav_commanders/nav_commanders/gps_utils.py (100%) rename src/{ => nav}/nav_commanders/nav_commanders/incremental_gps_commander.py (100%) rename src/{ => nav}/nav_commanders/nav_commanders/nav_to_gps_aruco.py (100%) rename src/{ => nav}/nav_commanders/nav_commanders/nav_to_gps_coords.py (100%) rename src/{ => nav}/nav_commanders/package.xml (100%) rename src/{ => nav}/nav_commanders/resource/nav_commanders (100%) rename src/{ => nav}/nav_commanders/setup.cfg (100%) rename src/{ => nav}/nav_commanders/setup.py (100%) rename src/{ => nav}/nav_commanders/test/test_copyright.py (100%) rename src/{gps => nav/nav_commanders}/test/test_flake8.py (100%) rename src/{gps => nav/nav_commanders}/test/test_pep257.py (100%) rename src/{ => nav}/navigation/CMakeLists.txt (100%) rename src/{ => nav}/navigation/behavior_trees/behaviorTree.xml (100%) rename src/{ => nav}/navigation/behavior_trees/behaviorTree_NavPathingTestTree.xml (100%) rename src/{ => nav}/navigation/behavior_trees/behaviorTree_original.xml (100%) rename src/{ => nav}/navigation/behavior_trees/bt_dynamic_replanning.xml (100%) rename src/{ => nav}/navigation/behavior_trees/spike_bt.xml (100%) rename src/{ => nav}/navigation/config/elevation_mapping/ouster_OS0_32U_sensor_processor.yaml (100%) rename src/{ => nav}/navigation/config/elevation_mapping/ouster_elevation_mapping.yaml (100%) rename src/{ => nav}/navigation/config/elevation_mapping/postprocessing_traversability.yaml (100%) rename src/{ => nav}/navigation/config/elevation_mapping/spike_robot.yaml (100%) rename src/{ => nav}/navigation/config/elevation_mapping/zed2i_elevation_mapping.yaml (100%) rename src/{ => nav}/navigation/config/elevation_mapping/zed2i_sensor_processor.yaml (100%) rename src/{ => nav}/navigation/config/ouster/driver_params.yaml (100%) rename src/{ => nav}/navigation/config/zed/zed_override_params.yaml (100%) rename src/{ => nav}/navigation/launch/bringup.launch.py (100%) rename src/{ => nav}/navigation/launch/nav2.launch.py (100%) rename src/{ => nav}/navigation/launch/navigation.launch.py (100%) rename src/{ => nav}/navigation/launch/ouster.launch.py (100%) rename src/{ => nav}/navigation/launch/ouster_composable.launch.py (100%) rename src/{ => nav}/navigation/launch/traversability_gridmap.launch.py (100%) rename src/{ => nav}/navigation/launch/zed.launch.py (100%) rename src/{ => nav}/navigation/lidar_masks/ouster_mask_combined_shifted.png (100%) rename src/{ => nav}/navigation/lidar_masks/ouster_mask_front.png (100%) rename src/{ => nav}/navigation/lidar_masks/ouster_mask_left.png (100%) rename src/{ => nav}/navigation/lidar_masks/ouster_mask_rear.png (100%) rename src/{ => nav}/navigation/lidar_masks/ouster_mask_right.png (100%) rename src/{ => nav}/navigation/package.xml (100%) rename src/{ => nav}/navigation/params/nav2.yaml (100%) rename src/{ => nav}/navigation/rviz/nav2_cprt.rviz (100%) rename src/{ => nav}/navigation/rviz/nav2_offical_default_config.rviz (100%) rename src/{ => nav}/navigation/rviz/traversibility_gridmap.rviz (100%) delete mode 160000 src/ouster-ros delete mode 100644 src/science_sensors/science_sensors/stellarnet_driverLibs/__init__.py delete mode 100644 src/science_sensors/test/test_flake8.py delete mode 100644 src/science_sensors/test/test_pep257.py delete mode 100644 src/servo_pkg/servo_pkg/__init__.py delete mode 100644 src/servo_pkg/test/test_copyright.py delete mode 100644 src/servo_pkg/test/test_flake8.py delete mode 100644 src/servo_pkg/test/test_pep257.py rename src/{ => telemetry}/system-telemetry-cpp/CMakeLists.txt (100%) rename src/{ => telemetry}/system-telemetry-cpp/LICENSE (100%) rename src/{ => telemetry}/system-telemetry-cpp/include/system-telemetry-cpp/cpu_collector.hpp (100%) rename src/{ => telemetry}/system-telemetry-cpp/include/system-telemetry-cpp/gpu_collector.hpp (100%) rename src/{ => telemetry}/system-telemetry-cpp/include/system-telemetry-cpp/memory_collector.hpp (100%) rename src/{ => telemetry}/system-telemetry-cpp/include/system-telemetry-cpp/system_telemetry_publisher.hpp (100%) rename src/{ => telemetry}/system-telemetry-cpp/include/system-telemetry-cpp/telemetry_collector.hpp (100%) rename src/{ => telemetry}/system-telemetry-cpp/package.xml (100%) rename src/{ => telemetry}/system-telemetry-cpp/src/cpu_collector.cpp (100%) rename src/{ => telemetry}/system-telemetry-cpp/src/gpu_collector.cpp (100%) rename src/{ => telemetry}/system-telemetry-cpp/src/memory_collector.cpp (100%) rename src/{ => telemetry}/system-telemetry-cpp/src/system_telemetry_publisher.cpp (100%) delete mode 160000 src/ublox rename src/{ => video}/camera_streaming/CMakeLists.txt (100%) rename src/{ => video}/camera_streaming/config/webrtc.yaml (100%) rename src/{ => video}/camera_streaming/gst-plugins/CMakeLists.txt (100%) rename src/{ => video}/camera_streaming/gst-plugins/gstarucomarker.cpp (100%) rename src/{ => video}/camera_streaming/gst-plugins/gstarucomarker.hpp (100%) rename src/{ => video}/camera_streaming/hooks/gst_env_hook.sh (100%) rename src/{ => video}/camera_streaming/include/camera_streaming/webrtc_node.h (100%) rename src/{ => video}/camera_streaming/launch/webRTC.launch.py (100%) rename src/{ => video}/camera_streaming/package.xml (100%) rename src/{ => video}/camera_streaming/src/webrtc_node.cpp (100%) rename src/{ => video}/camera_streaming_qt/.gitignore (100%) rename src/{ => video}/camera_streaming_qt/CMakeLists.txt (100%) rename src/{ => video}/camera_streaming_qt/include/camera_streaming_qt/camera_client.h (100%) rename src/{ => video}/camera_streaming_qt/include/camera_streaming_qt/mainwindow.h (100%) rename src/{ => video}/camera_streaming_qt/include/camera_streaming_qt/tests/test_camera_node.h (100%) rename src/{ => video}/camera_streaming_qt/include/camera_streaming_qt/widgets/capture_image_widget.h (100%) rename src/{ => video}/camera_streaming_qt/include/camera_streaming_qt/widgets/main_widget.h (100%) rename src/{ => video}/camera_streaming_qt/include/camera_streaming_qt/widgets/preset_widget.h (100%) rename src/{ => video}/camera_streaming_qt/include/camera_streaming_qt/widgets/source_widget.h (100%) rename src/{ => video}/camera_streaming_qt/package.xml (100%) rename src/{ => video}/camera_streaming_qt/src/camera_client.cpp (100%) rename src/{ => video}/camera_streaming_qt/src/mainwindow.cpp (100%) rename src/{ => video}/camera_streaming_qt/src/tests/test_camera_node.cpp (100%) rename src/{ => video}/camera_streaming_qt/src/widgets/capture_image_widget.cpp (100%) rename src/{ => video}/camera_streaming_qt/src/widgets/main_widget.cpp (100%) rename src/{ => video}/camera_streaming_qt/src/widgets/preset_widget.cpp (100%) rename src/{ => video}/camera_streaming_qt/src/widgets/source_widget.cpp (100%) rename src/{science_sensors/science_sensors => video/computer_vision/computer_vision}/__init__.py (100%) rename src/{ => video}/computer_vision/computer_vision/zed_aruco_detector.py (100%) rename src/{ => video}/computer_vision/launch/aruco_detector_zed.launch.py (100%) rename src/{ => video}/computer_vision/package.xml (100%) rename src/{drive/resource/drive => video/computer_vision/resource/computer_vision} (100%) rename src/{ => video}/computer_vision/setup.cfg (100%) rename src/{ => video}/computer_vision/setup.py (100%) rename src/{science_sensors => video/computer_vision}/test/test_copyright.py (100%) rename src/{nav_commanders => video/computer_vision}/test/test_flake8.py (100%) rename src/{nav_commanders => video/computer_vision}/test/test_pep257.py (100%) delete mode 160000 src/zed-ros2-wrapper diff --git a/.github/workflows/build-docker.yml b/.github/workflows/build-docker.yml index 7a94a5d8..9ba6b8f0 100644 --- a/.github/workflows/build-docker.yml +++ b/.github/workflows/build-docker.yml @@ -141,40 +141,4 @@ jobs: ${{ 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 - - push-manifest: - runs-on: ubuntu-latest - needs: [build-amd64, build-arm64, setup] - env: - IMAGE_NAME: ${{ needs.setup.outputs.IMAGE_NAME }} - steps: - - name: Docker login - uses: docker/login-action@v3 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - # Create multi-arch manifest for APP image (latest + commit) - - name: Create and push APP manifest - run: | - APP_AMD64_DIGEST="${{ needs.build-amd64.outputs.app_digest }}" - APP_ARM64_DIGEST="${{ needs.build-arm64.outputs.app_digest }}" - - docker buildx imagetools create \ - -t "$IMAGE_NAME:latest" \ - -t "$IMAGE_NAME:${{ github.sha }}" \ - "$IMAGE_NAME@${APP_AMD64_DIGEST}" \ - "$IMAGE_NAME@${APP_ARM64_DIGEST}" - - # Create multi-arch manifest for DEV image (dev + dev-${sha}) - - name: Create and push DEV manifest - run: | - DEV_AMD64_DIGEST="${{ needs.build-amd64.outputs.dev_digest }}" - DEV_ARM64_DIGEST="${{ needs.build-arm64.outputs.dev_digest }}" - - docker buildx imagetools create \ - -t "$IMAGE_NAME:dev" \ - -t "$IMAGE_NAME:dev-${{ github.sha }}" \ - "$IMAGE_NAME@${DEV_AMD64_DIGEST}" \ - "$IMAGE_NAME@${DEV_ARM64_DIGEST}" + provenance: false \ No newline at end of file diff --git a/build.sh b/build.sh index 049fa69e..918747e7 100755 --- a/build.sh +++ b/build.sh @@ -26,6 +26,4 @@ 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) \ - --build-base build-$aarch \ - --install-base install-$aarch \ No newline at end of file +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/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 fc799d65..00000000 --- a/setup/science/setup_science.sh +++ /dev/null @@ -1,79 +0,0 @@ -#!/bin/bash -SCRIPT_DIR="$(dirname "$0")" -CURRENT_DIR="$(pwd)" -CURRENT_USER=$(whoami) -VERSION="science-1.0.0" -AARCH=$(uname -m) -if [ "$AARCH" = "aarch64" ]; then - echo "Detected ARM architecture." - AARCH="arm64" -else - echo "Detected AMD64 architecture." - AARCH="amd64" -fi -echo "Using architecture: $AARCH" - -# 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-science:$AARCH - -# 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 75359c6a..00000000 --- a/setup/science/start_science.sh +++ /dev/null @@ -1,42 +0,0 @@ -#!/bin/bash - -AARCH=$(uname -m) -if [ "$AARCH" = "aarch64" ]; then - echo "Detected ARM architecture." - AARCH="arm64" -else - echo "Detected AMD64 architecture." - AARCH="amd64" -fi -echo "Using architecture: $AARCH" - -IMAGE=cprtsoftware/cprt_rover_24-science -TAG=$AARCH -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/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 similarity index 100% rename from src/bringup/launch/arm.launch.py rename to src/Bringup/launch/arm.launch.py 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 similarity index 100% rename from src/bringup/launch/core.launch.py rename to src/Bringup/launch/core.launch.py diff --git a/src/bringup/launch/drive.launch.py b/src/Bringup/launch/drive.launch.py similarity index 94% rename from src/bringup/launch/drive.launch.py rename to src/Bringup/launch/drive.launch.py index 9b9050eb..4c4b1191 100644 --- a/src/bringup/launch/drive.launch.py +++ b/src/Bringup/launch/drive.launch.py @@ -18,6 +18,6 @@ def get_included_launch_descriptions(launch_files): def generate_launch_description(): launch_files = [ - ("drive", "talon.launch.py"), + ("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 similarity index 100% rename from src/bringup/launch/localization.launch.py rename to src/Bringup/launch/localization.launch.py diff --git a/src/bringup/launch/navigation.launch.py b/src/Bringup/launch/navigation.launch.py similarity index 100% rename from src/bringup/launch/navigation.launch.py rename to src/Bringup/launch/navigation.launch.py 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 similarity index 100% rename from src/bringup/launch/video_streaming.launch.py rename to src/Bringup/launch/video_streaming.launch.py 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/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/camera_controller/camera_controller/__init__.py b/src/HW-Devices/gpio_controller/gpio_controller/__init__.py similarity index 100% rename from src/camera_controller/camera_controller/__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 100% rename from src/gpio_controller/package.xml rename to src/HW-Devices/gpio_controller/package.xml 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/camera_controller/test/test_copyright.py b/src/HW-Devices/gpio_controller/test/test_copyright.py similarity index 100% rename from src/camera_controller/test/test_copyright.py rename to src/HW-Devices/gpio_controller/test/test_copyright.py diff --git a/src/camera_controller/test/test_flake8.py b/src/HW-Devices/gpio_controller/test/test_flake8.py similarity index 100% rename from src/camera_controller/test/test_flake8.py rename to src/HW-Devices/gpio_controller/test/test_flake8.py diff --git a/src/camera_controller/test/test_pep257.py b/src/HW-Devices/gpio_controller/test/test_pep257.py similarity index 100% rename from src/camera_controller/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 100% rename from src/hardware/package.xml rename to src/HW-Devices/hardware/package.xml 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/computer_vision/computer_vision/__init__.py b/src/HW-Devices/science_sensors/science_sensors/__init__.py similarity index 100% rename from src/computer_vision/computer_vision/__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/drive/drive/__init__.py b/src/HW-Devices/science_sensors/science_sensors/stellarnet_driverLibs/__init__.py similarity index 100% rename from src/drive/drive/__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/computer_vision/test/test_copyright.py b/src/HW-Devices/science_sensors/test/test_copyright.py similarity index 100% rename from src/computer_vision/test/test_copyright.py rename to src/HW-Devices/science_sensors/test/test_copyright.py diff --git a/src/computer_vision/test/test_flake8.py b/src/HW-Devices/science_sensors/test/test_flake8.py similarity index 100% rename from src/computer_vision/test/test_flake8.py rename to src/HW-Devices/science_sensors/test/test_flake8.py diff --git a/src/computer_vision/test/test_pep257.py b/src/HW-Devices/science_sensors/test/test_pep257.py similarity index 100% rename from src/computer_vision/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/gpio_controller/gpio_controller/__init__.py b/src/HW-Devices/servo_pkg/servo_pkg/__init__.py similarity index 100% rename from src/gpio_controller/gpio_controller/__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/gpio_controller/test/test_copyright.py b/src/HW-Devices/servo_pkg/test/test_copyright.py similarity index 100% rename from src/gpio_controller/test/test_copyright.py rename to src/HW-Devices/servo_pkg/test/test_copyright.py diff --git a/src/drive/test/test_flake8.py b/src/HW-Devices/servo_pkg/test/test_flake8.py similarity index 100% rename from src/drive/test/test_flake8.py rename to src/HW-Devices/servo_pkg/test/test_flake8.py diff --git a/src/drive/test/test_pep257.py b/src/HW-Devices/servo_pkg/test/test_pep257.py similarity index 100% rename from src/drive/test/test_pep257.py rename to src/HW-Devices/servo_pkg/test/test_pep257.py 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 93% rename from src/drive/launch/talon.launch.py rename to src/Teleop-Control/drive_cpp/launch/talon.launch.py index 96b79769..fe67cb0f 100644 --- a/src/drive/launch/talon.launch.py +++ b/src/Teleop-Control/drive_cpp/launch/talon.launch.py @@ -12,10 +12,10 @@ def generate_launch_description(): """Generate launch description with multiple components.""" 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/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/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/gps/gps/__init__.py b/src/nav/gps/gps/__init__.py similarity index 100% rename from src/gps/gps/__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/gps/test/test_copyright.py b/src/nav/gps/test/test_copyright.py similarity index 100% rename from src/gps/test/test_copyright.py rename to src/nav/gps/test/test_copyright.py diff --git a/src/gpio_controller/test/test_flake8.py b/src/nav/gps/test/test_flake8.py similarity index 100% rename from src/gpio_controller/test/test_flake8.py rename to src/nav/gps/test/test_flake8.py diff --git a/src/gpio_controller/test/test_pep257.py b/src/nav/gps/test/test_pep257.py similarity index 100% rename from src/gpio_controller/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..4f4ada37 --- /dev/null +++ b/src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPhysicalQuantities.hpp @@ -0,0 +1,108 @@ +/* + * 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..0e64d3f0 --- /dev/null +++ b/src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPose.hpp @@ -0,0 +1,109 @@ +/* + * 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..0ac0ddee --- /dev/null +++ b/src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgRotation.hpp @@ -0,0 +1,68 @@ +/* + * 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..a7b76612 --- /dev/null +++ b/src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgTwist.hpp @@ -0,0 +1,67 @@ +/* + * 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..fd837e1c --- /dev/null +++ b/src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosTfPose.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__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 & 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..d5789383 --- /dev/null +++ b/src/nav/kindr_ros/kindr_ros/include/kindr_ros/kindr_ros.hpp @@ -0,0 +1,44 @@ +/* + * 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/RosGeometryMsgTwist.hpp" +#include "kindr_ros/RosGeometryMsgRotation.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..259a904f --- /dev/null +++ b/src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgPhysicalQuantitiesTest.cpp @@ -0,0 +1,132 @@ +/* + * 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..8fdb9d7a --- /dev/null +++ b/src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgPoseTest.cpp @@ -0,0 +1,129 @@ +/* + * 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..523844b0 --- /dev/null +++ b/src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgRotationTest.cpp @@ -0,0 +1,68 @@ +/* + * 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..c12de0bb --- /dev/null +++ b/src/nav/kindr_ros/kindr_ros/test/RosTfPoseTest.cpp @@ -0,0 +1,318 @@ +/* + * 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..c69002c8 --- /dev/null +++ b/src/nav/kindr_ros/kindr_ros/test/TfConventionTest.cpp @@ -0,0 +1,155 @@ +/* + * 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..df19e2a2 --- /dev/null +++ b/src/nav/kindr_ros/kindr_ros/test/test_main.cpp @@ -0,0 +1,36 @@ +/* + * 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 0000000000000000000000000000000000000000..4c78906557c3b519ac3664c5d65da4241b3c4942 GIT binary patch literal 585 zcmV-P0=E5$P)K}6tDZGQK|Of#pn`vlhbri$g0x5f zf!1pADupT$qB(@-B9nAYx<6+;WJ!1F>cNF!2j2Jd-uJzE@E>1ixih)TY+&ci4!!HW zf5PkM)={Y91P9dv0J>MYImp6vdS;v7SMIvfoy451@X#LON|(mz2CbzQ^>Ur+c5NE? zk`}7~Cucu40Nx#QFieIFq5(k?Ff>D4=_244a0ysRWg}p#u!X(R9{sS7Kk^wRBOK|V zZ)kvd;63mV7ywCO0`Ip+XhM`!=vd>09@VYd1>gg4JN@^8May`SxfulIfY`=ZWzm6V z=2JZjZwfFLmH|(sE^-d^zQ(sc7R&wCgJUqjoAa;xKbG3XEo`tsWFn@vPZh!dj&xA+ zFM9xRbN5yb?p@qB*|@Y4ECmTl_JoDYS@_%4uV#+9TmdCi(I(nEgl6!9N1uI4_khip zXID*3oP(P*$Gag#Hi314HIStzyi)UYcRSa9nu@3(5O`DGL|mhWM2_3YIEaDAKnV7^ zW~LXJh>|Q}Fo@)+nnvS@+{+_DSl}(N2x|2y&yun%(+=81CZb|06Y)Z00VyR)9L?Yl XW4g2`&`2|!00000NkvXXu0mjf3$OtE literal 0 HcmV?d00001 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..a39796e5 --- /dev/null +++ b/src/nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp @@ -0,0 +1,109 @@ +#pragma once + +#ifndef Q_MOC_RUN +// boost +#include + +// rviz +#include + +// kindr ros +#include + +// ogre +#include +#endif + + +namespace Ogre { +class SceneNode; +} // Ogre + +namespace rviz { +class ColorProperty; +class FloatProperty; +class IntProperty; +} // 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_; +}; + +} // 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..59018dbf --- /dev/null +++ b/src/nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionVisual.hpp @@ -0,0 +1,111 @@ +#pragma once + +// rviz +#include + +// kindr ros +#include + + +namespace Ogre { +class Vector3; +class Quaternion; +} // Ogre + +namespace rviz { +class Arrow; +class BillboardLine; +class MovableText; +} // 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(); +}; + +} // 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..762de031 --- /dev/null +++ b/src/nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp @@ -0,0 +1,224 @@ +// 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(); +} + +} // 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..132c18bf --- /dev/null +++ b/src/nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp @@ -0,0 +1,194 @@ +/* + * 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 +#include +#include + +#include +#include +#include + +#include "kindr_rviz_plugins/VectorAtPositionVisual.hpp" + + +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()); +} + +} // 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/computer_vision/resource/computer_vision b/src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/COLCON_IGNORE similarity index 100% rename from src/computer_vision/resource/computer_vision 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 0000000000000000000000000000000000000000..4e415e0fd4a84ccdec52f63f547cb9edb01c71eb GIT binary patch literal 273 zcmeAS@N?(olHy`uVBq!ia0vp^0wB!61|;P_|4#%`Y)RhkE)4%caKYZ?lYt_f1s;*b z3=DinK$vl=HlH+5kiEpy*OmP~vmh52EBor}i-AJ(JY5_^EKXmYw3nC3kf(LO%Zn8Y z(gn4`5+?RIoaTGuc-G)c$V$cXekC#cos&IQ9GI%S|M~fCO!mUw6CP-FOmla9Gp|x; z&3-nHGTDrfhywx6!Z}m6u1=e!Fym*4^}A*@4ztJ=ru#3l)K0!BZen`Leam#wZNc** zKYm+!;NU^M`Vg}nESw)c2mS91+kNa_pT&+lZ`59_{pecoEm;0l{bRoA3lm@R=R~~# Px|G4w)z4*}Q$iB}6|iW6 literal 0 HcmV?d00001 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..ffc20665 --- /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,159 @@ +#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 +{ +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>> 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_; +}; + +} // 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..7e49ebd1 --- /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,47 @@ +#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_; +}; + +} // 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..d6c7aa5d --- /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,135 @@ +#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_; +}; + +} // 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..41b22f54 --- /dev/null +++ b/src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp @@ -0,0 +1,652 @@ +#include + +#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp" + + +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_); + } + } +} + +} // 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..e85cde41 --- /dev/null +++ b/src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp @@ -0,0 +1,69 @@ +#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_); + } +} + +} // 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..2d610596 --- /dev/null +++ b/src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp @@ -0,0 +1,438 @@ +#include + +#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp" + + +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; + } +} + +} // 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/navsat.launch.py b/src/nav/localization/launch/navsat.launch.py similarity index 100% rename from src/localization/launch/navsat.launch.py rename to src/nav/localization/launch/navsat.launch.py 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/localization/localization/__init__.py b/src/nav/localization/localization/__init__.py similarity index 100% rename from src/localization/localization/__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/nav_commanders/nav_commanders/__init__.py b/src/nav/nav_commanders/nav_commanders/__init__.py similarity index 100% rename from src/nav_commanders/nav_commanders/__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/nav_commanders/test/test_copyright.py b/src/nav/nav_commanders/test/test_copyright.py similarity index 100% rename from src/nav_commanders/test/test_copyright.py rename to src/nav/nav_commanders/test/test_copyright.py diff --git a/src/gps/test/test_flake8.py b/src/nav/nav_commanders/test/test_flake8.py similarity index 100% rename from src/gps/test/test_flake8.py rename to src/nav/nav_commanders/test/test_flake8.py diff --git a/src/gps/test/test_pep257.py b/src/nav/nav_commanders/test/test_pep257.py similarity index 100% rename from src/gps/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/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/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/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/camera_streaming/CMakeLists.txt b/src/video/camera_streaming/CMakeLists.txt similarity index 100% rename from src/camera_streaming/CMakeLists.txt rename to src/video/camera_streaming/CMakeLists.txt diff --git a/src/camera_streaming/config/webrtc.yaml b/src/video/camera_streaming/config/webrtc.yaml similarity index 100% rename from src/camera_streaming/config/webrtc.yaml rename to src/video/camera_streaming/config/webrtc.yaml diff --git a/src/camera_streaming/gst-plugins/CMakeLists.txt b/src/video/camera_streaming/gst-plugins/CMakeLists.txt similarity index 100% rename from src/camera_streaming/gst-plugins/CMakeLists.txt rename to src/video/camera_streaming/gst-plugins/CMakeLists.txt diff --git a/src/camera_streaming/gst-plugins/gstarucomarker.cpp b/src/video/camera_streaming/gst-plugins/gstarucomarker.cpp similarity index 100% rename from src/camera_streaming/gst-plugins/gstarucomarker.cpp rename to src/video/camera_streaming/gst-plugins/gstarucomarker.cpp diff --git a/src/camera_streaming/gst-plugins/gstarucomarker.hpp b/src/video/camera_streaming/gst-plugins/gstarucomarker.hpp similarity index 100% rename from src/camera_streaming/gst-plugins/gstarucomarker.hpp rename to src/video/camera_streaming/gst-plugins/gstarucomarker.hpp diff --git a/src/camera_streaming/hooks/gst_env_hook.sh b/src/video/camera_streaming/hooks/gst_env_hook.sh similarity index 100% rename from src/camera_streaming/hooks/gst_env_hook.sh rename to src/video/camera_streaming/hooks/gst_env_hook.sh diff --git a/src/camera_streaming/include/camera_streaming/webrtc_node.h b/src/video/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/video/camera_streaming/include/camera_streaming/webrtc_node.h diff --git a/src/camera_streaming/launch/webRTC.launch.py b/src/video/camera_streaming/launch/webRTC.launch.py similarity index 100% rename from src/camera_streaming/launch/webRTC.launch.py rename to src/video/camera_streaming/launch/webRTC.launch.py diff --git a/src/camera_streaming/package.xml b/src/video/camera_streaming/package.xml similarity index 100% rename from src/camera_streaming/package.xml rename to src/video/camera_streaming/package.xml diff --git a/src/camera_streaming/src/webrtc_node.cpp b/src/video/camera_streaming/src/webrtc_node.cpp similarity index 100% rename from src/camera_streaming/src/webrtc_node.cpp rename to src/video/camera_streaming/src/webrtc_node.cpp diff --git a/src/camera_streaming_qt/.gitignore b/src/video/camera_streaming_qt/.gitignore similarity index 100% rename from src/camera_streaming_qt/.gitignore rename to src/video/camera_streaming_qt/.gitignore diff --git a/src/camera_streaming_qt/CMakeLists.txt b/src/video/camera_streaming_qt/CMakeLists.txt similarity index 100% rename from src/camera_streaming_qt/CMakeLists.txt rename to src/video/camera_streaming_qt/CMakeLists.txt diff --git a/src/camera_streaming_qt/include/camera_streaming_qt/camera_client.h b/src/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/camera_streaming_qt/include/camera_streaming_qt/widgets/source_widget.h diff --git a/src/camera_streaming_qt/package.xml b/src/video/camera_streaming_qt/package.xml similarity index 100% rename from src/camera_streaming_qt/package.xml rename to src/video/camera_streaming_qt/package.xml diff --git a/src/camera_streaming_qt/src/camera_client.cpp b/src/video/camera_streaming_qt/src/camera_client.cpp similarity index 100% rename from src/camera_streaming_qt/src/camera_client.cpp rename to src/video/camera_streaming_qt/src/camera_client.cpp diff --git a/src/camera_streaming_qt/src/mainwindow.cpp b/src/video/camera_streaming_qt/src/mainwindow.cpp similarity index 100% rename from src/camera_streaming_qt/src/mainwindow.cpp rename to src/video/camera_streaming_qt/src/mainwindow.cpp diff --git a/src/camera_streaming_qt/src/tests/test_camera_node.cpp b/src/video/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/video/camera_streaming_qt/src/tests/test_camera_node.cpp diff --git a/src/camera_streaming_qt/src/widgets/capture_image_widget.cpp b/src/video/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/video/camera_streaming_qt/src/widgets/capture_image_widget.cpp diff --git a/src/camera_streaming_qt/src/widgets/main_widget.cpp b/src/video/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/video/camera_streaming_qt/src/widgets/main_widget.cpp diff --git a/src/camera_streaming_qt/src/widgets/preset_widget.cpp b/src/video/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/video/camera_streaming_qt/src/widgets/preset_widget.cpp diff --git a/src/camera_streaming_qt/src/widgets/source_widget.cpp b/src/video/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/video/camera_streaming_qt/src/widgets/source_widget.cpp diff --git a/src/science_sensors/science_sensors/__init__.py b/src/video/computer_vision/computer_vision/__init__.py similarity index 100% rename from src/science_sensors/science_sensors/__init__.py rename to src/video/computer_vision/computer_vision/__init__.py diff --git a/src/computer_vision/computer_vision/zed_aruco_detector.py b/src/video/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/video/computer_vision/computer_vision/zed_aruco_detector.py diff --git a/src/computer_vision/launch/aruco_detector_zed.launch.py b/src/video/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/video/computer_vision/launch/aruco_detector_zed.launch.py diff --git a/src/computer_vision/package.xml b/src/video/computer_vision/package.xml similarity index 100% rename from src/computer_vision/package.xml rename to src/video/computer_vision/package.xml diff --git a/src/drive/resource/drive b/src/video/computer_vision/resource/computer_vision similarity index 100% rename from src/drive/resource/drive rename to src/video/computer_vision/resource/computer_vision diff --git a/src/computer_vision/setup.cfg b/src/video/computer_vision/setup.cfg similarity index 100% rename from src/computer_vision/setup.cfg rename to src/video/computer_vision/setup.cfg diff --git a/src/computer_vision/setup.py b/src/video/computer_vision/setup.py similarity index 100% rename from src/computer_vision/setup.py rename to src/video/computer_vision/setup.py diff --git a/src/science_sensors/test/test_copyright.py b/src/video/computer_vision/test/test_copyright.py similarity index 100% rename from src/science_sensors/test/test_copyright.py rename to src/video/computer_vision/test/test_copyright.py diff --git a/src/nav_commanders/test/test_flake8.py b/src/video/computer_vision/test/test_flake8.py similarity index 100% rename from src/nav_commanders/test/test_flake8.py rename to src/video/computer_vision/test/test_flake8.py diff --git a/src/nav_commanders/test/test_pep257.py b/src/video/computer_vision/test/test_pep257.py similarity index 100% rename from src/nav_commanders/test/test_pep257.py rename to src/video/computer_vision/test/test_pep257.py 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 From eeb2c0938bea2a57b974009e54394f10591e6e30 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Tue, 26 Aug 2025 10:11:39 +0000 Subject: [PATCH 33/36] fix typo --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index 296ef968..e2a0f3b1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -6,7 +6,7 @@ path = src/third-party/ublox url = https://github.com/KumarRobotics/ublox.git branch = ros2 -[submodule "src/thrid-party/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 From eb6d1f4155dc386ac4a8e16f2d32ed3cbdf1e95b Mon Sep 17 00:00:00 2001 From: ConnorN Date: Tue, 26 Aug 2025 10:20:45 +0000 Subject: [PATCH 34/36] sub --- .gitmodules | 4 ---- 1 file changed, 4 deletions(-) diff --git a/.gitmodules b/.gitmodules index e2a0f3b1..55ee4fb3 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,7 +1,3 @@ -[submodule "src/third-party/zed-ros2-wrapper"] - path = src/third-party/zed-ros2-wrapper - url = https://github.com/stereolabs/zed-ros2-wrapper.git - branch = Humble-v4.2.5 [submodule "src/third-party/ublox"] path = src/third-party/ublox url = https://github.com/KumarRobotics/ublox.git From 8965ac0a8494039341de393eec24412a4c288394 Mon Sep 17 00:00:00 2001 From: ConnorN Date: Tue, 26 Aug 2025 10:25:23 +0000 Subject: [PATCH 35/36] zed this time? --- .gitmodules | 3 +++ src/third-party/zed-ros2-wrapper | 1 + 2 files changed, 4 insertions(+) create mode 160000 src/third-party/zed-ros2-wrapper diff --git a/.gitmodules b/.gitmodules index 55ee4fb3..c57094fd 100644 --- a/.gitmodules +++ b/.gitmodules @@ -6,3 +6,6 @@ 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/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 From 54d2c468d99c78299f189b525ce9492ddd9aea4e Mon Sep 17 00:00:00 2001 From: ConnorN Date: Tue, 26 Aug 2025 10:22:54 -0400 Subject: [PATCH 36/36] move shit --- Dockerfile | 19 +- local_docker_build.sh | 1 - .../camera_streaming/CMakeLists.txt | 0 .../camera_streaming/config/webrtc.yaml | 0 .../gst-plugins/CMakeLists.txt | 0 .../gst-plugins/gstarucomarker.cpp | 0 .../gst-plugins/gstarucomarker.hpp | 0 .../camera_streaming/hooks/gst_env_hook.sh | 0 .../include/camera_streaming/webrtc_node.h | 0 .../camera_streaming/launch/webRTC.launch.py | 0 .../camera_streaming/package.xml | 0 .../camera_streaming/src/webrtc_node.cpp | 0 .../camera_streaming_qt/.gitignore | 0 .../camera_streaming_qt/CMakeLists.txt | 0 .../camera_streaming_qt/camera_client.h | 0 .../include/camera_streaming_qt/mainwindow.h | 0 .../tests/test_camera_node.h | 0 .../widgets/capture_image_widget.h | 0 .../camera_streaming_qt/widgets/main_widget.h | 0 .../widgets/preset_widget.h | 0 .../widgets/source_widget.h | 0 .../camera_streaming_qt/package.xml | 0 .../camera_streaming_qt/src/camera_client.cpp | 0 .../camera_streaming_qt/src/mainwindow.cpp | 0 .../src/tests/test_camera_node.cpp | 0 .../src/widgets/capture_image_widget.cpp | 0 .../src/widgets/main_widget.cpp | 0 .../src/widgets/preset_widget.cpp | 0 .../src/widgets/source_widget.cpp | 0 .../computer_vision}/__init__.py | 0 .../computer_vision/zed_aruco_detector.py | 0 .../launch/aruco_detector_zed.launch.py | 0 .../computer_vision/package.xml | 0 .../computer_vision/resource/computer_vision | 0 .../computer_vision/setup.cfg | 0 .../computer_vision/setup.py | 0 .../computer_vision}/test/test_copyright.py | 0 .../computer_vision}/test/test_flake8.py | 0 .../computer_vision}/test/test_pep257.py | 0 .../cprt_costmap_plugins/CMakeLists.txt | 0 .../cprt_costmap_plugins/costmap_plugins.xml | 0 .../cprt_costmap_plugins/gridmap_layer.hpp | 0 .../cprt_costmap_plugins/package.xml | 0 .../src/gridmap_layer.cpp | 0 .../cprt_gridmap_filters/CMakeLists.txt | 0 src/{nav => Nav}/cprt_gridmap_filters/LICENSE | 0 .../cprt_gridmap_filters/filter_plugins.xml | 0 .../include/PreserveCostInflationFilter.hpp | 0 .../cprt_gridmap_filters/package.xml | 0 .../scripts/calculate_decay_rate.py | 0 .../scripts/visualize_decay_rate.py | 0 .../src/PreserveCostInflationFilter.cpp | 0 .../cprt_planner_plugins/CMakeLists.txt | 0 src/{nav => Nav}/cprt_planner_plugins/LICENSE | 0 .../global_planner_plugin.xml | 0 .../event_horizon_planner.hpp | 0 .../cprt_planner_plugins/package.xml | 0 .../src/event_horizon_planner.cpp | 0 .../elevation_mapping/CMakeLists.txt | 0 src/{nav => Nav}/elevation_mapping/README.md | 0 .../config/elevation_maps/long_range.yaml | 0 .../config/elevation_maps/remove_object.yaml | 0 .../elevation_maps/simple_demo_map.yaml | 0 .../config/elevation_maps/starleth_map.yaml | 0 .../config/elevation_maps/zed2i_map.yaml | 0 .../elevation_mapping/config/file_path.yaml | 0 .../postprocessor_pipeline.yaml | 0 .../traversability_pipeline.yaml | 0 .../config/robots/ground_truth_demo.yaml | 0 .../config/robots/remove_object.yaml | 0 .../config/robots/simple_demo_robot.yaml | 0 .../config/robots/starleth.yaml | 0 .../config/robots/waffle_robot.yaml | 0 .../config/robots/zed2i_robot.yaml | 0 .../elevation_mapping/config/rosconsole.conf | 0 .../config/sensor_processors/RSLidar-32.yaml | 0 .../config/sensor_processors/aslam.yaml | 0 .../config/sensor_processors/fotonic_g45.yaml | 0 .../sensor_processors/hokuyo_utm-30lx.yaml | 0 .../kinect_nguyen_et_al.yaml | 0 .../config/sensor_processors/perfect.yaml | 0 ...nse_carmine_109_short_range_datasheet.yaml | 0 ..._carmine_109_short_range_uncalibrated.yaml | 0 .../sensor_processors/realsense_ZR300.yaml | 0 .../sensor_processors/realsense_d435.yaml | 0 .../sensor_processors/velodyne_HDL-32E.yaml | 0 .../sensor_processors/zed2i_processor.yaml | 0 .../config/visualization/fused.yaml | 0 .../config/visualization/raw.yaml | 0 .../config/zed_configs/common_stereo.yaml | 0 .../config/zed_configs/zed2i.yaml | 0 .../elevation_mapping/docker/.gitignore | 0 .../docker/Dockerfile.elevation_mapping | 0 .../docker/Dockerfile.ros2_humble | 0 .../elevation_mapping/docker/Dockerfile.user | 0 .../elevation_mapping/docker/build_docker.sh | 0 .../docker/container_scripts/.dockerignore | 0 .../container_scripts/workspace-entrypoint.sh | 0 .../middleware_profiles/rtps_udp_profile.xml | 0 .../docker/rosdep/extra_rosdeps.yaml | 0 .../elevation_mapping/docker/run_docker.sh | 0 .../docker/sourced_scripts/bash_functions.sh | 0 .../elevation_mapping/docker_instructions.md | 0 .../elevation_mapping/ElevationMap.hpp | 0 .../ElevationMapFunctors.hpp | 0 .../elevation_mapping/ElevationMapping.hpp | 0 .../PointXYZRGBConfidenceRatio.hpp | 0 .../RobotMotionMapUpdater.hpp | 0 .../ThreadSafeDataWrapper.hpp | 0 ...mpiricalCumulativeDistributionFunction.hpp | 0 .../elevation_mapping_node.hpp | 0 .../elevation_mapping/input_sources/Input.hpp | 0 .../input_sources/InputSourceManager.hpp | 0 .../PostprocessingPipelineFunctor.hpp | 0 .../postprocessing/PostprocessingWorker.hpp | 0 .../postprocessing/PostprocessorPool.hpp | 0 .../LaserSensorProcessor.hpp | 0 .../PerfectSensorProcessor.hpp | 0 .../sensor_processors/SensorProcessorBase.hpp | 0 .../StereoSensorProcessor.hpp | 0 .../StructuredLightSensorProcessor.hpp | 0 .../launch/elevationMapping_launch.py | 0 .../elevation_mapping/launch/rviz.launch.py | 0 .../launch/visualization.launch.py | 0 .../elevation_mapping/launch/zed2i.launch.py | 0 .../elevation_mapping/package.xml | 0 .../elevation_mapping/rviz2/custom_rviz2.rviz | 0 .../elevation_mapping/rviz2/map.rviz | 0 .../elevation_mapping/rviz2/viz.rviz | 0 .../elevation_mapping/rviz2/zed2i.rviz | 0 .../elevation_mapping/sample_data/maze.ply | Bin .../elevation_mapping/src/ElevationMap.cpp | 0 .../src/ElevationMapping.cpp | 0 .../src/PointXYZRGBConfidenceRatio.cpp | 0 .../src/RobotMotionMapUpdater.cpp | 0 .../src/elevation_mapping_node.cpp | 0 .../src/input_sources/Input.cpp | 0 .../src/input_sources/InputSourceManager.cpp | 0 .../PostprocessingPipelineFunctor.cpp | 0 .../postprocessing/PostprocessingWorker.cpp | 0 .../src/postprocessing/PostprocessorPool.cpp | 0 .../LaserSensorProcessor.cpp | 0 .../PerfectSensorProcessor.cpp | 0 .../sensor_processors/SensorProcessorBase.cpp | 0 .../StereoSensorProcessor.cpp | 0 .../StructuredLightSensorProcessor.cpp | 0 .../elevation_mapping/src/test.cpp | 0 .../elevation_mapping/src/test_service.cpp | 0 .../test/ElevationMapTest.cpp | 0 ...icalCumulativeDistributionFunctionTest.cpp | 0 .../test/elevation_mapping.test | 0 .../test/input_sources/InputSourcesTest.cpp | 0 .../config/TestConfigurations.yaml | 0 .../input_sources/config/rosconsole.config | 0 .../test/input_sources/input_sources.test | 0 .../test/input_sources/test_input_sources.cpp | 0 .../test/postprocessing/PostprocessorTest.cpp | 0 .../test/postprocessing/main.cpp | 0 .../test/postprocessing/postprocessor.test | 0 .../postprocessor_pipeline.yaml | 0 .../test/test_elevation_mapping.cpp | 0 .../gps/config/Heading_config.ubx | Bin src/{nav => Nav}/gps/config/Rover_config.ubx | Bin src/{nav => Nav}/gps/config/gps.yaml | 0 .../localization => Nav/gps/gps}/__init__.py | 0 src/{nav => Nav}/gps/gps/heading_pub_node.py | 0 src/{nav => Nav}/gps/gps/rtcm_pub_node.py | 0 src/{nav => Nav}/gps/gps/ubx_io_manager.py | 0 .../gps/launch/basestation.launch.py | 0 src/{nav => Nav}/gps/launch/heading.launch.py | 0 src/{nav => Nav}/gps/launch/rover.launch.py | 0 src/{nav => Nav}/gps/package.xml | 0 src/{nav => Nav}/gps/resource/gps | 0 src/{nav => Nav}/gps/setup.cfg | 0 src/{nav => Nav}/gps/setup.py | 0 .../gps}/test/test_copyright.py | 0 .../gps}/test/test_flake8.py | 0 .../gps}/test/test_pep257.py | 0 src/{nav => Nav}/kindr_ros/.gitignore | 0 src/{nav => Nav}/kindr_ros/LICENSE | 0 src/{nav => Nav}/kindr_ros/README.md | 0 .../kindr_ros/kindr_msgs/CMakeLists.txt | 0 .../kindr_msgs/msg/VectorAtPosition.msg | 0 .../kindr_ros/kindr_msgs/package.xml | 0 .../kindr_ros/kindr_ros/CMakeLists.txt | 0 .../RosGeometryMsgPhysicalQuantities.hpp | 77 +-- .../include/kindr_ros/RosGeometryMsgPose.hpp | 79 +-- .../kindr_ros/RosGeometryMsgRotation.hpp | 55 +- .../include/kindr_ros/RosGeometryMsgTwist.hpp | 66 ++ .../kindr_ros/include/kindr_ros/RosTfPose.hpp | 103 +++ .../kindr_ros/include/kindr_ros/kindr_ros.hpp | 29 +- .../kindr_ros/kindr_ros/package.xml | 0 .../kindr_ros/kindr_ros/test/CMakeLists.txt | 0 .../RosGeometryMsgPhysicalQuantitiesTest.cpp | 71 +- .../kindr_ros/test/RosGeometryMsgPoseTest.cpp | 82 +-- .../test/RosGeometryMsgRotationTest.cpp | 49 +- .../kindr_ros/test/RosTfPoseTest.cpp | 249 ++++--- .../kindr_ros/test/TfConventionTest.cpp | 104 ++- .../kindr_ros/kindr_ros/test/test_main.cpp | 31 +- .../kindr_rviz_plugins/CMakeLists.txt | 0 .../kindr_rviz_plugins/COLCON_IGNORE | 0 .../icons/classes/VectorAtPosition.png | Bin .../VectorAtPositionDisplay.hpp | 31 +- .../VectorAtPositionVisual.hpp | 27 +- .../kindr_ros/kindr_rviz_plugins/package.xml | 0 .../kindr_rviz_plugins/plugin_description.xml | 0 .../src/VectorAtPositionDisplay.cpp | 218 ++++++ .../src/VectorAtPositionVisual.cpp | 126 ++-- .../kindr_rviz_plugins/test/EmptyTests.cpp | 0 .../CMakeLists.txt | 0 .../COLCON_IGNORE | 0 .../icons/classes/MultiDOFJointTrajectory.png | Bin .../MultiDOFJointTrajectoryDisplay.hpp | 53 +- ...OFJointTrajectoryPointConnectionVisual.hpp | 26 +- .../MultiDOFJointTrajectoryPointVisual.hpp | 47 +- .../package.xml | 0 .../plugin_description.xml | 0 .../src/MultiDOFJointTrajectoryDisplay.cpp | 556 +++++++++++++++ ...OFJointTrajectoryPointConnectionVisual.cpp | 67 ++ .../MultiDOFJointTrajectoryPointVisual.cpp | 416 +++++++++++ .../test/EmptyTests.cpp | 0 .../localization/config/driver_params.yaml | 0 .../config/ekf/gps_ekf_global.yaml | 0 .../config/ekf/gps_ekf_local.yaml | 0 .../localization/config/navsat.yaml | 0 .../localization/config/rtabmap.yaml | 0 .../config/rviz.views/basicNavMap.rviz | 0 .../localization/config/slam.yaml | 0 .../localization/config/urdf/spike.urdf.xml | 0 .../localization/launch/description.launch.py | 0 .../localization/launch/ekf.launch.py | 0 .../launch/localization.launch.py | 0 .../localization/launch/navsat.launch.py | 0 .../localization/launch/rtabmap.launch.py | 0 .../localization/localization}/__init__.py | 0 .../localization/localization/imu_filter.py | 0 .../localization/lidar_mask_tool.py | 0 .../localization/republish_odometry.py | 0 src/{nav => Nav}/localization/package.xml | 0 .../localization/resource/localization | 0 src/{nav => Nav}/localization/setup.cfg | 0 src/{nav => Nav}/localization/setup.py | 0 .../old/drumheller_traversal_guess.yaml | 0 .../old/edc_crosswalk_to_res.yaml | 0 .../old/edcfront_northeast.yaml | 0 .../old/edcfront_ramp_to_edcparkinglot.yaml | 0 .../old/edcfront_southwest.yaml | 0 .../old/edcfront_trees_grass.yaml | 0 .../known_gps_coords/old/useful_locations.md | 0 .../known_gps_coords/send_gps_command.py | 0 .../launch/aruco_gps_commander.py | 0 .../launch/gps_commander.launch.py | 0 .../incremental_gps_commander.launch.py | 0 .../nav_commanders}/__init__.py | 0 .../nav_commanders/gps_utils.py | 0 .../incremental_gps_commander.py | 0 .../nav_commanders/nav_to_gps_aruco.py | 0 .../nav_commanders/nav_to_gps_coords.py | 0 src/{nav => Nav}/nav_commanders/package.xml | 0 .../nav_commanders/resource/nav_commanders | 0 src/{nav => Nav}/nav_commanders/setup.cfg | 0 src/{nav => Nav}/nav_commanders/setup.py | 0 .../nav_commanders}/test/test_copyright.py | 0 .../nav_commanders}/test/test_flake8.py | 0 .../nav_commanders}/test/test_pep257.py | 0 src/{nav => Nav}/navigation/CMakeLists.txt | 0 .../behavior_trees/behaviorTree.xml | 0 .../behaviorTree_NavPathingTestTree.xml | 0 .../behavior_trees/behaviorTree_original.xml | 0 .../behavior_trees/bt_dynamic_replanning.xml | 0 .../navigation/behavior_trees/spike_bt.xml | 0 .../ouster_OS0_32U_sensor_processor.yaml | 0 .../ouster_elevation_mapping.yaml | 0 .../postprocessing_traversability.yaml | 0 .../config/elevation_mapping/spike_robot.yaml | 0 .../zed2i_elevation_mapping.yaml | 0 .../zed2i_sensor_processor.yaml | 0 .../config/ouster/driver_params.yaml | 0 .../config/zed/zed_override_params.yaml | 0 .../navigation/launch/bringup.launch.py | 0 .../navigation/launch/nav2.launch.py | 0 .../navigation/launch/navigation.launch.py | 0 .../navigation/launch/ouster.launch.py | 0 .../launch/ouster_composable.launch.py | 0 .../launch/traversability_gridmap.launch.py | 0 .../navigation/launch/zed.launch.py | 0 .../ouster_mask_combined_shifted.png | Bin .../lidar_masks/ouster_mask_front.png | Bin .../lidar_masks/ouster_mask_left.png | Bin .../lidar_masks/ouster_mask_rear.png | Bin .../lidar_masks/ouster_mask_right.png | Bin src/{nav => Nav}/navigation/package.xml | 0 src/{nav => Nav}/navigation/params/nav2.yaml | 0 .../navigation/rviz/nav2_cprt.rviz | 0 .../rviz/nav2_offical_default_config.rviz | 0 .../rviz/traversibility_gridmap.rviz | 0 .../system-telemetry-cpp/CMakeLists.txt | 0 .../system-telemetry-cpp/LICENSE | 0 .../system-telemetry-cpp/cpu_collector.hpp | 0 .../system-telemetry-cpp/gpu_collector.hpp | 0 .../system-telemetry-cpp/memory_collector.hpp | 0 .../system_telemetry_publisher.hpp | 0 .../telemetry_collector.hpp | 0 .../system-telemetry-cpp/package.xml | 0 .../src/cpu_collector.cpp | 0 .../src/gpu_collector.cpp | 0 .../src/memory_collector.cpp | 0 .../src/system_telemetry_publisher.cpp | 0 .../include/kindr_ros/RosGeometryMsgTwist.hpp | 67 -- .../kindr_ros/include/kindr_ros/RosTfPose.hpp | 110 --- .../src/VectorAtPositionDisplay.cpp | 224 ------ .../src/MultiDOFJointTrajectoryDisplay.cpp | 652 ------------------ ...OFJointTrajectoryPointConnectionVisual.cpp | 69 -- .../MultiDOFJointTrajectoryPointVisual.cpp | 438 ------------ 314 files changed, 1975 insertions(+), 2167 deletions(-) rename src/{video => Cameras}/camera_streaming/CMakeLists.txt (100%) rename src/{video => Cameras}/camera_streaming/config/webrtc.yaml (100%) rename src/{video => Cameras}/camera_streaming/gst-plugins/CMakeLists.txt (100%) rename src/{video => Cameras}/camera_streaming/gst-plugins/gstarucomarker.cpp (100%) rename src/{video => Cameras}/camera_streaming/gst-plugins/gstarucomarker.hpp (100%) rename src/{video => Cameras}/camera_streaming/hooks/gst_env_hook.sh (100%) rename src/{video => Cameras}/camera_streaming/include/camera_streaming/webrtc_node.h (100%) rename src/{video => Cameras}/camera_streaming/launch/webRTC.launch.py (100%) rename src/{video => Cameras}/camera_streaming/package.xml (100%) rename src/{video => Cameras}/camera_streaming/src/webrtc_node.cpp (100%) rename src/{video => Cameras}/camera_streaming_qt/.gitignore (100%) rename src/{video => Cameras}/camera_streaming_qt/CMakeLists.txt (100%) rename src/{video => Cameras}/camera_streaming_qt/include/camera_streaming_qt/camera_client.h (100%) rename src/{video => Cameras}/camera_streaming_qt/include/camera_streaming_qt/mainwindow.h (100%) rename src/{video => Cameras}/camera_streaming_qt/include/camera_streaming_qt/tests/test_camera_node.h (100%) rename src/{video => Cameras}/camera_streaming_qt/include/camera_streaming_qt/widgets/capture_image_widget.h (100%) rename src/{video => Cameras}/camera_streaming_qt/include/camera_streaming_qt/widgets/main_widget.h (100%) rename src/{video => Cameras}/camera_streaming_qt/include/camera_streaming_qt/widgets/preset_widget.h (100%) rename src/{video => Cameras}/camera_streaming_qt/include/camera_streaming_qt/widgets/source_widget.h (100%) rename src/{video => Cameras}/camera_streaming_qt/package.xml (100%) rename src/{video => Cameras}/camera_streaming_qt/src/camera_client.cpp (100%) rename src/{video => Cameras}/camera_streaming_qt/src/mainwindow.cpp (100%) rename src/{video => Cameras}/camera_streaming_qt/src/tests/test_camera_node.cpp (100%) rename src/{video => Cameras}/camera_streaming_qt/src/widgets/capture_image_widget.cpp (100%) rename src/{video => Cameras}/camera_streaming_qt/src/widgets/main_widget.cpp (100%) rename src/{video => Cameras}/camera_streaming_qt/src/widgets/preset_widget.cpp (100%) rename src/{video => Cameras}/camera_streaming_qt/src/widgets/source_widget.cpp (100%) rename src/{nav/gps/gps => Cameras/computer_vision/computer_vision}/__init__.py (100%) rename src/{video => Cameras}/computer_vision/computer_vision/zed_aruco_detector.py (100%) rename src/{video => Cameras}/computer_vision/launch/aruco_detector_zed.launch.py (100%) rename src/{video => Cameras}/computer_vision/package.xml (100%) rename src/{video => Cameras}/computer_vision/resource/computer_vision (100%) rename src/{video => Cameras}/computer_vision/setup.cfg (100%) rename src/{video => Cameras}/computer_vision/setup.py (100%) rename src/{nav/gps => Cameras/computer_vision}/test/test_copyright.py (100%) rename src/{nav/gps => Cameras/computer_vision}/test/test_flake8.py (100%) rename src/{nav/gps => Cameras/computer_vision}/test/test_pep257.py (100%) rename src/{nav => Nav}/cprt_costmap_plugins/CMakeLists.txt (100%) rename src/{nav => Nav}/cprt_costmap_plugins/costmap_plugins.xml (100%) rename src/{nav => Nav}/cprt_costmap_plugins/include/cprt_costmap_plugins/gridmap_layer.hpp (100%) rename src/{nav => Nav}/cprt_costmap_plugins/package.xml (100%) rename src/{nav => Nav}/cprt_costmap_plugins/src/gridmap_layer.cpp (100%) rename src/{nav => Nav}/cprt_gridmap_filters/CMakeLists.txt (100%) rename src/{nav => Nav}/cprt_gridmap_filters/LICENSE (100%) rename src/{nav => Nav}/cprt_gridmap_filters/filter_plugins.xml (100%) rename src/{nav => Nav}/cprt_gridmap_filters/include/PreserveCostInflationFilter.hpp (100%) rename src/{nav => Nav}/cprt_gridmap_filters/package.xml (100%) rename src/{nav => Nav}/cprt_gridmap_filters/scripts/calculate_decay_rate.py (100%) rename src/{nav => Nav}/cprt_gridmap_filters/scripts/visualize_decay_rate.py (100%) rename src/{nav => Nav}/cprt_gridmap_filters/src/PreserveCostInflationFilter.cpp (100%) rename src/{nav => Nav}/cprt_planner_plugins/CMakeLists.txt (100%) rename src/{nav => Nav}/cprt_planner_plugins/LICENSE (100%) rename src/{nav => Nav}/cprt_planner_plugins/global_planner_plugin.xml (100%) rename src/{nav => Nav}/cprt_planner_plugins/include/cprt_planner_plugins/event_horizon_planner.hpp (100%) rename src/{nav => Nav}/cprt_planner_plugins/package.xml (100%) rename src/{nav => Nav}/cprt_planner_plugins/src/event_horizon_planner.cpp (100%) rename src/{nav => Nav}/elevation_mapping/CMakeLists.txt (100%) rename src/{nav => Nav}/elevation_mapping/README.md (100%) rename src/{nav => Nav}/elevation_mapping/config/elevation_maps/long_range.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/elevation_maps/remove_object.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/elevation_maps/simple_demo_map.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/elevation_maps/starleth_map.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/elevation_maps/zed2i_map.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/file_path.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/postprocessing/postprocessor_pipeline.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/postprocessing/traversability_pipeline.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/robots/ground_truth_demo.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/robots/remove_object.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/robots/simple_demo_robot.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/robots/starleth.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/robots/waffle_robot.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/robots/zed2i_robot.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/rosconsole.conf (100%) rename src/{nav => Nav}/elevation_mapping/config/sensor_processors/RSLidar-32.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/sensor_processors/aslam.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/sensor_processors/fotonic_g45.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/sensor_processors/hokuyo_utm-30lx.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/sensor_processors/kinect_nguyen_et_al.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/sensor_processors/perfect.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_datasheet.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_uncalibrated.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/sensor_processors/realsense_ZR300.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/sensor_processors/realsense_d435.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/sensor_processors/velodyne_HDL-32E.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/sensor_processors/zed2i_processor.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/visualization/fused.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/visualization/raw.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/zed_configs/common_stereo.yaml (100%) rename src/{nav => Nav}/elevation_mapping/config/zed_configs/zed2i.yaml (100%) rename src/{nav => Nav}/elevation_mapping/docker/.gitignore (100%) rename src/{nav => Nav}/elevation_mapping/docker/Dockerfile.elevation_mapping (100%) rename src/{nav => Nav}/elevation_mapping/docker/Dockerfile.ros2_humble (100%) rename src/{nav => Nav}/elevation_mapping/docker/Dockerfile.user (100%) rename src/{nav => Nav}/elevation_mapping/docker/build_docker.sh (100%) rename src/{nav => Nav}/elevation_mapping/docker/container_scripts/.dockerignore (100%) rename src/{nav => Nav}/elevation_mapping/docker/container_scripts/workspace-entrypoint.sh (100%) rename src/{nav => Nav}/elevation_mapping/docker/middleware_profiles/rtps_udp_profile.xml (100%) rename src/{nav => Nav}/elevation_mapping/docker/rosdep/extra_rosdeps.yaml (100%) rename src/{nav => Nav}/elevation_mapping/docker/run_docker.sh (100%) rename src/{nav => Nav}/elevation_mapping/docker/sourced_scripts/bash_functions.sh (100%) rename src/{nav => Nav}/elevation_mapping/docker_instructions.md (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/ElevationMap.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/ElevationMapFunctors.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/PointXYZRGBConfidenceRatio.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/RobotMotionMapUpdater.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/ThreadSafeDataWrapper.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/elevation_mapping_node.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/input_sources/Input.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/input_sources/InputSourceManager.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingPipelineFunctor.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingWorker.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessorPool.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/sensor_processors/LaserSensorProcessor.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/sensor_processors/StereoSensorProcessor.hpp (100%) rename src/{nav => Nav}/elevation_mapping/include/elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp (100%) rename src/{nav => Nav}/elevation_mapping/launch/elevationMapping_launch.py (100%) rename src/{nav => Nav}/elevation_mapping/launch/rviz.launch.py (100%) rename src/{nav => Nav}/elevation_mapping/launch/visualization.launch.py (100%) rename src/{nav => Nav}/elevation_mapping/launch/zed2i.launch.py (100%) rename src/{nav => Nav}/elevation_mapping/package.xml (100%) rename src/{nav => Nav}/elevation_mapping/rviz2/custom_rviz2.rviz (100%) rename src/{nav => Nav}/elevation_mapping/rviz2/map.rviz (100%) rename src/{nav => Nav}/elevation_mapping/rviz2/viz.rviz (100%) rename src/{nav => Nav}/elevation_mapping/rviz2/zed2i.rviz (100%) rename src/{nav => Nav}/elevation_mapping/sample_data/maze.ply (100%) rename src/{nav => Nav}/elevation_mapping/src/ElevationMap.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/ElevationMapping.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/PointXYZRGBConfidenceRatio.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/RobotMotionMapUpdater.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/elevation_mapping_node.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/input_sources/Input.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/input_sources/InputSourceManager.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/postprocessing/PostprocessingPipelineFunctor.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/postprocessing/PostprocessingWorker.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/postprocessing/PostprocessorPool.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/test.cpp (100%) rename src/{nav => Nav}/elevation_mapping/src/test_service.cpp (100%) rename src/{nav => Nav}/elevation_mapping/test/ElevationMapTest.cpp (100%) rename src/{nav => Nav}/elevation_mapping/test/WeightedEmpiricalCumulativeDistributionFunctionTest.cpp (100%) rename src/{nav => Nav}/elevation_mapping/test/elevation_mapping.test (100%) rename src/{nav => Nav}/elevation_mapping/test/input_sources/InputSourcesTest.cpp (100%) rename src/{nav => Nav}/elevation_mapping/test/input_sources/config/TestConfigurations.yaml (100%) rename src/{nav => Nav}/elevation_mapping/test/input_sources/config/rosconsole.config (100%) rename src/{nav => Nav}/elevation_mapping/test/input_sources/input_sources.test (100%) rename src/{nav => Nav}/elevation_mapping/test/input_sources/test_input_sources.cpp (100%) rename src/{nav => Nav}/elevation_mapping/test/postprocessing/PostprocessorTest.cpp (100%) rename src/{nav => Nav}/elevation_mapping/test/postprocessing/main.cpp (100%) rename src/{nav => Nav}/elevation_mapping/test/postprocessing/postprocessor.test (100%) rename src/{nav => Nav}/elevation_mapping/test/postprocessing/postprocessor_pipeline.yaml (100%) rename src/{nav => Nav}/elevation_mapping/test/test_elevation_mapping.cpp (100%) rename src/{nav => Nav}/gps/config/Heading_config.ubx (100%) rename src/{nav => Nav}/gps/config/Rover_config.ubx (100%) rename src/{nav => Nav}/gps/config/gps.yaml (100%) rename src/{nav/localization/localization => Nav/gps/gps}/__init__.py (100%) rename src/{nav => Nav}/gps/gps/heading_pub_node.py (100%) rename src/{nav => Nav}/gps/gps/rtcm_pub_node.py (100%) rename src/{nav => Nav}/gps/gps/ubx_io_manager.py (100%) rename src/{nav => Nav}/gps/launch/basestation.launch.py (100%) rename src/{nav => Nav}/gps/launch/heading.launch.py (100%) rename src/{nav => Nav}/gps/launch/rover.launch.py (100%) rename src/{nav => Nav}/gps/package.xml (100%) rename src/{nav => Nav}/gps/resource/gps (100%) rename src/{nav => Nav}/gps/setup.cfg (100%) rename src/{nav => Nav}/gps/setup.py (100%) rename src/{nav/nav_commanders => Nav/gps}/test/test_copyright.py (100%) rename src/{nav/nav_commanders => Nav/gps}/test/test_flake8.py (100%) rename src/{nav/nav_commanders => Nav/gps}/test/test_pep257.py (100%) rename src/{nav => Nav}/kindr_ros/.gitignore (100%) rename src/{nav => Nav}/kindr_ros/LICENSE (100%) rename src/{nav => Nav}/kindr_ros/README.md (100%) rename src/{nav => Nav}/kindr_ros/kindr_msgs/CMakeLists.txt (100%) rename src/{nav => Nav}/kindr_ros/kindr_msgs/msg/VectorAtPosition.msg (100%) rename src/{nav => Nav}/kindr_ros/kindr_msgs/package.xml (100%) rename src/{nav => Nav}/kindr_ros/kindr_ros/CMakeLists.txt (100%) rename src/{nav => Nav}/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPhysicalQuantities.hpp (59%) rename src/{nav => Nav}/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPose.hpp (53%) rename src/{nav => Nav}/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgRotation.hpp (50%) create mode 100644 src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgTwist.hpp create mode 100644 src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosTfPose.hpp rename src/{nav => Nav}/kindr_ros/kindr_ros/include/kindr_ros/kindr_ros.hpp (61%) rename src/{nav => Nav}/kindr_ros/kindr_ros/package.xml (100%) rename src/{nav => Nav}/kindr_ros/kindr_ros/test/CMakeLists.txt (100%) rename src/{nav => Nav}/kindr_ros/kindr_ros/test/RosGeometryMsgPhysicalQuantitiesTest.cpp (71%) rename src/{nav => Nav}/kindr_ros/kindr_ros/test/RosGeometryMsgPoseTest.cpp (73%) rename src/{nav => Nav}/kindr_ros/kindr_ros/test/RosGeometryMsgRotationTest.cpp (66%) rename src/{nav => Nav}/kindr_ros/kindr_ros/test/RosTfPoseTest.cpp (60%) rename src/{nav => Nav}/kindr_ros/kindr_ros/test/TfConventionTest.cpp (54%) rename src/{nav => Nav}/kindr_ros/kindr_ros/test/test_main.cpp (55%) rename src/{nav => Nav}/kindr_ros/kindr_rviz_plugins/CMakeLists.txt (100%) rename src/{nav => Nav}/kindr_ros/kindr_rviz_plugins/COLCON_IGNORE (100%) rename src/{nav => Nav}/kindr_ros/kindr_rviz_plugins/icons/classes/VectorAtPosition.png (100%) rename src/{nav => Nav}/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp (89%) rename src/{nav => Nav}/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionVisual.hpp (87%) rename src/{nav => Nav}/kindr_ros/kindr_rviz_plugins/package.xml (100%) rename src/{nav => Nav}/kindr_ros/kindr_rviz_plugins/plugin_description.xml (100%) create mode 100644 src/Nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp rename src/{nav => Nav}/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp (65%) rename src/{nav => Nav}/kindr_ros/kindr_rviz_plugins/test/EmptyTests.cpp (100%) rename src/{nav => Nav}/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/CMakeLists.txt (100%) rename src/{nav => Nav}/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/COLCON_IGNORE (100%) rename src/{nav => Nav}/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/icons/classes/MultiDOFJointTrajectory.png (100%) rename src/{nav => Nav}/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp (87%) rename src/{nav => Nav}/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp (77%) rename src/{nav => Nav}/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp (84%) rename src/{nav => Nav}/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/package.xml (100%) rename src/{nav => Nav}/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/plugin_description.xml (100%) create mode 100644 src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp create mode 100644 src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp create mode 100644 src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp rename src/{nav => Nav}/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/test/EmptyTests.cpp (100%) rename src/{nav => Nav}/localization/config/driver_params.yaml (100%) rename src/{nav => Nav}/localization/config/ekf/gps_ekf_global.yaml (100%) rename src/{nav => Nav}/localization/config/ekf/gps_ekf_local.yaml (100%) rename src/{nav => Nav}/localization/config/navsat.yaml (100%) rename src/{nav => Nav}/localization/config/rtabmap.yaml (100%) rename src/{nav => Nav}/localization/config/rviz.views/basicNavMap.rviz (100%) rename src/{nav => Nav}/localization/config/slam.yaml (100%) rename src/{nav => Nav}/localization/config/urdf/spike.urdf.xml (100%) rename src/{nav => Nav}/localization/launch/description.launch.py (100%) rename src/{nav => Nav}/localization/launch/ekf.launch.py (100%) rename src/{nav => Nav}/localization/launch/localization.launch.py (100%) rename src/{nav => Nav}/localization/launch/navsat.launch.py (100%) rename src/{nav => Nav}/localization/launch/rtabmap.launch.py (100%) rename src/{nav/nav_commanders/nav_commanders => Nav/localization/localization}/__init__.py (100%) rename src/{nav => Nav}/localization/localization/imu_filter.py (100%) rename src/{nav => Nav}/localization/localization/lidar_mask_tool.py (100%) rename src/{nav => Nav}/localization/localization/republish_odometry.py (100%) rename src/{nav => Nav}/localization/package.xml (100%) rename src/{nav => Nav}/localization/resource/localization (100%) rename src/{nav => Nav}/localization/setup.cfg (100%) rename src/{nav => Nav}/localization/setup.py (100%) rename src/{nav => Nav}/nav_commanders/known_gps_coords/old/drumheller_traversal_guess.yaml (100%) rename src/{nav => Nav}/nav_commanders/known_gps_coords/old/edc_crosswalk_to_res.yaml (100%) rename src/{nav => Nav}/nav_commanders/known_gps_coords/old/edcfront_northeast.yaml (100%) rename src/{nav => Nav}/nav_commanders/known_gps_coords/old/edcfront_ramp_to_edcparkinglot.yaml (100%) rename src/{nav => Nav}/nav_commanders/known_gps_coords/old/edcfront_southwest.yaml (100%) rename src/{nav => Nav}/nav_commanders/known_gps_coords/old/edcfront_trees_grass.yaml (100%) rename src/{nav => Nav}/nav_commanders/known_gps_coords/old/useful_locations.md (100%) rename src/{nav => Nav}/nav_commanders/known_gps_coords/send_gps_command.py (100%) rename src/{nav => Nav}/nav_commanders/launch/aruco_gps_commander.py (100%) rename src/{nav => Nav}/nav_commanders/launch/gps_commander.launch.py (100%) rename src/{nav => Nav}/nav_commanders/launch/incremental_gps_commander.launch.py (100%) rename src/{video/computer_vision/computer_vision => Nav/nav_commanders/nav_commanders}/__init__.py (100%) rename src/{nav => Nav}/nav_commanders/nav_commanders/gps_utils.py (100%) rename src/{nav => Nav}/nav_commanders/nav_commanders/incremental_gps_commander.py (100%) rename src/{nav => Nav}/nav_commanders/nav_commanders/nav_to_gps_aruco.py (100%) rename src/{nav => Nav}/nav_commanders/nav_commanders/nav_to_gps_coords.py (100%) rename src/{nav => Nav}/nav_commanders/package.xml (100%) rename src/{nav => Nav}/nav_commanders/resource/nav_commanders (100%) rename src/{nav => Nav}/nav_commanders/setup.cfg (100%) rename src/{nav => Nav}/nav_commanders/setup.py (100%) rename src/{video/computer_vision => Nav/nav_commanders}/test/test_copyright.py (100%) rename src/{video/computer_vision => Nav/nav_commanders}/test/test_flake8.py (100%) rename src/{video/computer_vision => Nav/nav_commanders}/test/test_pep257.py (100%) rename src/{nav => Nav}/navigation/CMakeLists.txt (100%) rename src/{nav => Nav}/navigation/behavior_trees/behaviorTree.xml (100%) rename src/{nav => Nav}/navigation/behavior_trees/behaviorTree_NavPathingTestTree.xml (100%) rename src/{nav => Nav}/navigation/behavior_trees/behaviorTree_original.xml (100%) rename src/{nav => Nav}/navigation/behavior_trees/bt_dynamic_replanning.xml (100%) rename src/{nav => Nav}/navigation/behavior_trees/spike_bt.xml (100%) rename src/{nav => Nav}/navigation/config/elevation_mapping/ouster_OS0_32U_sensor_processor.yaml (100%) rename src/{nav => Nav}/navigation/config/elevation_mapping/ouster_elevation_mapping.yaml (100%) rename src/{nav => Nav}/navigation/config/elevation_mapping/postprocessing_traversability.yaml (100%) rename src/{nav => Nav}/navigation/config/elevation_mapping/spike_robot.yaml (100%) rename src/{nav => Nav}/navigation/config/elevation_mapping/zed2i_elevation_mapping.yaml (100%) rename src/{nav => Nav}/navigation/config/elevation_mapping/zed2i_sensor_processor.yaml (100%) rename src/{nav => Nav}/navigation/config/ouster/driver_params.yaml (100%) rename src/{nav => Nav}/navigation/config/zed/zed_override_params.yaml (100%) rename src/{nav => Nav}/navigation/launch/bringup.launch.py (100%) rename src/{nav => Nav}/navigation/launch/nav2.launch.py (100%) rename src/{nav => Nav}/navigation/launch/navigation.launch.py (100%) rename src/{nav => Nav}/navigation/launch/ouster.launch.py (100%) rename src/{nav => Nav}/navigation/launch/ouster_composable.launch.py (100%) rename src/{nav => Nav}/navigation/launch/traversability_gridmap.launch.py (100%) rename src/{nav => Nav}/navigation/launch/zed.launch.py (100%) rename src/{nav => Nav}/navigation/lidar_masks/ouster_mask_combined_shifted.png (100%) rename src/{nav => Nav}/navigation/lidar_masks/ouster_mask_front.png (100%) rename src/{nav => Nav}/navigation/lidar_masks/ouster_mask_left.png (100%) rename src/{nav => Nav}/navigation/lidar_masks/ouster_mask_rear.png (100%) rename src/{nav => Nav}/navigation/lidar_masks/ouster_mask_right.png (100%) rename src/{nav => Nav}/navigation/package.xml (100%) rename src/{nav => Nav}/navigation/params/nav2.yaml (100%) rename src/{nav => Nav}/navigation/rviz/nav2_cprt.rviz (100%) rename src/{nav => Nav}/navigation/rviz/nav2_offical_default_config.rviz (100%) rename src/{nav => Nav}/navigation/rviz/traversibility_gridmap.rviz (100%) rename src/{telemetry => Telemetry}/system-telemetry-cpp/CMakeLists.txt (100%) rename src/{telemetry => Telemetry}/system-telemetry-cpp/LICENSE (100%) rename src/{telemetry => Telemetry}/system-telemetry-cpp/include/system-telemetry-cpp/cpu_collector.hpp (100%) rename src/{telemetry => Telemetry}/system-telemetry-cpp/include/system-telemetry-cpp/gpu_collector.hpp (100%) rename src/{telemetry => Telemetry}/system-telemetry-cpp/include/system-telemetry-cpp/memory_collector.hpp (100%) rename src/{telemetry => Telemetry}/system-telemetry-cpp/include/system-telemetry-cpp/system_telemetry_publisher.hpp (100%) rename src/{telemetry => Telemetry}/system-telemetry-cpp/include/system-telemetry-cpp/telemetry_collector.hpp (100%) rename src/{telemetry => Telemetry}/system-telemetry-cpp/package.xml (100%) rename src/{telemetry => Telemetry}/system-telemetry-cpp/src/cpu_collector.cpp (100%) rename src/{telemetry => Telemetry}/system-telemetry-cpp/src/gpu_collector.cpp (100%) rename src/{telemetry => Telemetry}/system-telemetry-cpp/src/memory_collector.cpp (100%) rename src/{telemetry => Telemetry}/system-telemetry-cpp/src/system_telemetry_publisher.cpp (100%) delete mode 100644 src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgTwist.hpp delete mode 100644 src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosTfPose.hpp delete mode 100644 src/nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp delete mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp delete mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp delete mode 100644 src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp diff --git a/Dockerfile b/Dockerfile index ed620a7a..c2f64da1 100644 --- a/Dockerfile +++ b/Dockerfile @@ -21,13 +21,13 @@ 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 \ - && rm -rf /var/lib/apt/lists/* + && 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 @@ -40,10 +40,10 @@ RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ > /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 \ - && rm -rf /var/lib/apt/lists/* + libeigen3-dev python3-rosdep # Python dependencies (per-arch pip cache) COPY requirements.txt . @@ -74,11 +74,11 @@ 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 \ - && rm -rf /var/lib/apt/lists/* # Enable compiler caching ENV CCACHE_DIR=/root/.cache/ccache @@ -139,6 +139,7 @@ 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 && \ @@ -155,11 +156,11 @@ RUN git clone https://github.com/ANYbotics/kindr.git && \ ############################ 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 \ - && rm -rf /var/lib/apt/lists/* + python3-colcon-ros clang-format cuda-toolkit-12 # Enable compiler caching @@ -186,10 +187,10 @@ 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 \ - && rm -rf /var/lib/apt/lists/* + python3-colcon-ros ccache cuda-toolkit-12 COPY src/ ${DIR}/src/ diff --git a/local_docker_build.sh b/local_docker_build.sh index 8f2ed08d..cf48f2ab 100755 --- a/local_docker_build.sh +++ b/local_docker_build.sh @@ -45,7 +45,6 @@ docker buildx build \ -f Dockerfile \ --target builder \ -t ${IMAGE_NAME}:${TARGETARCH} \ - -t ${IMAGE_NAME}:${GIT_SHA}-${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/src/video/camera_streaming/CMakeLists.txt b/src/Cameras/camera_streaming/CMakeLists.txt similarity index 100% rename from src/video/camera_streaming/CMakeLists.txt rename to src/Cameras/camera_streaming/CMakeLists.txt diff --git a/src/video/camera_streaming/config/webrtc.yaml b/src/Cameras/camera_streaming/config/webrtc.yaml similarity index 100% rename from src/video/camera_streaming/config/webrtc.yaml rename to src/Cameras/camera_streaming/config/webrtc.yaml diff --git a/src/video/camera_streaming/gst-plugins/CMakeLists.txt b/src/Cameras/camera_streaming/gst-plugins/CMakeLists.txt similarity index 100% rename from src/video/camera_streaming/gst-plugins/CMakeLists.txt rename to src/Cameras/camera_streaming/gst-plugins/CMakeLists.txt diff --git a/src/video/camera_streaming/gst-plugins/gstarucomarker.cpp b/src/Cameras/camera_streaming/gst-plugins/gstarucomarker.cpp similarity index 100% rename from src/video/camera_streaming/gst-plugins/gstarucomarker.cpp rename to src/Cameras/camera_streaming/gst-plugins/gstarucomarker.cpp diff --git a/src/video/camera_streaming/gst-plugins/gstarucomarker.hpp b/src/Cameras/camera_streaming/gst-plugins/gstarucomarker.hpp similarity index 100% rename from src/video/camera_streaming/gst-plugins/gstarucomarker.hpp rename to src/Cameras/camera_streaming/gst-plugins/gstarucomarker.hpp diff --git a/src/video/camera_streaming/hooks/gst_env_hook.sh b/src/Cameras/camera_streaming/hooks/gst_env_hook.sh similarity index 100% rename from src/video/camera_streaming/hooks/gst_env_hook.sh rename to src/Cameras/camera_streaming/hooks/gst_env_hook.sh diff --git a/src/video/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/video/camera_streaming/include/camera_streaming/webrtc_node.h rename to src/Cameras/camera_streaming/include/camera_streaming/webrtc_node.h diff --git a/src/video/camera_streaming/launch/webRTC.launch.py b/src/Cameras/camera_streaming/launch/webRTC.launch.py similarity index 100% rename from src/video/camera_streaming/launch/webRTC.launch.py rename to src/Cameras/camera_streaming/launch/webRTC.launch.py diff --git a/src/video/camera_streaming/package.xml b/src/Cameras/camera_streaming/package.xml similarity index 100% rename from src/video/camera_streaming/package.xml rename to src/Cameras/camera_streaming/package.xml diff --git a/src/video/camera_streaming/src/webrtc_node.cpp b/src/Cameras/camera_streaming/src/webrtc_node.cpp similarity index 100% rename from src/video/camera_streaming/src/webrtc_node.cpp rename to src/Cameras/camera_streaming/src/webrtc_node.cpp diff --git a/src/video/camera_streaming_qt/.gitignore b/src/Cameras/camera_streaming_qt/.gitignore similarity index 100% rename from src/video/camera_streaming_qt/.gitignore rename to src/Cameras/camera_streaming_qt/.gitignore diff --git a/src/video/camera_streaming_qt/CMakeLists.txt b/src/Cameras/camera_streaming_qt/CMakeLists.txt similarity index 100% rename from src/video/camera_streaming_qt/CMakeLists.txt rename to src/Cameras/camera_streaming_qt/CMakeLists.txt diff --git a/src/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/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/video/camera_streaming_qt/package.xml b/src/Cameras/camera_streaming_qt/package.xml similarity index 100% rename from src/video/camera_streaming_qt/package.xml rename to src/Cameras/camera_streaming_qt/package.xml diff --git a/src/video/camera_streaming_qt/src/camera_client.cpp b/src/Cameras/camera_streaming_qt/src/camera_client.cpp similarity index 100% rename from src/video/camera_streaming_qt/src/camera_client.cpp rename to src/Cameras/camera_streaming_qt/src/camera_client.cpp diff --git a/src/video/camera_streaming_qt/src/mainwindow.cpp b/src/Cameras/camera_streaming_qt/src/mainwindow.cpp similarity index 100% rename from src/video/camera_streaming_qt/src/mainwindow.cpp rename to src/Cameras/camera_streaming_qt/src/mainwindow.cpp diff --git a/src/video/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/video/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/video/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/video/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/video/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/video/camera_streaming_qt/src/widgets/main_widget.cpp rename to src/Cameras/camera_streaming_qt/src/widgets/main_widget.cpp diff --git a/src/video/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/video/camera_streaming_qt/src/widgets/preset_widget.cpp rename to src/Cameras/camera_streaming_qt/src/widgets/preset_widget.cpp diff --git a/src/video/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/video/camera_streaming_qt/src/widgets/source_widget.cpp rename to src/Cameras/camera_streaming_qt/src/widgets/source_widget.cpp diff --git a/src/nav/gps/gps/__init__.py b/src/Cameras/computer_vision/computer_vision/__init__.py similarity index 100% rename from src/nav/gps/gps/__init__.py rename to src/Cameras/computer_vision/computer_vision/__init__.py diff --git a/src/video/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/video/computer_vision/computer_vision/zed_aruco_detector.py rename to src/Cameras/computer_vision/computer_vision/zed_aruco_detector.py diff --git a/src/video/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/video/computer_vision/launch/aruco_detector_zed.launch.py rename to src/Cameras/computer_vision/launch/aruco_detector_zed.launch.py diff --git a/src/video/computer_vision/package.xml b/src/Cameras/computer_vision/package.xml similarity index 100% rename from src/video/computer_vision/package.xml rename to src/Cameras/computer_vision/package.xml diff --git a/src/video/computer_vision/resource/computer_vision b/src/Cameras/computer_vision/resource/computer_vision similarity index 100% rename from src/video/computer_vision/resource/computer_vision rename to src/Cameras/computer_vision/resource/computer_vision diff --git a/src/video/computer_vision/setup.cfg b/src/Cameras/computer_vision/setup.cfg similarity index 100% rename from src/video/computer_vision/setup.cfg rename to src/Cameras/computer_vision/setup.cfg diff --git a/src/video/computer_vision/setup.py b/src/Cameras/computer_vision/setup.py similarity index 100% rename from src/video/computer_vision/setup.py rename to src/Cameras/computer_vision/setup.py diff --git a/src/nav/gps/test/test_copyright.py b/src/Cameras/computer_vision/test/test_copyright.py similarity index 100% rename from src/nav/gps/test/test_copyright.py rename to src/Cameras/computer_vision/test/test_copyright.py diff --git a/src/nav/gps/test/test_flake8.py b/src/Cameras/computer_vision/test/test_flake8.py similarity index 100% rename from src/nav/gps/test/test_flake8.py rename to src/Cameras/computer_vision/test/test_flake8.py diff --git a/src/nav/gps/test/test_pep257.py b/src/Cameras/computer_vision/test/test_pep257.py similarity index 100% rename from src/nav/gps/test/test_pep257.py rename to src/Cameras/computer_vision/test/test_pep257.py diff --git a/src/nav/cprt_costmap_plugins/CMakeLists.txt b/src/Nav/cprt_costmap_plugins/CMakeLists.txt similarity index 100% rename from src/nav/cprt_costmap_plugins/CMakeLists.txt rename to src/Nav/cprt_costmap_plugins/CMakeLists.txt diff --git a/src/nav/cprt_costmap_plugins/costmap_plugins.xml b/src/Nav/cprt_costmap_plugins/costmap_plugins.xml similarity index 100% rename from src/nav/cprt_costmap_plugins/costmap_plugins.xml rename to src/Nav/cprt_costmap_plugins/costmap_plugins.xml diff --git a/src/nav/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/nav/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/nav/cprt_costmap_plugins/package.xml b/src/Nav/cprt_costmap_plugins/package.xml similarity index 100% rename from src/nav/cprt_costmap_plugins/package.xml rename to src/Nav/cprt_costmap_plugins/package.xml diff --git a/src/nav/cprt_costmap_plugins/src/gridmap_layer.cpp b/src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp similarity index 100% rename from src/nav/cprt_costmap_plugins/src/gridmap_layer.cpp rename to src/Nav/cprt_costmap_plugins/src/gridmap_layer.cpp diff --git a/src/nav/cprt_gridmap_filters/CMakeLists.txt b/src/Nav/cprt_gridmap_filters/CMakeLists.txt similarity index 100% rename from src/nav/cprt_gridmap_filters/CMakeLists.txt rename to src/Nav/cprt_gridmap_filters/CMakeLists.txt diff --git a/src/nav/cprt_gridmap_filters/LICENSE b/src/Nav/cprt_gridmap_filters/LICENSE similarity index 100% rename from src/nav/cprt_gridmap_filters/LICENSE rename to src/Nav/cprt_gridmap_filters/LICENSE diff --git a/src/nav/cprt_gridmap_filters/filter_plugins.xml b/src/Nav/cprt_gridmap_filters/filter_plugins.xml similarity index 100% rename from src/nav/cprt_gridmap_filters/filter_plugins.xml rename to src/Nav/cprt_gridmap_filters/filter_plugins.xml diff --git a/src/nav/cprt_gridmap_filters/include/PreserveCostInflationFilter.hpp b/src/Nav/cprt_gridmap_filters/include/PreserveCostInflationFilter.hpp similarity index 100% rename from src/nav/cprt_gridmap_filters/include/PreserveCostInflationFilter.hpp rename to src/Nav/cprt_gridmap_filters/include/PreserveCostInflationFilter.hpp diff --git a/src/nav/cprt_gridmap_filters/package.xml b/src/Nav/cprt_gridmap_filters/package.xml similarity index 100% rename from src/nav/cprt_gridmap_filters/package.xml rename to src/Nav/cprt_gridmap_filters/package.xml diff --git a/src/nav/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/nav/cprt_gridmap_filters/scripts/calculate_decay_rate.py rename to src/Nav/cprt_gridmap_filters/scripts/calculate_decay_rate.py diff --git a/src/nav/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/nav/cprt_gridmap_filters/scripts/visualize_decay_rate.py rename to src/Nav/cprt_gridmap_filters/scripts/visualize_decay_rate.py diff --git a/src/nav/cprt_gridmap_filters/src/PreserveCostInflationFilter.cpp b/src/Nav/cprt_gridmap_filters/src/PreserveCostInflationFilter.cpp similarity index 100% rename from src/nav/cprt_gridmap_filters/src/PreserveCostInflationFilter.cpp rename to src/Nav/cprt_gridmap_filters/src/PreserveCostInflationFilter.cpp diff --git a/src/nav/cprt_planner_plugins/CMakeLists.txt b/src/Nav/cprt_planner_plugins/CMakeLists.txt similarity index 100% rename from src/nav/cprt_planner_plugins/CMakeLists.txt rename to src/Nav/cprt_planner_plugins/CMakeLists.txt diff --git a/src/nav/cprt_planner_plugins/LICENSE b/src/Nav/cprt_planner_plugins/LICENSE similarity index 100% rename from src/nav/cprt_planner_plugins/LICENSE rename to src/Nav/cprt_planner_plugins/LICENSE diff --git a/src/nav/cprt_planner_plugins/global_planner_plugin.xml b/src/Nav/cprt_planner_plugins/global_planner_plugin.xml similarity index 100% rename from src/nav/cprt_planner_plugins/global_planner_plugin.xml rename to src/Nav/cprt_planner_plugins/global_planner_plugin.xml diff --git a/src/nav/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/nav/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/nav/cprt_planner_plugins/package.xml b/src/Nav/cprt_planner_plugins/package.xml similarity index 100% rename from src/nav/cprt_planner_plugins/package.xml rename to src/Nav/cprt_planner_plugins/package.xml diff --git a/src/nav/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/nav/cprt_planner_plugins/src/event_horizon_planner.cpp rename to src/Nav/cprt_planner_plugins/src/event_horizon_planner.cpp diff --git a/src/nav/elevation_mapping/CMakeLists.txt b/src/Nav/elevation_mapping/CMakeLists.txt similarity index 100% rename from src/nav/elevation_mapping/CMakeLists.txt rename to src/Nav/elevation_mapping/CMakeLists.txt diff --git a/src/nav/elevation_mapping/README.md b/src/Nav/elevation_mapping/README.md similarity index 100% rename from src/nav/elevation_mapping/README.md rename to src/Nav/elevation_mapping/README.md diff --git a/src/nav/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/nav/elevation_mapping/config/elevation_maps/long_range.yaml rename to src/Nav/elevation_mapping/config/elevation_maps/long_range.yaml diff --git a/src/nav/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/nav/elevation_mapping/config/elevation_maps/remove_object.yaml rename to src/Nav/elevation_mapping/config/elevation_maps/remove_object.yaml diff --git a/src/nav/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/nav/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/nav/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/nav/elevation_mapping/config/elevation_maps/starleth_map.yaml rename to src/Nav/elevation_mapping/config/elevation_maps/starleth_map.yaml diff --git a/src/nav/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/nav/elevation_mapping/config/elevation_maps/zed2i_map.yaml rename to src/Nav/elevation_mapping/config/elevation_maps/zed2i_map.yaml diff --git a/src/nav/elevation_mapping/config/file_path.yaml b/src/Nav/elevation_mapping/config/file_path.yaml similarity index 100% rename from src/nav/elevation_mapping/config/file_path.yaml rename to src/Nav/elevation_mapping/config/file_path.yaml diff --git a/src/nav/elevation_mapping/config/postprocessing/postprocessor_pipeline.yaml b/src/Nav/elevation_mapping/config/postprocessing/postprocessor_pipeline.yaml similarity index 100% rename from src/nav/elevation_mapping/config/postprocessing/postprocessor_pipeline.yaml rename to src/Nav/elevation_mapping/config/postprocessing/postprocessor_pipeline.yaml diff --git a/src/nav/elevation_mapping/config/postprocessing/traversability_pipeline.yaml b/src/Nav/elevation_mapping/config/postprocessing/traversability_pipeline.yaml similarity index 100% rename from src/nav/elevation_mapping/config/postprocessing/traversability_pipeline.yaml rename to src/Nav/elevation_mapping/config/postprocessing/traversability_pipeline.yaml diff --git a/src/nav/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/nav/elevation_mapping/config/robots/ground_truth_demo.yaml rename to src/Nav/elevation_mapping/config/robots/ground_truth_demo.yaml diff --git a/src/nav/elevation_mapping/config/robots/remove_object.yaml b/src/Nav/elevation_mapping/config/robots/remove_object.yaml similarity index 100% rename from src/nav/elevation_mapping/config/robots/remove_object.yaml rename to src/Nav/elevation_mapping/config/robots/remove_object.yaml diff --git a/src/nav/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/nav/elevation_mapping/config/robots/simple_demo_robot.yaml rename to src/Nav/elevation_mapping/config/robots/simple_demo_robot.yaml diff --git a/src/nav/elevation_mapping/config/robots/starleth.yaml b/src/Nav/elevation_mapping/config/robots/starleth.yaml similarity index 100% rename from src/nav/elevation_mapping/config/robots/starleth.yaml rename to src/Nav/elevation_mapping/config/robots/starleth.yaml diff --git a/src/nav/elevation_mapping/config/robots/waffle_robot.yaml b/src/Nav/elevation_mapping/config/robots/waffle_robot.yaml similarity index 100% rename from src/nav/elevation_mapping/config/robots/waffle_robot.yaml rename to src/Nav/elevation_mapping/config/robots/waffle_robot.yaml diff --git a/src/nav/elevation_mapping/config/robots/zed2i_robot.yaml b/src/Nav/elevation_mapping/config/robots/zed2i_robot.yaml similarity index 100% rename from src/nav/elevation_mapping/config/robots/zed2i_robot.yaml rename to src/Nav/elevation_mapping/config/robots/zed2i_robot.yaml diff --git a/src/nav/elevation_mapping/config/rosconsole.conf b/src/Nav/elevation_mapping/config/rosconsole.conf similarity index 100% rename from src/nav/elevation_mapping/config/rosconsole.conf rename to src/Nav/elevation_mapping/config/rosconsole.conf diff --git a/src/nav/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/nav/elevation_mapping/config/sensor_processors/RSLidar-32.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/RSLidar-32.yaml diff --git a/src/nav/elevation_mapping/config/sensor_processors/aslam.yaml b/src/Nav/elevation_mapping/config/sensor_processors/aslam.yaml similarity index 100% rename from src/nav/elevation_mapping/config/sensor_processors/aslam.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/aslam.yaml diff --git a/src/nav/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/nav/elevation_mapping/config/sensor_processors/fotonic_g45.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/fotonic_g45.yaml diff --git a/src/nav/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/nav/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/nav/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/nav/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/nav/elevation_mapping/config/sensor_processors/perfect.yaml b/src/Nav/elevation_mapping/config/sensor_processors/perfect.yaml similarity index 100% rename from src/nav/elevation_mapping/config/sensor_processors/perfect.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/perfect.yaml diff --git a/src/nav/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/nav/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/nav/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/nav/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/nav/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/nav/elevation_mapping/config/sensor_processors/realsense_ZR300.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/realsense_ZR300.yaml diff --git a/src/nav/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/nav/elevation_mapping/config/sensor_processors/realsense_d435.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/realsense_d435.yaml diff --git a/src/nav/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/nav/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/nav/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/nav/elevation_mapping/config/sensor_processors/zed2i_processor.yaml rename to src/Nav/elevation_mapping/config/sensor_processors/zed2i_processor.yaml diff --git a/src/nav/elevation_mapping/config/visualization/fused.yaml b/src/Nav/elevation_mapping/config/visualization/fused.yaml similarity index 100% rename from src/nav/elevation_mapping/config/visualization/fused.yaml rename to src/Nav/elevation_mapping/config/visualization/fused.yaml diff --git a/src/nav/elevation_mapping/config/visualization/raw.yaml b/src/Nav/elevation_mapping/config/visualization/raw.yaml similarity index 100% rename from src/nav/elevation_mapping/config/visualization/raw.yaml rename to src/Nav/elevation_mapping/config/visualization/raw.yaml diff --git a/src/nav/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/nav/elevation_mapping/config/zed_configs/common_stereo.yaml rename to src/Nav/elevation_mapping/config/zed_configs/common_stereo.yaml diff --git a/src/nav/elevation_mapping/config/zed_configs/zed2i.yaml b/src/Nav/elevation_mapping/config/zed_configs/zed2i.yaml similarity index 100% rename from src/nav/elevation_mapping/config/zed_configs/zed2i.yaml rename to src/Nav/elevation_mapping/config/zed_configs/zed2i.yaml diff --git a/src/nav/elevation_mapping/docker/.gitignore b/src/Nav/elevation_mapping/docker/.gitignore similarity index 100% rename from src/nav/elevation_mapping/docker/.gitignore rename to src/Nav/elevation_mapping/docker/.gitignore diff --git a/src/nav/elevation_mapping/docker/Dockerfile.elevation_mapping b/src/Nav/elevation_mapping/docker/Dockerfile.elevation_mapping similarity index 100% rename from src/nav/elevation_mapping/docker/Dockerfile.elevation_mapping rename to src/Nav/elevation_mapping/docker/Dockerfile.elevation_mapping diff --git a/src/nav/elevation_mapping/docker/Dockerfile.ros2_humble b/src/Nav/elevation_mapping/docker/Dockerfile.ros2_humble similarity index 100% rename from src/nav/elevation_mapping/docker/Dockerfile.ros2_humble rename to src/Nav/elevation_mapping/docker/Dockerfile.ros2_humble diff --git a/src/nav/elevation_mapping/docker/Dockerfile.user b/src/Nav/elevation_mapping/docker/Dockerfile.user similarity index 100% rename from src/nav/elevation_mapping/docker/Dockerfile.user rename to src/Nav/elevation_mapping/docker/Dockerfile.user diff --git a/src/nav/elevation_mapping/docker/build_docker.sh b/src/Nav/elevation_mapping/docker/build_docker.sh similarity index 100% rename from src/nav/elevation_mapping/docker/build_docker.sh rename to src/Nav/elevation_mapping/docker/build_docker.sh diff --git a/src/nav/elevation_mapping/docker/container_scripts/.dockerignore b/src/Nav/elevation_mapping/docker/container_scripts/.dockerignore similarity index 100% rename from src/nav/elevation_mapping/docker/container_scripts/.dockerignore rename to src/Nav/elevation_mapping/docker/container_scripts/.dockerignore diff --git a/src/nav/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/nav/elevation_mapping/docker/container_scripts/workspace-entrypoint.sh rename to src/Nav/elevation_mapping/docker/container_scripts/workspace-entrypoint.sh diff --git a/src/nav/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/nav/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/nav/elevation_mapping/docker/rosdep/extra_rosdeps.yaml b/src/Nav/elevation_mapping/docker/rosdep/extra_rosdeps.yaml similarity index 100% rename from src/nav/elevation_mapping/docker/rosdep/extra_rosdeps.yaml rename to src/Nav/elevation_mapping/docker/rosdep/extra_rosdeps.yaml diff --git a/src/nav/elevation_mapping/docker/run_docker.sh b/src/Nav/elevation_mapping/docker/run_docker.sh similarity index 100% rename from src/nav/elevation_mapping/docker/run_docker.sh rename to src/Nav/elevation_mapping/docker/run_docker.sh diff --git a/src/nav/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/nav/elevation_mapping/docker/sourced_scripts/bash_functions.sh rename to src/Nav/elevation_mapping/docker/sourced_scripts/bash_functions.sh diff --git a/src/nav/elevation_mapping/docker_instructions.md b/src/Nav/elevation_mapping/docker_instructions.md similarity index 100% rename from src/nav/elevation_mapping/docker_instructions.md rename to src/Nav/elevation_mapping/docker_instructions.md diff --git a/src/nav/elevation_mapping/include/elevation_mapping/ElevationMap.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/ElevationMap.hpp similarity index 100% rename from src/nav/elevation_mapping/include/elevation_mapping/ElevationMap.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/ElevationMap.hpp diff --git a/src/nav/elevation_mapping/include/elevation_mapping/ElevationMapFunctors.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/ElevationMapFunctors.hpp similarity index 100% rename from src/nav/elevation_mapping/include/elevation_mapping/ElevationMapFunctors.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/ElevationMapFunctors.hpp diff --git a/src/nav/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp similarity index 100% rename from src/nav/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/ElevationMapping.hpp diff --git a/src/nav/elevation_mapping/include/elevation_mapping/PointXYZRGBConfidenceRatio.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/PointXYZRGBConfidenceRatio.hpp similarity index 100% rename from src/nav/elevation_mapping/include/elevation_mapping/PointXYZRGBConfidenceRatio.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/PointXYZRGBConfidenceRatio.hpp diff --git a/src/nav/elevation_mapping/include/elevation_mapping/RobotMotionMapUpdater.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/RobotMotionMapUpdater.hpp similarity index 100% rename from src/nav/elevation_mapping/include/elevation_mapping/RobotMotionMapUpdater.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/RobotMotionMapUpdater.hpp diff --git a/src/nav/elevation_mapping/include/elevation_mapping/ThreadSafeDataWrapper.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/ThreadSafeDataWrapper.hpp similarity index 100% rename from src/nav/elevation_mapping/include/elevation_mapping/ThreadSafeDataWrapper.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/ThreadSafeDataWrapper.hpp diff --git a/src/nav/elevation_mapping/include/elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp b/src/Nav/elevation_mapping/include/elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp similarity index 100% rename from src/nav/elevation_mapping/include/elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp diff --git a/src/nav/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/nav/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/nav/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/nav/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/nav/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/nav/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/nav/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/nav/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingPipelineFunctor.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingPipelineFunctor.hpp diff --git a/src/nav/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/nav/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingWorker.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingWorker.hpp diff --git a/src/nav/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/nav/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessorPool.hpp rename to src/Nav/elevation_mapping/include/elevation_mapping/postprocessing/PostprocessorPool.hpp diff --git a/src/nav/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/nav/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/nav/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/nav/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/nav/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/nav/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/nav/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/nav/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/nav/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/nav/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/nav/elevation_mapping/launch/elevationMapping_launch.py b/src/Nav/elevation_mapping/launch/elevationMapping_launch.py similarity index 100% rename from src/nav/elevation_mapping/launch/elevationMapping_launch.py rename to src/Nav/elevation_mapping/launch/elevationMapping_launch.py diff --git a/src/nav/elevation_mapping/launch/rviz.launch.py b/src/Nav/elevation_mapping/launch/rviz.launch.py similarity index 100% rename from src/nav/elevation_mapping/launch/rviz.launch.py rename to src/Nav/elevation_mapping/launch/rviz.launch.py diff --git a/src/nav/elevation_mapping/launch/visualization.launch.py b/src/Nav/elevation_mapping/launch/visualization.launch.py similarity index 100% rename from src/nav/elevation_mapping/launch/visualization.launch.py rename to src/Nav/elevation_mapping/launch/visualization.launch.py diff --git a/src/nav/elevation_mapping/launch/zed2i.launch.py b/src/Nav/elevation_mapping/launch/zed2i.launch.py similarity index 100% rename from src/nav/elevation_mapping/launch/zed2i.launch.py rename to src/Nav/elevation_mapping/launch/zed2i.launch.py diff --git a/src/nav/elevation_mapping/package.xml b/src/Nav/elevation_mapping/package.xml similarity index 100% rename from src/nav/elevation_mapping/package.xml rename to src/Nav/elevation_mapping/package.xml diff --git a/src/nav/elevation_mapping/rviz2/custom_rviz2.rviz b/src/Nav/elevation_mapping/rviz2/custom_rviz2.rviz similarity index 100% rename from src/nav/elevation_mapping/rviz2/custom_rviz2.rviz rename to src/Nav/elevation_mapping/rviz2/custom_rviz2.rviz diff --git a/src/nav/elevation_mapping/rviz2/map.rviz b/src/Nav/elevation_mapping/rviz2/map.rviz similarity index 100% rename from src/nav/elevation_mapping/rviz2/map.rviz rename to src/Nav/elevation_mapping/rviz2/map.rviz diff --git a/src/nav/elevation_mapping/rviz2/viz.rviz b/src/Nav/elevation_mapping/rviz2/viz.rviz similarity index 100% rename from src/nav/elevation_mapping/rviz2/viz.rviz rename to src/Nav/elevation_mapping/rviz2/viz.rviz diff --git a/src/nav/elevation_mapping/rviz2/zed2i.rviz b/src/Nav/elevation_mapping/rviz2/zed2i.rviz similarity index 100% rename from src/nav/elevation_mapping/rviz2/zed2i.rviz rename to src/Nav/elevation_mapping/rviz2/zed2i.rviz diff --git a/src/nav/elevation_mapping/sample_data/maze.ply b/src/Nav/elevation_mapping/sample_data/maze.ply similarity index 100% rename from src/nav/elevation_mapping/sample_data/maze.ply rename to src/Nav/elevation_mapping/sample_data/maze.ply diff --git a/src/nav/elevation_mapping/src/ElevationMap.cpp b/src/Nav/elevation_mapping/src/ElevationMap.cpp similarity index 100% rename from src/nav/elevation_mapping/src/ElevationMap.cpp rename to src/Nav/elevation_mapping/src/ElevationMap.cpp diff --git a/src/nav/elevation_mapping/src/ElevationMapping.cpp b/src/Nav/elevation_mapping/src/ElevationMapping.cpp similarity index 100% rename from src/nav/elevation_mapping/src/ElevationMapping.cpp rename to src/Nav/elevation_mapping/src/ElevationMapping.cpp diff --git a/src/nav/elevation_mapping/src/PointXYZRGBConfidenceRatio.cpp b/src/Nav/elevation_mapping/src/PointXYZRGBConfidenceRatio.cpp similarity index 100% rename from src/nav/elevation_mapping/src/PointXYZRGBConfidenceRatio.cpp rename to src/Nav/elevation_mapping/src/PointXYZRGBConfidenceRatio.cpp diff --git a/src/nav/elevation_mapping/src/RobotMotionMapUpdater.cpp b/src/Nav/elevation_mapping/src/RobotMotionMapUpdater.cpp similarity index 100% rename from src/nav/elevation_mapping/src/RobotMotionMapUpdater.cpp rename to src/Nav/elevation_mapping/src/RobotMotionMapUpdater.cpp diff --git a/src/nav/elevation_mapping/src/elevation_mapping_node.cpp b/src/Nav/elevation_mapping/src/elevation_mapping_node.cpp similarity index 100% rename from src/nav/elevation_mapping/src/elevation_mapping_node.cpp rename to src/Nav/elevation_mapping/src/elevation_mapping_node.cpp diff --git a/src/nav/elevation_mapping/src/input_sources/Input.cpp b/src/Nav/elevation_mapping/src/input_sources/Input.cpp similarity index 100% rename from src/nav/elevation_mapping/src/input_sources/Input.cpp rename to src/Nav/elevation_mapping/src/input_sources/Input.cpp diff --git a/src/nav/elevation_mapping/src/input_sources/InputSourceManager.cpp b/src/Nav/elevation_mapping/src/input_sources/InputSourceManager.cpp similarity index 100% rename from src/nav/elevation_mapping/src/input_sources/InputSourceManager.cpp rename to src/Nav/elevation_mapping/src/input_sources/InputSourceManager.cpp diff --git a/src/nav/elevation_mapping/src/postprocessing/PostprocessingPipelineFunctor.cpp b/src/Nav/elevation_mapping/src/postprocessing/PostprocessingPipelineFunctor.cpp similarity index 100% rename from src/nav/elevation_mapping/src/postprocessing/PostprocessingPipelineFunctor.cpp rename to src/Nav/elevation_mapping/src/postprocessing/PostprocessingPipelineFunctor.cpp diff --git a/src/nav/elevation_mapping/src/postprocessing/PostprocessingWorker.cpp b/src/Nav/elevation_mapping/src/postprocessing/PostprocessingWorker.cpp similarity index 100% rename from src/nav/elevation_mapping/src/postprocessing/PostprocessingWorker.cpp rename to src/Nav/elevation_mapping/src/postprocessing/PostprocessingWorker.cpp diff --git a/src/nav/elevation_mapping/src/postprocessing/PostprocessorPool.cpp b/src/Nav/elevation_mapping/src/postprocessing/PostprocessorPool.cpp similarity index 100% rename from src/nav/elevation_mapping/src/postprocessing/PostprocessorPool.cpp rename to src/Nav/elevation_mapping/src/postprocessing/PostprocessorPool.cpp diff --git a/src/nav/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp b/src/Nav/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp similarity index 100% rename from src/nav/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp rename to src/Nav/elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp diff --git a/src/nav/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp b/src/Nav/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp similarity index 100% rename from src/nav/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp rename to src/Nav/elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp diff --git a/src/nav/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp b/src/Nav/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp similarity index 100% rename from src/nav/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp rename to src/Nav/elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp diff --git a/src/nav/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp b/src/Nav/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp similarity index 100% rename from src/nav/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp rename to src/Nav/elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp diff --git a/src/nav/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp b/src/Nav/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp similarity index 100% rename from src/nav/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp rename to src/Nav/elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp diff --git a/src/nav/elevation_mapping/src/test.cpp b/src/Nav/elevation_mapping/src/test.cpp similarity index 100% rename from src/nav/elevation_mapping/src/test.cpp rename to src/Nav/elevation_mapping/src/test.cpp diff --git a/src/nav/elevation_mapping/src/test_service.cpp b/src/Nav/elevation_mapping/src/test_service.cpp similarity index 100% rename from src/nav/elevation_mapping/src/test_service.cpp rename to src/Nav/elevation_mapping/src/test_service.cpp diff --git a/src/nav/elevation_mapping/test/ElevationMapTest.cpp b/src/Nav/elevation_mapping/test/ElevationMapTest.cpp similarity index 100% rename from src/nav/elevation_mapping/test/ElevationMapTest.cpp rename to src/Nav/elevation_mapping/test/ElevationMapTest.cpp diff --git a/src/nav/elevation_mapping/test/WeightedEmpiricalCumulativeDistributionFunctionTest.cpp b/src/Nav/elevation_mapping/test/WeightedEmpiricalCumulativeDistributionFunctionTest.cpp similarity index 100% rename from src/nav/elevation_mapping/test/WeightedEmpiricalCumulativeDistributionFunctionTest.cpp rename to src/Nav/elevation_mapping/test/WeightedEmpiricalCumulativeDistributionFunctionTest.cpp diff --git a/src/nav/elevation_mapping/test/elevation_mapping.test b/src/Nav/elevation_mapping/test/elevation_mapping.test similarity index 100% rename from src/nav/elevation_mapping/test/elevation_mapping.test rename to src/Nav/elevation_mapping/test/elevation_mapping.test diff --git a/src/nav/elevation_mapping/test/input_sources/InputSourcesTest.cpp b/src/Nav/elevation_mapping/test/input_sources/InputSourcesTest.cpp similarity index 100% rename from src/nav/elevation_mapping/test/input_sources/InputSourcesTest.cpp rename to src/Nav/elevation_mapping/test/input_sources/InputSourcesTest.cpp diff --git a/src/nav/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/nav/elevation_mapping/test/input_sources/config/TestConfigurations.yaml rename to src/Nav/elevation_mapping/test/input_sources/config/TestConfigurations.yaml diff --git a/src/nav/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/nav/elevation_mapping/test/input_sources/config/rosconsole.config rename to src/Nav/elevation_mapping/test/input_sources/config/rosconsole.config diff --git a/src/nav/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/nav/elevation_mapping/test/input_sources/input_sources.test rename to src/Nav/elevation_mapping/test/input_sources/input_sources.test diff --git a/src/nav/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/nav/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/nav/elevation_mapping/test/postprocessing/PostprocessorTest.cpp b/src/Nav/elevation_mapping/test/postprocessing/PostprocessorTest.cpp similarity index 100% rename from src/nav/elevation_mapping/test/postprocessing/PostprocessorTest.cpp rename to src/Nav/elevation_mapping/test/postprocessing/PostprocessorTest.cpp diff --git a/src/nav/elevation_mapping/test/postprocessing/main.cpp b/src/Nav/elevation_mapping/test/postprocessing/main.cpp similarity index 100% rename from src/nav/elevation_mapping/test/postprocessing/main.cpp rename to src/Nav/elevation_mapping/test/postprocessing/main.cpp diff --git a/src/nav/elevation_mapping/test/postprocessing/postprocessor.test b/src/Nav/elevation_mapping/test/postprocessing/postprocessor.test similarity index 100% rename from src/nav/elevation_mapping/test/postprocessing/postprocessor.test rename to src/Nav/elevation_mapping/test/postprocessing/postprocessor.test diff --git a/src/nav/elevation_mapping/test/postprocessing/postprocessor_pipeline.yaml b/src/Nav/elevation_mapping/test/postprocessing/postprocessor_pipeline.yaml similarity index 100% rename from src/nav/elevation_mapping/test/postprocessing/postprocessor_pipeline.yaml rename to src/Nav/elevation_mapping/test/postprocessing/postprocessor_pipeline.yaml diff --git a/src/nav/elevation_mapping/test/test_elevation_mapping.cpp b/src/Nav/elevation_mapping/test/test_elevation_mapping.cpp similarity index 100% rename from src/nav/elevation_mapping/test/test_elevation_mapping.cpp rename to src/Nav/elevation_mapping/test/test_elevation_mapping.cpp diff --git a/src/nav/gps/config/Heading_config.ubx b/src/Nav/gps/config/Heading_config.ubx similarity index 100% rename from src/nav/gps/config/Heading_config.ubx rename to src/Nav/gps/config/Heading_config.ubx diff --git a/src/nav/gps/config/Rover_config.ubx b/src/Nav/gps/config/Rover_config.ubx similarity index 100% rename from src/nav/gps/config/Rover_config.ubx rename to src/Nav/gps/config/Rover_config.ubx diff --git a/src/nav/gps/config/gps.yaml b/src/Nav/gps/config/gps.yaml similarity index 100% rename from src/nav/gps/config/gps.yaml rename to src/Nav/gps/config/gps.yaml diff --git a/src/nav/localization/localization/__init__.py b/src/Nav/gps/gps/__init__.py similarity index 100% rename from src/nav/localization/localization/__init__.py rename to src/Nav/gps/gps/__init__.py diff --git a/src/nav/gps/gps/heading_pub_node.py b/src/Nav/gps/gps/heading_pub_node.py similarity index 100% rename from src/nav/gps/gps/heading_pub_node.py rename to src/Nav/gps/gps/heading_pub_node.py diff --git a/src/nav/gps/gps/rtcm_pub_node.py b/src/Nav/gps/gps/rtcm_pub_node.py similarity index 100% rename from src/nav/gps/gps/rtcm_pub_node.py rename to src/Nav/gps/gps/rtcm_pub_node.py diff --git a/src/nav/gps/gps/ubx_io_manager.py b/src/Nav/gps/gps/ubx_io_manager.py similarity index 100% rename from src/nav/gps/gps/ubx_io_manager.py rename to src/Nav/gps/gps/ubx_io_manager.py diff --git a/src/nav/gps/launch/basestation.launch.py b/src/Nav/gps/launch/basestation.launch.py similarity index 100% rename from src/nav/gps/launch/basestation.launch.py rename to src/Nav/gps/launch/basestation.launch.py diff --git a/src/nav/gps/launch/heading.launch.py b/src/Nav/gps/launch/heading.launch.py similarity index 100% rename from src/nav/gps/launch/heading.launch.py rename to src/Nav/gps/launch/heading.launch.py diff --git a/src/nav/gps/launch/rover.launch.py b/src/Nav/gps/launch/rover.launch.py similarity index 100% rename from src/nav/gps/launch/rover.launch.py rename to src/Nav/gps/launch/rover.launch.py diff --git a/src/nav/gps/package.xml b/src/Nav/gps/package.xml similarity index 100% rename from src/nav/gps/package.xml rename to src/Nav/gps/package.xml diff --git a/src/nav/gps/resource/gps b/src/Nav/gps/resource/gps similarity index 100% rename from src/nav/gps/resource/gps rename to src/Nav/gps/resource/gps diff --git a/src/nav/gps/setup.cfg b/src/Nav/gps/setup.cfg similarity index 100% rename from src/nav/gps/setup.cfg rename to src/Nav/gps/setup.cfg diff --git a/src/nav/gps/setup.py b/src/Nav/gps/setup.py similarity index 100% rename from src/nav/gps/setup.py rename to src/Nav/gps/setup.py diff --git a/src/nav/nav_commanders/test/test_copyright.py b/src/Nav/gps/test/test_copyright.py similarity index 100% rename from src/nav/nav_commanders/test/test_copyright.py rename to src/Nav/gps/test/test_copyright.py diff --git a/src/nav/nav_commanders/test/test_flake8.py b/src/Nav/gps/test/test_flake8.py similarity index 100% rename from src/nav/nav_commanders/test/test_flake8.py rename to src/Nav/gps/test/test_flake8.py diff --git a/src/nav/nav_commanders/test/test_pep257.py b/src/Nav/gps/test/test_pep257.py similarity index 100% rename from src/nav/nav_commanders/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 similarity index 100% rename from src/nav/kindr_ros/.gitignore rename to src/Nav/kindr_ros/.gitignore diff --git a/src/nav/kindr_ros/LICENSE b/src/Nav/kindr_ros/LICENSE similarity index 100% rename from src/nav/kindr_ros/LICENSE rename to src/Nav/kindr_ros/LICENSE diff --git a/src/nav/kindr_ros/README.md b/src/Nav/kindr_ros/README.md similarity index 100% rename from src/nav/kindr_ros/README.md rename to src/Nav/kindr_ros/README.md diff --git a/src/nav/kindr_ros/kindr_msgs/CMakeLists.txt b/src/Nav/kindr_ros/kindr_msgs/CMakeLists.txt similarity index 100% rename from src/nav/kindr_ros/kindr_msgs/CMakeLists.txt rename to src/Nav/kindr_ros/kindr_msgs/CMakeLists.txt diff --git a/src/nav/kindr_ros/kindr_msgs/msg/VectorAtPosition.msg b/src/Nav/kindr_ros/kindr_msgs/msg/VectorAtPosition.msg similarity index 100% rename from src/nav/kindr_ros/kindr_msgs/msg/VectorAtPosition.msg rename to src/Nav/kindr_ros/kindr_msgs/msg/VectorAtPosition.msg diff --git a/src/nav/kindr_ros/kindr_msgs/package.xml b/src/Nav/kindr_ros/kindr_msgs/package.xml similarity index 100% rename from src/nav/kindr_ros/kindr_msgs/package.xml rename to src/Nav/kindr_ros/kindr_msgs/package.xml diff --git a/src/nav/kindr_ros/kindr_ros/CMakeLists.txt b/src/Nav/kindr_ros/kindr_ros/CMakeLists.txt similarity index 100% rename from src/nav/kindr_ros/kindr_ros/CMakeLists.txt rename to src/Nav/kindr_ros/kindr_ros/CMakeLists.txt 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 similarity index 59% rename from src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPhysicalQuantities.hpp rename to src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPhysicalQuantities.hpp index 4f4ada37..2e272b39 100644 --- a/src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPhysicalQuantities.hpp +++ b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPhysicalQuantities.hpp @@ -1,6 +1,6 @@ /* - * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm - * All rights reserved. + * 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: @@ -13,18 +13,19 @@ * 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. + * 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_ @@ -33,76 +34,66 @@ #include // ros -#include #include #include +#include +namespace kindr_ros { -namespace kindr_ros -{ - - -template +template inline static void convertFromRosGeometryMsg( - const geometry_msgs::msg::Vector3 & geometryVector3Msg, - kindr::Vector & vector) -{ + 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 +template inline static void convertToRosGeometryMsg( - const kindr::Vector & vector, - geometry_msgs::msg::Vector3 & geometryVector3Msg) -{ + 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 +template inline static void convertFromRosGeometryMsg( - const geometry_msgs::msg::Point32 & geometryPoint32Msg, - kindr::Position & position) -{ + 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 +template inline static void convertToRosGeometryMsg( - const kindr::Position & position, - geometry_msgs::msg::Point32 & geometryPoint32Msg) -{ + 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 +template inline static void convertFromRosGeometryMsg( - const geometry_msgs::msg::Point & geometryPointMsg, - kindr::Position & position) -{ + 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 +template inline static void convertToRosGeometryMsg( - const kindr::Position & position, - geometry_msgs::msg::Point & geometryPointMsg) -{ + 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 similarity index 53% rename from src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPose.hpp rename to src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPose.hpp index 0e64d3f0..339ae118 100644 --- a/src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPose.hpp +++ b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgPose.hpp @@ -1,6 +1,6 @@ /* - * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm - * All rights reserved. + * 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: @@ -13,18 +13,19 @@ * 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. + * 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_ @@ -40,16 +41,12 @@ #include "kindr_ros/RosGeometryMsgPhysicalQuantities.hpp" #include "kindr_ros/RosGeometryMsgRotation.hpp" +namespace kindr_ros { -namespace kindr_ros -{ - - -template +template inline static void convertFromRosGeometryMsg( - const geometry_msgs::msg::Pose & geometryPoseMsg, - kindr::HomogeneousTransformation & pose) -{ + const geometry_msgs::msg::Pose& geometryPoseMsg, + kindr::HomogeneousTransformation& pose) { convertFromRosGeometryMsg(geometryPoseMsg.position, pose.getPosition()); // This is the definition of ROS Geometry pose. @@ -60,11 +57,11 @@ inline static void convertFromRosGeometryMsg( pose.getRotation() = rotation; } -template +template inline static void convertToRosGeometryMsg( - const kindr::HomogeneousTransformation & pose, - geometry_msgs::msg::Pose & geometryPoseMsg) -{ + const kindr::HomogeneousTransformation& + pose, + geometry_msgs::msg::Pose& geometryPoseMsg) { convertToRosGeometryMsg(pose.getPosition(), geometryPoseMsg.position); // This is the definition of ROS Geometry pose. @@ -74,36 +71,40 @@ inline static void convertToRosGeometryMsg( convertToRosGeometryMsg(rotation, geometryPoseMsg.orientation); } -template +template inline static void convertFromRosGeometryMsg( - const geometry_msgs::msg::Transform & geometryTransformMsg, - kindr::HomogeneousTransformation & transformation) -{ - convertFromRosGeometryMsg(geometryTransformMsg.translation, transformation.getPosition()); + 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; + typedef kindr::RotationQuaternion + RotationQuaternionGeometryTransformLike; RotationQuaternionGeometryTransformLike rotation; convertFromRosGeometryMsg(geometryTransformMsg.rotation, rotation); transformation.getRotation() = rotation; } -template +template inline static void convertToRosGeometryMsg( - const kindr::HomogeneousTransformation & transformation, - geometry_msgs::msg::Transform & geometryTransformMsg) -{ - convertToRosGeometryMsg(transformation.getPosition(), geometryTransformMsg.translation); + 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; + typedef kindr::RotationQuaternion + RotationQuaternionGeometryTransformLike; - RotationQuaternionGeometryTransformLike rotation(transformation.getRotation()); + 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 similarity index 50% rename from src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgRotation.hpp rename to src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgRotation.hpp index 0ac0ddee..46afefa3 100644 --- a/src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgRotation.hpp +++ b/src/Nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgRotation.hpp @@ -1,6 +1,6 @@ /* - * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm - * All rights reserved. + * 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: @@ -13,18 +13,19 @@ * 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. + * 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_ @@ -35,28 +36,22 @@ // ros #include +namespace kindr_ros { -namespace kindr_ros -{ - - -template +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)); + 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 +template inline static void convertToRosGeometryMsg( - const kindr::RotationQuaternion & rotationQuaternion, - geometry_msgs::msg::Quaternion & geometryQuaternionMsg) -{ + 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()); 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 similarity index 61% rename from src/nav/kindr_ros/kindr_ros/include/kindr_ros/kindr_ros.hpp rename to src/Nav/kindr_ros/kindr_ros/include/kindr_ros/kindr_ros.hpp index d5789383..84a73492 100644 --- 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 @@ -1,6 +1,6 @@ /* - * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm - * All rights reserved. + * 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: @@ -13,18 +13,19 @@ * 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. + * 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 * @@ -37,8 +38,8 @@ #include "kindr_ros/RosGeometryMsgPhysicalQuantities.hpp" #include "kindr_ros/RosGeometryMsgPose.hpp" -#include "kindr_ros/RosGeometryMsgTwist.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 similarity index 100% rename from src/nav/kindr_ros/kindr_ros/package.xml rename to src/Nav/kindr_ros/kindr_ros/package.xml diff --git a/src/nav/kindr_ros/kindr_ros/test/CMakeLists.txt b/src/Nav/kindr_ros/kindr_ros/test/CMakeLists.txt similarity index 100% rename from src/nav/kindr_ros/kindr_ros/test/CMakeLists.txt rename to src/Nav/kindr_ros/kindr_ros/test/CMakeLists.txt diff --git a/src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgPhysicalQuantitiesTest.cpp b/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgPhysicalQuantitiesTest.cpp similarity index 71% rename from src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgPhysicalQuantitiesTest.cpp rename to src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgPhysicalQuantitiesTest.cpp index 259a904f..7b1716cb 100644 --- a/src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgPhysicalQuantitiesTest.cpp +++ b/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgPhysicalQuantitiesTest.cpp @@ -1,6 +1,6 @@ /* - * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm - * All rights reserved. + * 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: @@ -13,33 +13,31 @@ * 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. + * 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 "kindr_ros/RosGeometryMsgPhysicalQuantities.hpp" -TEST(RosGeometryMsgVectorEigen, convertFromRosGeometryMsg) -{ +TEST(RosGeometryMsgVectorEigen, convertFromRosGeometryMsg) { const kindr::VectorTypeless3D referenceVector(-9.12, -25.5, 0.6); geometry_msgs::msg::Vector3 geometryVector3Msg; @@ -50,13 +48,12 @@ TEST(RosGeometryMsgVectorEigen, convertFromRosGeometryMsg) kindr::VectorTypeless3D vector; kindr_ros::convertFromRosGeometryMsg(geometryVector3Msg, vector); - kindr::expectNear( - vector.toImplementation(), - referenceVector.toImplementation(), 1e-8, KINDR_SOURCE_FILE_POS); + kindr::expectNear(vector.toImplementation(), + referenceVector.toImplementation(), 1e-8, + KINDR_SOURCE_FILE_POS); } -TEST(RosGeometryMsgVectorEigen, convertToRosGeometryMsg) -{ +TEST(RosGeometryMsgVectorEigen, convertToRosGeometryMsg) { const kindr::VectorTypeless3D referenceVector(0.3, -1.1, -0.6); kindr::VectorTypeless3D vector(referenceVector); @@ -69,8 +66,7 @@ TEST(RosGeometryMsgVectorEigen, convertToRosGeometryMsg) EXPECT_NEAR(geometryVectorMsg.z, referenceVector.z(), 1e-8); } -TEST(RosGeometryMsgPositionEigen, convertFromRosGeometryPoint32Msg) -{ +TEST(RosGeometryMsgPositionEigen, convertFromRosGeometryPoint32Msg) { const kindr::Position3F referenceTranslation(-9.3, 2.5, 5.6); geometry_msgs::msg::Point32 geometryPointMsg; @@ -81,13 +77,12 @@ TEST(RosGeometryMsgPositionEigen, convertFromRosGeometryPoint32Msg) kindr::Position3F position; kindr_ros::convertFromRosGeometryMsg(geometryPointMsg, position); - kindr::expectNear( - position.toImplementation(), - referenceTranslation.toImplementation(), 1e-8, KINDR_SOURCE_FILE_POS); + kindr::expectNear(position.toImplementation(), + referenceTranslation.toImplementation(), 1e-8, + KINDR_SOURCE_FILE_POS); } -TEST(RosGeometryMsgPositionEigen, convertToRosGeometryPoint32Msg) -{ +TEST(RosGeometryMsgPositionEigen, convertToRosGeometryPoint32Msg) { const kindr::Position3F referenceTranslation(0.3, -1.1, -0.6); kindr::Position3F position(referenceTranslation); @@ -100,8 +95,7 @@ TEST(RosGeometryMsgPositionEigen, convertToRosGeometryPoint32Msg) EXPECT_NEAR(geometryPointMsg.z, referenceTranslation.z(), 1e-8); } -TEST(RosGeometryMsgPositionEigen, convertFromRosGeometryPointMsg) -{ +TEST(RosGeometryMsgPositionEigen, convertFromRosGeometryPointMsg) { const kindr::Position3D referenceTranslation(19.3, -2.5, 5.6); geometry_msgs::msg::Point geometryPointMsg; @@ -112,13 +106,12 @@ TEST(RosGeometryMsgPositionEigen, convertFromRosGeometryPointMsg) kindr::Position3D position; kindr_ros::convertFromRosGeometryMsg(geometryPointMsg, position); - kindr::expectNear( - position.toImplementation(), - referenceTranslation.toImplementation(), 1e-8, KINDR_SOURCE_FILE_POS); + kindr::expectNear(position.toImplementation(), + referenceTranslation.toImplementation(), 1e-8, + KINDR_SOURCE_FILE_POS); } -TEST(RosGeometryMsgPositionEigen, convertToRosGeometryPointMsg) -{ +TEST(RosGeometryMsgPositionEigen, convertToRosGeometryPointMsg) { const kindr::Position3D referenceTranslation(0.3, -1.1, -0.6); kindr::Position3D position(referenceTranslation); diff --git a/src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgPoseTest.cpp b/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgPoseTest.cpp similarity index 73% rename from src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgPoseTest.cpp rename to src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgPoseTest.cpp index 8fdb9d7a..af60b583 100644 --- a/src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgPoseTest.cpp +++ b/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgPoseTest.cpp @@ -1,6 +1,6 @@ /* - * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm - * All rights reserved. + * 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: @@ -13,37 +13,36 @@ * 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. + * 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 +#include #include "kindr_ros/RosGeometryMsgPose.hpp" - -TEST(RosGeometryMsgPoseEigen, convertFromRosGeometryMsg) -{ +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); + const kindr::RotationQuaternionPD referenceQuaternion(0.113, 0.071, -0.924, + 0.35835); geometry_msgs::msg::Pose geometryPoseMsg; geometryPoseMsg.orientation.x = referenceQuaternion.x(); @@ -57,16 +56,16 @@ TEST(RosGeometryMsgPoseEigen, convertFromRosGeometryMsg) kindr::HomogeneousTransformationPosition3RotationQuaternionD pose; kindr_ros::convertFromRosGeometryMsg(geometryPoseMsg, pose); - kindr::expectNear( - pose.getPosition().toImplementation(), referenceTranslation.toImplementation(), 1e-4, - KINDR_SOURCE_FILE_POS); + kindr::expectNear(pose.getPosition().toImplementation(), + referenceTranslation.toImplementation(), 1e-4, + KINDR_SOURCE_FILE_POS); EXPECT_TRUE(pose.getRotation().isNear(referenceQuaternion, 1e-8)); } -TEST(RosGeometryMsgPoseEigen, convertToRosGeometryMsg) -{ +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); + const kindr::RotationQuaternionPD referenceQuaternion(0.212, 0.0421, -0.958, + 0.1885); kindr::HomogeneousTransformationPosition3RotationQuaternionD pose; pose.getPosition() = referenceTranslation; @@ -84,10 +83,10 @@ TEST(RosGeometryMsgPoseEigen, convertToRosGeometryMsg) EXPECT_NEAR(geometryPoseMsg.position.z, referenceTranslation.z(), 1e-8); } -TEST(RosGeometryMsgTransformationEigen, convertFromRosGeometryMsg) -{ +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); + const kindr::RotationQuaternionPD referenceQuaternion(0.949, 0.133, 0.169, + 0.230541); geometry_msgs::msg::Transform geometryTransformMsg; geometryTransformMsg.rotation.x = referenceQuaternion.x(); @@ -101,16 +100,16 @@ TEST(RosGeometryMsgTransformationEigen, convertFromRosGeometryMsg) kindr::HomogeneousTransformationPosition3RotationQuaternionD transformation; kindr_ros::convertFromRosGeometryMsg(geometryTransformMsg, transformation); - kindr::expectNear( - transformation.getPosition().toImplementation(), referenceTranslation.toImplementation(), 1e-4, - KINDR_SOURCE_FILE_POS); + kindr::expectNear(transformation.getPosition().toImplementation(), + referenceTranslation.toImplementation(), 1e-4, + KINDR_SOURCE_FILE_POS); EXPECT_TRUE(transformation.getRotation().isNear(referenceQuaternion, 1e-8)); } -TEST(RosGeometryMsgTransformationEigen, convertToRosGeometryMsg) -{ +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); + const kindr::RotationQuaternionPD referenceQuaternion(0.212, 0.0421, -0.958, + 0.1885); kindr::HomogeneousTransformationPosition3RotationQuaternionD transformation; transformation.getPosition() = referenceTranslation; @@ -123,7 +122,10 @@ TEST(RosGeometryMsgTransformationEigen, convertToRosGeometryMsg) 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); + 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 similarity index 66% rename from src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgRotationTest.cpp rename to src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgRotationTest.cpp index 523844b0..ec189fe2 100644 --- a/src/nav/kindr_ros/kindr_ros/test/RosGeometryMsgRotationTest.cpp +++ b/src/Nav/kindr_ros/kindr_ros/test/RosGeometryMsgRotationTest.cpp @@ -1,6 +1,6 @@ /* - * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm - * All rights reserved. + * 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: @@ -13,32 +13,32 @@ * 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. + * 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/RosGeometryMsgRotation.hpp" -TEST(RosGeometryMsgRotationQuaternionEigen, convertFromRosGeometryMsg) -{ - const kindr::RotationQuaternionPD referenceQuaternion(0.113, 0.071, -0.924, 0.35835); +TEST(RosGeometryMsgRotationQuaternionEigen, convertFromRosGeometryMsg) { + const kindr::RotationQuaternionPD referenceQuaternion(0.113, 0.071, -0.924, + 0.35835); geometry_msgs::msg::Quaternion geometryQuaternionMsg; geometryQuaternionMsg.x = referenceQuaternion.x(); @@ -47,14 +47,15 @@ TEST(RosGeometryMsgRotationQuaternionEigen, convertFromRosGeometryMsg) geometryQuaternionMsg.w = referenceQuaternion.w(); kindr::RotationQuaternionPD rotationQuaternion; - kindr_ros::convertFromRosGeometryMsg(geometryQuaternionMsg, 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); +TEST(RosGeometryMsgRotationQuaternionEigen, convertToRosGeometryMsg) { + const kindr::RotationQuaternionPD referenceQuaternion(0.212, 0.0421, -0.958, + 0.1885); kindr::RotationQuaternionPD rotationQuaternion(referenceQuaternion); diff --git a/src/nav/kindr_ros/kindr_ros/test/RosTfPoseTest.cpp b/src/Nav/kindr_ros/kindr_ros/test/RosTfPoseTest.cpp similarity index 60% rename from src/nav/kindr_ros/kindr_ros/test/RosTfPoseTest.cpp rename to src/Nav/kindr_ros/kindr_ros/test/RosTfPoseTest.cpp index c12de0bb..b582493a 100644 --- a/src/nav/kindr_ros/kindr_ros/test/RosTfPoseTest.cpp +++ b/src/Nav/kindr_ros/kindr_ros/test/RosTfPoseTest.cpp @@ -1,6 +1,6 @@ /* - * Copyright (c) 2014, Peter Fankhauser, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm - * All rights reserved. + * 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: @@ -13,56 +13,52 @@ * 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. + * 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 #include -#include - -#include -#include #include "kindr_ros/RosTfPose.hpp" -TEST(RosTfPoseEigen, convertFromRosTf) -{ - const kindr::RotationQuaternionD referenceQuaternion(0.949, 0.133, 0.169, 0.2305); +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 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()); + 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; @@ -71,29 +67,28 @@ TEST(RosTfPoseEigen, convertFromRosTf) // 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); + 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); + 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())); + 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); @@ -104,8 +99,8 @@ TEST(RosTfPoseEigen, convertFromRosTf) 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())); + 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); @@ -117,8 +112,8 @@ TEST(RosTfPoseEigen, convertFromRosTf) tfTransform.setOrigin(referenceOriginTf); tfTransform.setBasis(referenceBasisTf); tfTransformInverse = tfTransform.inverse(); - tf2::Vector3 tfTestVectorTransformed3 = - tfTransformInverse(tf2::Vector3(testVector3.x(), testVector3.y(), testVector3.z())); + 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); @@ -129,8 +124,8 @@ TEST(RosTfPoseEigen, convertFromRosTf) 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())); + 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); @@ -141,8 +136,8 @@ TEST(RosTfPoseEigen, convertFromRosTf) 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())); + 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); @@ -153,8 +148,8 @@ TEST(RosTfPoseEigen, convertFromRosTf) 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())); + 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); @@ -162,9 +157,9 @@ TEST(RosTfPoseEigen, convertFromRosTf) EXPECT_NEAR(testVectorTransformed6.z(), tfTestVectorTransformed6.z(), 1e-8); } -TEST(RosTfPoseEigen, convertToRosTf) -{ - const kindr::RotationQuaternionD referenceQuaternion(0.113, 0.071, -0.924, 0.3585); +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); @@ -177,8 +172,8 @@ TEST(RosTfPoseEigen, convertToRosTf) 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())); + 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); @@ -189,8 +184,8 @@ TEST(RosTfPoseEigen, convertToRosTf) 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())); + 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); @@ -202,8 +197,8 @@ TEST(RosTfPoseEigen, convertToRosTf) 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())); + 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); @@ -214,8 +209,8 @@ TEST(RosTfPoseEigen, convertToRosTf) 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())); + 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); @@ -226,8 +221,8 @@ TEST(RosTfPoseEigen, convertToRosTf) 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())); + 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); @@ -238,37 +233,35 @@ TEST(RosTfPoseEigen, convertToRosTf) 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())); + 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); +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 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()); + 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; + kindr::HomogeneousTransformationPosition3RotationQuaternionD pose, + poseConverted; tf2::Transform tfTransform, tfTransformConverted; // Kindr -> TF -> Kindr @@ -276,43 +269,37 @@ TEST(RosTfPoseEigen, convertTwoWays) 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); + 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); + 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 similarity index 54% rename from src/nav/kindr_ros/kindr_ros/test/TfConventionTest.cpp rename to src/Nav/kindr_ros/kindr_ros/test/TfConventionTest.cpp index c69002c8..ecba6915 100644 --- a/src/nav/kindr_ros/kindr_ros/test/TfConventionTest.cpp +++ b/src/Nav/kindr_ros/kindr_ros/test/TfConventionTest.cpp @@ -6,53 +6,46 @@ */ #include -#include -#include +#include #include #include -#include + +#include +#include /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -namespace kindr -{ +namespace kindr { -template<> -class RotationConversion -{ +template <> +class RotationConversion { typedef tf2::Quaternion Rotation; typedef tf2::Vector3 Vector; -public: + public: inline static void convertToOtherRotation( - Rotation & out, - const kindr::RotationQuaternion & in) - { + 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) - { + 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) - { + 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) - { + 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) - { + Eigen::Matrix3d& rotationMatrix, const Rotation& quaternion) { tf2::Matrix3x3 tfRotationmatrix; tfRotationmatrix.setRotation(quaternion); for (int i = 0; i < 3; i++) { @@ -62,56 +55,49 @@ class RotationConversion } } - 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())); + 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 -{ +template <> +class RotationConversion { typedef tf2::Matrix3x3 Rotation; typedef tf2::Vector3 Vector; -public: + public: inline static void convertToOtherRotation( - Rotation & out, - const kindr::RotationQuaternion & in) - { + 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) - { + 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()); + out = kindr::RotationQuaternion(quat.w(), quat.x(), quat.y(), + quat.z()); } inline static void convertToOtherVelocityVector( - Vector & out, Rotation & rot, - const Eigen::Matrix & in) - { + 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) - { + 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) - { + 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(); @@ -119,10 +105,9 @@ class RotationConversion } } - inline static void rotateVector( - Eigen::Matrix & A_r, const Rotation & rotationBToA, - const Eigen::Matrix & B_r) - { + 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(); @@ -131,18 +116,21 @@ class RotationConversion } }; - } // namespace kindr /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST(TfConventionTest, Concatenation) { - kindr::ConventionTest::testConcatenation(); - kindr::ConventionTest::testConcatenation(); + kindr::ConventionTest::testConcatenation(); + kindr::ConventionTest::testConcatenation(); } TEST(TfConventionTest, Rotation) { - kindr::ConventionTest::testRotationMatrix(); - kindr::ConventionTest::testRotationMatrix(); + kindr::ConventionTest::testRotationMatrix(); + kindr::ConventionTest::testRotationMatrix(); } // TEST(TfConventionTest, BoxPlus) { @@ -150,6 +138,8 @@ TEST(TfConventionTest, Rotation) { //} TEST(TfConventionTest, GeometricalInterpretation) { - kindr::ConventionTest::testGeometricalInterpretation(); - kindr::ConventionTest::testGeometricalInterpretation(); + 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 similarity index 55% rename from src/nav/kindr_ros/kindr_ros/test/test_main.cpp rename to src/Nav/kindr_ros/kindr_ros/test/test_main.cpp index df19e2a2..b7473a58 100644 --- a/src/nav/kindr_ros/kindr_ros/test/test_main.cpp +++ b/src/Nav/kindr_ros/kindr_ros/test/test_main.cpp @@ -1,6 +1,6 @@ /* - * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm - * All rights reserved. + * 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: @@ -13,24 +13,23 @@ * 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. + * 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) -{ +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 similarity index 100% rename from src/nav/kindr_ros/kindr_rviz_plugins/CMakeLists.txt rename to src/Nav/kindr_ros/kindr_rviz_plugins/CMakeLists.txt diff --git a/src/nav/kindr_ros/kindr_rviz_plugins/COLCON_IGNORE b/src/Nav/kindr_ros/kindr_rviz_plugins/COLCON_IGNORE similarity index 100% rename from src/nav/kindr_ros/kindr_rviz_plugins/COLCON_IGNORE 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 similarity index 100% rename from src/nav/kindr_ros/kindr_rviz_plugins/icons/classes/VectorAtPosition.png rename to src/Nav/kindr_ros/kindr_rviz_plugins/icons/classes/VectorAtPosition.png 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 similarity index 89% rename from src/nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp rename to src/Nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp index a39796e5..3c2b6f0d 100644 --- 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 @@ -14,17 +14,15 @@ #include #endif - namespace Ogre { class SceneNode; -} // Ogre +} // namespace Ogre namespace rviz { class ColorProperty; class FloatProperty; class IntProperty; -} // rviz - +} // namespace rviz // All the source in this plugin is in its own namespace. This is not // required but is good practice. @@ -36,8 +34,8 @@ class VectorAtPositionVisual; // 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 +// 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 @@ -48,10 +46,10 @@ class VectorAtPositionVisual; // 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: +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(); @@ -62,17 +60,18 @@ Q_OBJECT // subscribed to incoming data and should not show anything in the // 3D view. These functions are where these connections are made // and broken. -protected: + protected: virtual void onInitialize(); // A helper to clear this display back to the initial state. virtual void reset(); -Q_SIGNALS: + Q_SIGNALS: void updateVectorAtPositionSignal(); - // These Qt slots get connected to signals indicating changes in the user-editable properties. -private Q_SLOTS: + // These Qt slots get connected to signals indicating changes in the + // user-editable properties. + private Q_SLOTS: void updateScale(); void updateShowText(); void updateColorAndAlpha(); @@ -81,7 +80,7 @@ private Q_SLOTS: void updateVectorAtPosition(); // Function to handle an incoming ROS message. -private: + private: void processMessage(const kindr_msgs::VectorAtPosition::ConstPtr& msg); // Storage for the list of visuals. It is a circular buffer where @@ -106,4 +105,4 @@ private Q_SLOTS: kindr_msgs::VectorAtPosition::ConstPtr current_vector_at_position_; }; -} // kindr_rviz_plugins +} // 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 similarity index 87% rename from src/nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionVisual.hpp rename to src/Nav/kindr_ros/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionVisual.hpp index 59018dbf..34219359 100644 --- 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 @@ -6,33 +6,31 @@ // kindr ros #include - namespace Ogre { class Vector3; class Quaternion; -} // Ogre +} // namespace Ogre namespace rviz { class Arrow; class BillboardLine; class MovableText; -} // rviz - +} // 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 +// 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: +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); + VectorAtPositionVisual(Ogre::SceneManager* scene_manager, + Ogre::SceneNode* parent_node); // Destructor. Removes the visual stuff from the scene. virtual ~VectorAtPositionVisual(); @@ -60,7 +58,7 @@ class VectorAtPositionVisual // parameters and therefore don't come from the VectorAtPosition message. void setColor(const Ogre::ColourValue& color); -protected: + protected: // The object implementing the actual arrow shape boost::shared_ptr arrow_; boost::shared_ptr circleArrow_; @@ -75,7 +73,8 @@ class VectorAtPositionVisual // 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 + // Store the scaling factor and the length of the vector, so that it can be + // scale afterwards float length_; float lengthScalingFactor_; float widthScalingFactor_; @@ -108,4 +107,4 @@ class VectorAtPositionVisual void updateText(); }; -} // kindr_rviz_plugins +} // 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 similarity index 100% rename from src/nav/kindr_ros/kindr_rviz_plugins/package.xml rename to src/Nav/kindr_ros/kindr_rviz_plugins/package.xml diff --git a/src/nav/kindr_ros/kindr_rviz_plugins/plugin_description.xml b/src/Nav/kindr_ros/kindr_rviz_plugins/plugin_description.xml similarity index 100% rename from src/nav/kindr_ros/kindr_rviz_plugins/plugin_description.xml rename to src/Nav/kindr_ros/kindr_rviz_plugins/plugin_description.xml 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 similarity index 65% rename from src/nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp rename to src/Nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp index 132c18bf..5ed7c7d3 100644 --- a/src/nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp +++ b/src/Nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionVisual.cpp @@ -27,28 +27,26 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include +#include "kindr_rviz_plugins/VectorAtPositionVisual.hpp" +#include +#include +#include #include #include #include -#include "kindr_rviz_plugins/VectorAtPositionVisual.hpp" - - 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)) -{ +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 @@ -56,8 +54,8 @@ VectorAtPositionVisual::VectorAtPositionVisual(Ogre::SceneManager* scene_manager // 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. + // 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(); @@ -66,13 +64,13 @@ VectorAtPositionVisual::VectorAtPositionVisual(Ogre::SceneManager* scene_manager // 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)); + 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_ )); + circleArrow_.reset(new rviz::Arrow(scene_manager_, scene_node_circle_)); } -VectorAtPositionVisual::~VectorAtPositionVisual() -{ +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_); @@ -80,19 +78,20 @@ VectorAtPositionVisual::~VectorAtPositionVisual() scene_manager_->destroySceneNode(scene_node_text_); } -void VectorAtPositionVisual::setMessage(const kindr_msgs::VectorAtPosition::ConstPtr& msg) -{ +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. + // 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)); + arrow_->setPosition(Ogre::Vector3(0, 0, 0)); // Set the orientation of the arrow to match the direction of the vector. arrow_->setDirection(vector_); @@ -111,84 +110,91 @@ void VectorAtPositionVisual::setMessage(const kindr_msgs::VectorAtPosition::Cons // show circle arrow only if type is torque scene_node_circle_->setVisible(showTorque_); - } -void VectorAtPositionVisual::setArrowPosition(const Ogre::Vector3& position) -{ +void VectorAtPositionVisual::setArrowPosition(const Ogre::Vector3& position) { scene_node_frame_->setPosition(position); } -void VectorAtPositionVisual::setArrowOrientation(const Ogre::Quaternion& orientation) -{ +void VectorAtPositionVisual::setArrowOrientation( + const Ogre::Quaternion& orientation) { scene_node_frame_->setOrientation(orientation); } -void VectorAtPositionVisual::setScalingFactors(float lengthScalingFactor, float widthScalingFactor) -{ +void VectorAtPositionVisual::setScalingFactors(float lengthScalingFactor, + float widthScalingFactor) { lengthScalingFactor_ = lengthScalingFactor; widthScalingFactor_ = widthScalingFactor; updateScaling(); } -void VectorAtPositionVisual::setColor(const Ogre::ColourValue& color) -{ +void VectorAtPositionVisual::setColor(const Ogre::ColourValue& color) { color_ = color; updateColor(); } -void VectorAtPositionVisual::setShowText(bool showText) -{ +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_)); +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_) { + 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;} + 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)); + 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); + 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); + 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() -{ +void VectorAtPositionVisual::updateColor() { arrow_->setColor(color_); - if(showTorque_) { + 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() -{ +void VectorAtPositionVisual::updateText() { // add description text if available - const std::string textString = showText_? name_ : ""; + 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); + text_->setTextAlignment(rviz::MovableText::H_CENTER, + rviz::MovableText::V_BELOW); scene_node_text_->attachObject(text_.get()); } -} // kindr_rviz_plugins +} // 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 similarity index 100% rename from src/nav/kindr_ros/kindr_rviz_plugins/test/EmptyTests.cpp rename to src/Nav/kindr_ros/kindr_rviz_plugins/test/EmptyTests.cpp 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 similarity index 100% rename from src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/CMakeLists.txt rename to src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/CMakeLists.txt diff --git a/src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/COLCON_IGNORE b/src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/COLCON_IGNORE similarity index 100% rename from src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/COLCON_IGNORE 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 similarity index 100% rename from src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/icons/classes/MultiDOFJointTrajectory.png rename to src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/icons/classes/MultiDOFJointTrajectory.png 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 similarity index 87% rename from src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp rename to src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp index ffc20665..d141d7d6 100644 --- 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 @@ -1,49 +1,45 @@ #pragma once #ifndef Q_MOC_RUN -#include - -#include - #include #include + +#include +#include #endif -#include #include - -#include - -#include +#include +#include #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 -{ -Q_OBJECT -public: +class MultiDOFJointTrajectoryDisplay + : public rviz::MessageFilterDisplay< + trajectory_msgs::MultiDOFJointTrajectory> { + Q_OBJECT + public: MultiDOFJointTrajectoryDisplay(); virtual ~MultiDOFJointTrajectoryDisplay(); - -protected: + protected: virtual void onInitialize(); virtual void reset(); -Q_SIGNALS: + Q_SIGNALS: void updateTrajectorySignal(); -private Q_SLOTS: + private Q_SLOTS: void setShowConnection(); void setShowTransformRotation(); void setShowVelocityLinear(); @@ -72,8 +68,9 @@ private Q_SLOTS: void updateTrajectory(); -private: - void processMessage(const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr& msg); + private: + void processMessage( + const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr& msg); void updateShowConnection(); void updateShowTransformRotation(); @@ -99,8 +96,12 @@ private Q_SLOTS: void updateFontSize(); void updateShowText(); - boost::circular_buffer>> visuals_points_; - boost::circular_buffer>> visuals_connections_; + boost::circular_buffer< + std::vector>> + visuals_points_; + boost::circular_buffer>> + visuals_connections_; rviz::BoolProperty* property_show_connection_; rviz::BoolProperty* property_show_transform_rotation_; @@ -156,4 +157,4 @@ private Q_SLOTS: trajectory_msgs::MultiDOFJointTrajectory::ConstPtr current_trajectory_; }; -} // multi_dof_joint_trajectory_rviz_plugins +} // 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 similarity index 77% rename from src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp rename to src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointConnectionVisual.hpp index 7e49ebd1..726adb21 100644 --- 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 @@ -1,35 +1,28 @@ #pragma once -#include -#include #include - +#include +#include +#include #include #include - -#include - #include - namespace multi_dof_joint_trajectory_rviz_plugins { -class MultiDOFJointTrajectoryPointConnectionVisual -{ -public: +class MultiDOFJointTrajectoryPointConnectionVisual { + public: MultiDOFJointTrajectoryPointConnectionVisual( - Ogre::SceneManager* scene_manager, - Ogre::SceneNode* parent_node, + 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); + float show_connection, const Ogre::ColourValue& color); virtual ~MultiDOFJointTrajectoryPointConnectionVisual(); void setShowConnection(bool visible); void setColor(const Ogre::ColourValue& color); -private: + private: void updateShowConnection(); void updateColor(); @@ -43,5 +36,4 @@ class MultiDOFJointTrajectoryPointConnectionVisual Ogre::ColourValue color_; }; -} // multi_dof_joint_trajectory_rviz_plugins - +} // 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 similarity index 84% rename from src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp rename to src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp index d6c7aa5d..7c9cdb60 100644 --- 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 @@ -1,47 +1,34 @@ #pragma once -#include - -#include -#include #include - +#include +#include +#include #include #include #include - -#include - #include +#include namespace multi_dof_joint_trajectory_rviz_plugins { -class MultiDOFJointTrajectoryPointVisual -{ -public: +class MultiDOFJointTrajectoryPointVisual { + public: MultiDOFJointTrajectoryPointVisual( - Ogre::SceneManager* scene_manager, - Ogre::SceneNode* parent_node, + 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, + 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, + const std::vector& captions, float font_size, bool show_text); virtual ~MultiDOFJointTrajectoryPointVisual(); @@ -68,8 +55,7 @@ class MultiDOFJointTrajectoryPointVisual void setFontSize(float font_size); void setShowText(bool show_text); - -private: + private: void updateSizeTransformRotation(); void updateDiametersArrows(); void updateScaleVelocityLinear(); @@ -131,5 +117,4 @@ class MultiDOFJointTrajectoryPointVisual bool show_text_; }; -} // multi_dof_joint_trajectory_rviz_plugins - +} // 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 similarity index 100% rename from src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/package.xml rename to src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/package.xml 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 similarity index 100% rename from src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/plugin_description.xml rename to src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/plugin_description.xml 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 similarity index 100% rename from src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/test/EmptyTests.cpp rename to src/Nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/test/EmptyTests.cpp diff --git a/src/nav/localization/config/driver_params.yaml b/src/Nav/localization/config/driver_params.yaml similarity index 100% rename from src/nav/localization/config/driver_params.yaml rename to src/Nav/localization/config/driver_params.yaml diff --git a/src/nav/localization/config/ekf/gps_ekf_global.yaml b/src/Nav/localization/config/ekf/gps_ekf_global.yaml similarity index 100% rename from src/nav/localization/config/ekf/gps_ekf_global.yaml rename to src/Nav/localization/config/ekf/gps_ekf_global.yaml diff --git a/src/nav/localization/config/ekf/gps_ekf_local.yaml b/src/Nav/localization/config/ekf/gps_ekf_local.yaml similarity index 100% rename from src/nav/localization/config/ekf/gps_ekf_local.yaml rename to src/Nav/localization/config/ekf/gps_ekf_local.yaml diff --git a/src/nav/localization/config/navsat.yaml b/src/Nav/localization/config/navsat.yaml similarity index 100% rename from src/nav/localization/config/navsat.yaml rename to src/Nav/localization/config/navsat.yaml diff --git a/src/nav/localization/config/rtabmap.yaml b/src/Nav/localization/config/rtabmap.yaml similarity index 100% rename from src/nav/localization/config/rtabmap.yaml rename to src/Nav/localization/config/rtabmap.yaml diff --git a/src/nav/localization/config/rviz.views/basicNavMap.rviz b/src/Nav/localization/config/rviz.views/basicNavMap.rviz similarity index 100% rename from src/nav/localization/config/rviz.views/basicNavMap.rviz rename to src/Nav/localization/config/rviz.views/basicNavMap.rviz diff --git a/src/nav/localization/config/slam.yaml b/src/Nav/localization/config/slam.yaml similarity index 100% rename from src/nav/localization/config/slam.yaml rename to src/Nav/localization/config/slam.yaml diff --git a/src/nav/localization/config/urdf/spike.urdf.xml b/src/Nav/localization/config/urdf/spike.urdf.xml similarity index 100% rename from src/nav/localization/config/urdf/spike.urdf.xml rename to src/Nav/localization/config/urdf/spike.urdf.xml diff --git a/src/nav/localization/launch/description.launch.py b/src/Nav/localization/launch/description.launch.py similarity index 100% rename from src/nav/localization/launch/description.launch.py rename to src/Nav/localization/launch/description.launch.py diff --git a/src/nav/localization/launch/ekf.launch.py b/src/Nav/localization/launch/ekf.launch.py similarity index 100% rename from src/nav/localization/launch/ekf.launch.py rename to src/Nav/localization/launch/ekf.launch.py diff --git a/src/nav/localization/launch/localization.launch.py b/src/Nav/localization/launch/localization.launch.py similarity index 100% rename from src/nav/localization/launch/localization.launch.py rename to src/Nav/localization/launch/localization.launch.py diff --git a/src/nav/localization/launch/navsat.launch.py b/src/Nav/localization/launch/navsat.launch.py similarity index 100% rename from src/nav/localization/launch/navsat.launch.py rename to src/Nav/localization/launch/navsat.launch.py diff --git a/src/nav/localization/launch/rtabmap.launch.py b/src/Nav/localization/launch/rtabmap.launch.py similarity index 100% rename from src/nav/localization/launch/rtabmap.launch.py rename to src/Nav/localization/launch/rtabmap.launch.py diff --git a/src/nav/nav_commanders/nav_commanders/__init__.py b/src/Nav/localization/localization/__init__.py similarity index 100% rename from src/nav/nav_commanders/nav_commanders/__init__.py rename to src/Nav/localization/localization/__init__.py diff --git a/src/nav/localization/localization/imu_filter.py b/src/Nav/localization/localization/imu_filter.py similarity index 100% rename from src/nav/localization/localization/imu_filter.py rename to src/Nav/localization/localization/imu_filter.py diff --git a/src/nav/localization/localization/lidar_mask_tool.py b/src/Nav/localization/localization/lidar_mask_tool.py similarity index 100% rename from src/nav/localization/localization/lidar_mask_tool.py rename to src/Nav/localization/localization/lidar_mask_tool.py diff --git a/src/nav/localization/localization/republish_odometry.py b/src/Nav/localization/localization/republish_odometry.py similarity index 100% rename from src/nav/localization/localization/republish_odometry.py rename to src/Nav/localization/localization/republish_odometry.py diff --git a/src/nav/localization/package.xml b/src/Nav/localization/package.xml similarity index 100% rename from src/nav/localization/package.xml rename to src/Nav/localization/package.xml diff --git a/src/nav/localization/resource/localization b/src/Nav/localization/resource/localization similarity index 100% rename from src/nav/localization/resource/localization rename to src/Nav/localization/resource/localization diff --git a/src/nav/localization/setup.cfg b/src/Nav/localization/setup.cfg similarity index 100% rename from src/nav/localization/setup.cfg rename to src/Nav/localization/setup.cfg diff --git a/src/nav/localization/setup.py b/src/Nav/localization/setup.py similarity index 100% rename from src/nav/localization/setup.py rename to src/Nav/localization/setup.py diff --git a/src/nav/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/nav_commanders/launch/aruco_gps_commander.py b/src/Nav/nav_commanders/launch/aruco_gps_commander.py similarity index 100% rename from src/nav/nav_commanders/launch/aruco_gps_commander.py rename to src/Nav/nav_commanders/launch/aruco_gps_commander.py diff --git a/src/nav/nav_commanders/launch/gps_commander.launch.py b/src/Nav/nav_commanders/launch/gps_commander.launch.py similarity index 100% rename from src/nav/nav_commanders/launch/gps_commander.launch.py rename to src/Nav/nav_commanders/launch/gps_commander.launch.py diff --git a/src/nav/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/nav_commanders/launch/incremental_gps_commander.launch.py rename to src/Nav/nav_commanders/launch/incremental_gps_commander.launch.py diff --git a/src/video/computer_vision/computer_vision/__init__.py b/src/Nav/nav_commanders/nav_commanders/__init__.py similarity index 100% rename from src/video/computer_vision/computer_vision/__init__.py rename to src/Nav/nav_commanders/nav_commanders/__init__.py diff --git a/src/nav/nav_commanders/nav_commanders/gps_utils.py b/src/Nav/nav_commanders/nav_commanders/gps_utils.py similarity index 100% rename from src/nav/nav_commanders/nav_commanders/gps_utils.py rename to src/Nav/nav_commanders/nav_commanders/gps_utils.py diff --git a/src/nav/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/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/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/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/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/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/nav_commanders/package.xml b/src/Nav/nav_commanders/package.xml similarity index 100% rename from src/nav/nav_commanders/package.xml rename to src/Nav/nav_commanders/package.xml diff --git a/src/nav/nav_commanders/resource/nav_commanders b/src/Nav/nav_commanders/resource/nav_commanders similarity index 100% rename from src/nav/nav_commanders/resource/nav_commanders rename to src/Nav/nav_commanders/resource/nav_commanders diff --git a/src/nav/nav_commanders/setup.cfg b/src/Nav/nav_commanders/setup.cfg similarity index 100% rename from src/nav/nav_commanders/setup.cfg rename to src/Nav/nav_commanders/setup.cfg diff --git a/src/nav/nav_commanders/setup.py b/src/Nav/nav_commanders/setup.py similarity index 100% rename from src/nav/nav_commanders/setup.py rename to src/Nav/nav_commanders/setup.py diff --git a/src/video/computer_vision/test/test_copyright.py b/src/Nav/nav_commanders/test/test_copyright.py similarity index 100% rename from src/video/computer_vision/test/test_copyright.py rename to src/Nav/nav_commanders/test/test_copyright.py diff --git a/src/video/computer_vision/test/test_flake8.py b/src/Nav/nav_commanders/test/test_flake8.py similarity index 100% rename from src/video/computer_vision/test/test_flake8.py rename to src/Nav/nav_commanders/test/test_flake8.py diff --git a/src/video/computer_vision/test/test_pep257.py b/src/Nav/nav_commanders/test/test_pep257.py similarity index 100% rename from src/video/computer_vision/test/test_pep257.py rename to src/Nav/nav_commanders/test/test_pep257.py diff --git a/src/nav/navigation/CMakeLists.txt b/src/Nav/navigation/CMakeLists.txt similarity index 100% rename from src/nav/navigation/CMakeLists.txt rename to src/Nav/navigation/CMakeLists.txt diff --git a/src/nav/navigation/behavior_trees/behaviorTree.xml b/src/Nav/navigation/behavior_trees/behaviorTree.xml similarity index 100% rename from src/nav/navigation/behavior_trees/behaviorTree.xml rename to src/Nav/navigation/behavior_trees/behaviorTree.xml diff --git a/src/nav/navigation/behavior_trees/behaviorTree_NavPathingTestTree.xml b/src/Nav/navigation/behavior_trees/behaviorTree_NavPathingTestTree.xml similarity index 100% rename from src/nav/navigation/behavior_trees/behaviorTree_NavPathingTestTree.xml rename to src/Nav/navigation/behavior_trees/behaviorTree_NavPathingTestTree.xml diff --git a/src/nav/navigation/behavior_trees/behaviorTree_original.xml b/src/Nav/navigation/behavior_trees/behaviorTree_original.xml similarity index 100% rename from src/nav/navigation/behavior_trees/behaviorTree_original.xml rename to src/Nav/navigation/behavior_trees/behaviorTree_original.xml diff --git a/src/nav/navigation/behavior_trees/bt_dynamic_replanning.xml b/src/Nav/navigation/behavior_trees/bt_dynamic_replanning.xml similarity index 100% rename from src/nav/navigation/behavior_trees/bt_dynamic_replanning.xml rename to src/Nav/navigation/behavior_trees/bt_dynamic_replanning.xml diff --git a/src/nav/navigation/behavior_trees/spike_bt.xml b/src/Nav/navigation/behavior_trees/spike_bt.xml similarity index 100% rename from src/nav/navigation/behavior_trees/spike_bt.xml rename to src/Nav/navigation/behavior_trees/spike_bt.xml diff --git a/src/nav/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/nav/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/nav/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/nav/navigation/config/elevation_mapping/ouster_elevation_mapping.yaml rename to src/Nav/navigation/config/elevation_mapping/ouster_elevation_mapping.yaml diff --git a/src/nav/navigation/config/elevation_mapping/postprocessing_traversability.yaml b/src/Nav/navigation/config/elevation_mapping/postprocessing_traversability.yaml similarity index 100% rename from src/nav/navigation/config/elevation_mapping/postprocessing_traversability.yaml rename to src/Nav/navigation/config/elevation_mapping/postprocessing_traversability.yaml diff --git a/src/nav/navigation/config/elevation_mapping/spike_robot.yaml b/src/Nav/navigation/config/elevation_mapping/spike_robot.yaml similarity index 100% rename from src/nav/navigation/config/elevation_mapping/spike_robot.yaml rename to src/Nav/navigation/config/elevation_mapping/spike_robot.yaml diff --git a/src/nav/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/nav/navigation/config/elevation_mapping/zed2i_elevation_mapping.yaml rename to src/Nav/navigation/config/elevation_mapping/zed2i_elevation_mapping.yaml diff --git a/src/nav/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/nav/navigation/config/elevation_mapping/zed2i_sensor_processor.yaml rename to src/Nav/navigation/config/elevation_mapping/zed2i_sensor_processor.yaml diff --git a/src/nav/navigation/config/ouster/driver_params.yaml b/src/Nav/navigation/config/ouster/driver_params.yaml similarity index 100% rename from src/nav/navigation/config/ouster/driver_params.yaml rename to src/Nav/navigation/config/ouster/driver_params.yaml diff --git a/src/nav/navigation/config/zed/zed_override_params.yaml b/src/Nav/navigation/config/zed/zed_override_params.yaml similarity index 100% rename from src/nav/navigation/config/zed/zed_override_params.yaml rename to src/Nav/navigation/config/zed/zed_override_params.yaml diff --git a/src/nav/navigation/launch/bringup.launch.py b/src/Nav/navigation/launch/bringup.launch.py similarity index 100% rename from src/nav/navigation/launch/bringup.launch.py rename to src/Nav/navigation/launch/bringup.launch.py diff --git a/src/nav/navigation/launch/nav2.launch.py b/src/Nav/navigation/launch/nav2.launch.py similarity index 100% rename from src/nav/navigation/launch/nav2.launch.py rename to src/Nav/navigation/launch/nav2.launch.py diff --git a/src/nav/navigation/launch/navigation.launch.py b/src/Nav/navigation/launch/navigation.launch.py similarity index 100% rename from src/nav/navigation/launch/navigation.launch.py rename to src/Nav/navigation/launch/navigation.launch.py diff --git a/src/nav/navigation/launch/ouster.launch.py b/src/Nav/navigation/launch/ouster.launch.py similarity index 100% rename from src/nav/navigation/launch/ouster.launch.py rename to src/Nav/navigation/launch/ouster.launch.py diff --git a/src/nav/navigation/launch/ouster_composable.launch.py b/src/Nav/navigation/launch/ouster_composable.launch.py similarity index 100% rename from src/nav/navigation/launch/ouster_composable.launch.py rename to src/Nav/navigation/launch/ouster_composable.launch.py diff --git a/src/nav/navigation/launch/traversability_gridmap.launch.py b/src/Nav/navigation/launch/traversability_gridmap.launch.py similarity index 100% rename from src/nav/navigation/launch/traversability_gridmap.launch.py rename to src/Nav/navigation/launch/traversability_gridmap.launch.py diff --git a/src/nav/navigation/launch/zed.launch.py b/src/Nav/navigation/launch/zed.launch.py similarity index 100% rename from src/nav/navigation/launch/zed.launch.py rename to src/Nav/navigation/launch/zed.launch.py diff --git a/src/nav/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/nav/navigation/lidar_masks/ouster_mask_combined_shifted.png rename to src/Nav/navigation/lidar_masks/ouster_mask_combined_shifted.png diff --git a/src/nav/navigation/lidar_masks/ouster_mask_front.png b/src/Nav/navigation/lidar_masks/ouster_mask_front.png similarity index 100% rename from src/nav/navigation/lidar_masks/ouster_mask_front.png rename to src/Nav/navigation/lidar_masks/ouster_mask_front.png diff --git a/src/nav/navigation/lidar_masks/ouster_mask_left.png b/src/Nav/navigation/lidar_masks/ouster_mask_left.png similarity index 100% rename from src/nav/navigation/lidar_masks/ouster_mask_left.png rename to src/Nav/navigation/lidar_masks/ouster_mask_left.png diff --git a/src/nav/navigation/lidar_masks/ouster_mask_rear.png b/src/Nav/navigation/lidar_masks/ouster_mask_rear.png similarity index 100% rename from src/nav/navigation/lidar_masks/ouster_mask_rear.png rename to src/Nav/navigation/lidar_masks/ouster_mask_rear.png diff --git a/src/nav/navigation/lidar_masks/ouster_mask_right.png b/src/Nav/navigation/lidar_masks/ouster_mask_right.png similarity index 100% rename from src/nav/navigation/lidar_masks/ouster_mask_right.png rename to src/Nav/navigation/lidar_masks/ouster_mask_right.png diff --git a/src/nav/navigation/package.xml b/src/Nav/navigation/package.xml similarity index 100% rename from src/nav/navigation/package.xml rename to src/Nav/navigation/package.xml diff --git a/src/nav/navigation/params/nav2.yaml b/src/Nav/navigation/params/nav2.yaml similarity index 100% rename from src/nav/navigation/params/nav2.yaml rename to src/Nav/navigation/params/nav2.yaml diff --git a/src/nav/navigation/rviz/nav2_cprt.rviz b/src/Nav/navigation/rviz/nav2_cprt.rviz similarity index 100% rename from src/nav/navigation/rviz/nav2_cprt.rviz rename to src/Nav/navigation/rviz/nav2_cprt.rviz diff --git a/src/nav/navigation/rviz/nav2_offical_default_config.rviz b/src/Nav/navigation/rviz/nav2_offical_default_config.rviz similarity index 100% rename from src/nav/navigation/rviz/nav2_offical_default_config.rviz rename to src/Nav/navigation/rviz/nav2_offical_default_config.rviz diff --git a/src/nav/navigation/rviz/traversibility_gridmap.rviz b/src/Nav/navigation/rviz/traversibility_gridmap.rviz similarity index 100% rename from src/nav/navigation/rviz/traversibility_gridmap.rviz rename to src/Nav/navigation/rviz/traversibility_gridmap.rviz diff --git a/src/telemetry/system-telemetry-cpp/CMakeLists.txt b/src/Telemetry/system-telemetry-cpp/CMakeLists.txt similarity index 100% rename from src/telemetry/system-telemetry-cpp/CMakeLists.txt rename to src/Telemetry/system-telemetry-cpp/CMakeLists.txt diff --git a/src/telemetry/system-telemetry-cpp/LICENSE b/src/Telemetry/system-telemetry-cpp/LICENSE similarity index 100% rename from src/telemetry/system-telemetry-cpp/LICENSE rename to src/Telemetry/system-telemetry-cpp/LICENSE diff --git a/src/telemetry/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/telemetry/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/telemetry/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/telemetry/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/telemetry/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/telemetry/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/telemetry/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/telemetry/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/telemetry/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/telemetry/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/telemetry/system-telemetry-cpp/package.xml b/src/Telemetry/system-telemetry-cpp/package.xml similarity index 100% rename from src/telemetry/system-telemetry-cpp/package.xml rename to src/Telemetry/system-telemetry-cpp/package.xml diff --git a/src/telemetry/system-telemetry-cpp/src/cpu_collector.cpp b/src/Telemetry/system-telemetry-cpp/src/cpu_collector.cpp similarity index 100% rename from src/telemetry/system-telemetry-cpp/src/cpu_collector.cpp rename to src/Telemetry/system-telemetry-cpp/src/cpu_collector.cpp diff --git a/src/telemetry/system-telemetry-cpp/src/gpu_collector.cpp b/src/Telemetry/system-telemetry-cpp/src/gpu_collector.cpp similarity index 100% rename from src/telemetry/system-telemetry-cpp/src/gpu_collector.cpp rename to src/Telemetry/system-telemetry-cpp/src/gpu_collector.cpp diff --git a/src/telemetry/system-telemetry-cpp/src/memory_collector.cpp b/src/Telemetry/system-telemetry-cpp/src/memory_collector.cpp similarity index 100% rename from src/telemetry/system-telemetry-cpp/src/memory_collector.cpp rename to src/Telemetry/system-telemetry-cpp/src/memory_collector.cpp diff --git a/src/telemetry/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/telemetry/system-telemetry-cpp/src/system_telemetry_publisher.cpp rename to src/Telemetry/system-telemetry-cpp/src/system_telemetry_publisher.cpp 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 deleted file mode 100644 index a7b76612..00000000 --- a/src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosGeometryMsgTwist.hpp +++ /dev/null @@ -1,67 +0,0 @@ -/* - * 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 deleted file mode 100644 index fd837e1c..00000000 --- a/src/nav/kindr_ros/kindr_ros/include/kindr_ros/RosTfPose.hpp +++ /dev/null @@ -1,110 +0,0 @@ -/* - * 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 & 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_rviz_plugins/src/VectorAtPositionDisplay.cpp b/src/nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp deleted file mode 100644 index 762de031..00000000 --- a/src/nav/kindr_ros/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp +++ /dev/null @@ -1,224 +0,0 @@ -// 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(); -} - -} // 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/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp b/src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp deleted file mode 100644 index 41b22f54..00000000 --- a/src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp +++ /dev/null @@ -1,652 +0,0 @@ -#include - -#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp" - - -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_); - } - } -} - -} // 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 deleted file mode 100644 index e85cde41..00000000 --- a/src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointConnectionVisual.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#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_); - } -} - -} // 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 deleted file mode 100644 index 2d610596..00000000 --- a/src/nav/kindr_ros/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryPointVisual.cpp +++ /dev/null @@ -1,438 +0,0 @@ -#include - -#include "multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryPointVisual.hpp" - - -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; - } -} - -} // multi_dof_joint_trajectory_rviz_plugins -