diff --git a/.devcontainer/base_station/Dockerfile b/.devcontainer/base_station/Dockerfile new file mode 100644 index 00000000..5190d9b3 --- /dev/null +++ b/.devcontainer/base_station/Dockerfile @@ -0,0 +1,154 @@ +FROM ubuntu:22.04 + +ARG DEBIAN_FRONTEND=noninteractive +ARG ROS_DISTRO=humble +ARG USERNAME=loop_dev +ARG USER_UID=1000 +ARG USER_GID=${USER_UID} +ARG MACHINE_TYPE=developer + +ENV ROS_DISTRO=${ROS_DISTRO} +ENV LANG=en_US.UTF-8 +ENV LC_ALL=en_US.UTF-8 +ENV SHELL=/bin/bash + +SHELL ["/bin/bash", "-c"] + +RUN apt-get update && apt-get install -y --no-install-recommends \ + ca-certificates \ + curl \ + gnupg \ + lsb-release \ + locales \ + software-properties-common \ + sudo \ + && locale-gen en_US en_US.UTF-8 \ + && update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 \ + && add-apt-repository universe \ + && 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 \ + && rm -rf /var/lib/apt/lists/* + +RUN apt-get update && apt-get install -y --no-install-recommends \ + bash-completion \ + build-essential \ + can-utils \ + clang-format \ + cmake \ + evtest \ + geographiclib-tools \ + git \ + git-lfs \ + gdb \ + htop \ + iproute2 \ + iputils-ping \ + libboost-chrono-dev \ + libboost-date-time-dev \ + libboost-filesystem-dev \ + libboost-program-options-dev \ + libboost-regex-dev \ + libboost-serialization-dev \ + libboost-system-dev \ + libboost-thread-dev \ + libboost-timer-dev \ + libgeographic-dev \ + libopencv-dev \ + libpcl-dev \ + libserial-dev \ + nano \ + net-tools \ + openssh-client \ + python3-dev \ + python3-colcon-common-extensions \ + python3-pip \ + python3-rosdep \ + python3-serial \ + python3-vcstool \ + python3-venv \ + ruby \ + tmux \ + tmuxinator \ + udev \ + unzip \ + usbutils \ + vim \ + wget \ + && apt-get install -y --no-install-recommends \ + ros-${ROS_DISTRO}-desktop \ + ros-${ROS_DISTRO}-moveit \ + ros-${ROS_DISTRO}-navigation2 \ + ros-${ROS_DISTRO}-nav2-bringup \ + ros-${ROS_DISTRO}-ros2-control \ + ros-${ROS_DISTRO}-ros2-controllers \ + ros-${ROS_DISTRO}-ros2-control-test-assets \ + ros-${ROS_DISTRO}-ros2controlcli \ + ros-${ROS_DISTRO}-control-msgs \ + ros-${ROS_DISTRO}-controller-manager \ + ros-${ROS_DISTRO}-cv-bridge \ + ros-${ROS_DISTRO}-eigen3-cmake-module \ + ros-${ROS_DISTRO}-generate-parameter-library \ + ros-${ROS_DISTRO}-geographic-msgs \ + ros-${ROS_DISTRO}-gps-msgs \ + ros-${ROS_DISTRO}-gtsam \ + ros-${ROS_DISTRO}-gz-ros2-control \ + ros-${ROS_DISTRO}-image-transport-plugins \ + ros-${ROS_DISTRO}-joint-state-broadcaster \ + ros-${ROS_DISTRO}-joint-state-publisher \ + ros-${ROS_DISTRO}-joint-state-publisher-gui \ + ros-${ROS_DISTRO}-joint-trajectory-controller \ + ros-${ROS_DISTRO}-kdl-parser \ + ros-${ROS_DISTRO}-moveit-configs-utils \ + ros-${ROS_DISTRO}-moveit-kinematics \ + ros-${ROS_DISTRO}-moveit-planners \ + ros-${ROS_DISTRO}-moveit-ros-planning-interface \ + ros-${ROS_DISTRO}-moveit-simple-controller-manager \ + ros-${ROS_DISTRO}-nmea-msgs \ + ros-${ROS_DISTRO}-pcl-conversions \ + ros-${ROS_DISTRO}-position-controllers \ + ros-${ROS_DISTRO}-realtime-tools \ + ros-${ROS_DISTRO}-robot-localization \ + ros-${ROS_DISTRO}-robot-state-publisher \ + ros-${ROS_DISTRO}-ros-gz \ + ros-${ROS_DISTRO}-ros-gz-bridge \ + ros-${ROS_DISTRO}-ros-gz-sim \ + ros-${ROS_DISTRO}-rosidl-default-generators \ + ros-${ROS_DISTRO}-tf2-eigen \ + ros-${ROS_DISTRO}-tf2-geometry-msgs \ + ros-${ROS_DISTRO}-tf2-ros \ + ros-${ROS_DISTRO}-tf2-sensor-msgs \ + ros-${ROS_DISTRO}-topic-tools \ + ros-${ROS_DISTRO}-velocity-controllers \ + ros-${ROS_DISTRO}-vision-msgs \ + ros-${ROS_DISTRO}-xacro \ + && rm -rf /var/lib/apt/lists/* + +COPY requirements.txt /tmp/requirements.txt +RUN python3 -m pip install --no-cache-dir --upgrade pip \ + && python3 -m pip install --no-cache-dir -r /tmp/requirements.txt + +COPY libmavsdk-dev_3.14.0_ubuntu22.04_amd64.deb /tmp/libmavsdk-dev_3.14.0_ubuntu22.04_amd64.deb +RUN dpkg -i /tmp/libmavsdk-dev_3.14.0_ubuntu22.04_amd64.deb \ + || (apt-get update && apt-get install -y --no-install-recommends -f && rm -rf /var/lib/apt/lists/*) \ + && rm -f /tmp/libmavsdk-dev_3.14.0_ubuntu22.04_amd64.deb + +RUN if ! getent group ${USER_GID} >/dev/null; then groupadd --gid ${USER_GID} ${USERNAME}; fi \ + && if ! id -u ${USERNAME} >/dev/null 2>&1; then useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} -s /bin/bash; fi \ + && usermod -aG sudo,dialout,video,plugdev ${USERNAME} \ + && echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} \ + && chmod 0440 /etc/sudoers.d/${USERNAME} + +RUN rosdep init || true + +COPY .devcontainer/loop_ascii.txt /etc/loop_ascii.txt + +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /etc/bash.bashrc + +USER ${USERNAME} +RUN touch /home/${USERNAME}/.hushlogin +RUN printf '\nif [[ $- == *i* ]] && [[ -r /etc/loop_ascii.txt ]]; then\n printf '"'"'%%b\\n'"'"' "$(cat /etc/loop_ascii.txt)"\nfi\n' \ + >> /home/${USERNAME}/.bashrc + +WORKDIR /athena-code diff --git a/.devcontainer/base_station/build_docker.sh b/.devcontainer/base_station/build_docker.sh new file mode 100755 index 00000000..606d0329 --- /dev/null +++ b/.devcontainer/base_station/build_docker.sh @@ -0,0 +1,17 @@ +#!/usr/bin/env bash +set -euo pipefail + +config_dir="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)" +repo_root="$(cd -- "${config_dir}/../.." && pwd)" +machine_type="$(basename -- "${config_dir}")" +image_name="athena-code:${machine_type}" + +docker build \ + --file "${config_dir}/Dockerfile" \ + --build-arg "MACHINE_TYPE=${machine_type}" \ + --build-arg "USER_UID=$(id -u)" \ + --build-arg "USER_GID=$(id -g)" \ + --tag "${image_name}" \ + "${repo_root}" + +echo "Built ${image_name}" diff --git a/.devcontainer/base_station/devcontainer.json b/.devcontainer/base_station/devcontainer.json new file mode 100644 index 00000000..e03e1dd6 --- /dev/null +++ b/.devcontainer/base_station/devcontainer.json @@ -0,0 +1,40 @@ +{ + "name": "Athena ROS 2 Humble (Base Station)", + "build": { + "dockerfile": "Dockerfile", + "context": "../..", + "args": { + "MACHINE_TYPE": "base_station" + } + }, + "remoteUser": "loop_dev", + "updateRemoteUserUID": true, + "workspaceFolder": "/athena-code", + "workspaceMount": "source=${localWorkspaceFolder},target=/athena-code,type=bind,consistency=cached", + "postCreateCommand": ".devcontainer/base_station/post_create.sh", + "runArgs": [ + "--network=host", + "--ipc=host", + "--privileged" + ], + "containerEnv": { + "ROS_DISTRO": "humble", + "RMW_IMPLEMENTATION": "rmw_fastrtps_cpp", + "ATHENA_MACHINE_TYPE": "base_station" + }, + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cpptools", + "ms-vscode.cmake-tools", + "ms-python.python", + "ms-iot.vscode-ros", + "openai.chatgpt", + "cweijan.vscode-office", + "esben.prettier-vscode", + "redhat.vscode-xml", + "tamasfe.even-better-toml" + ] + } + } +} diff --git a/.devcontainer/base_station/post_create.sh b/.devcontainer/base_station/post_create.sh new file mode 100755 index 00000000..6a2e0ee0 --- /dev/null +++ b/.devcontainer/base_station/post_create.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +set +u +source "/opt/ros/${ROS_DISTRO}/setup.bash" +set -u + +sudo apt-get update +sudo rosdep update +sudo rosdep install -r --from-paths src --ignore-src --rosdistro "$ROS_DISTRO" -y + +colcon build --symlink-install diff --git a/.devcontainer/base_station/run_docker.sh b/.devcontainer/base_station/run_docker.sh new file mode 100755 index 00000000..04b61c4a --- /dev/null +++ b/.devcontainer/base_station/run_docker.sh @@ -0,0 +1,33 @@ +#!/usr/bin/env bash +set -euo pipefail + +config_dir="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)" +repo_root="$(cd -- "${config_dir}/../.." && pwd)" +machine_type="$(basename -- "${config_dir}")" +image_name="athena-code:${machine_type}" +command=("$@") +if [[ ${#command[@]} -eq 0 ]]; then + command=(bash) +fi + +docker_args=( + --rm + --interactive + --tty + --network host + --ipc host + --privileged + --env "ROS_DISTRO=humble" + --env "RMW_IMPLEMENTATION=rmw_fastrtps_cpp" + --volume "${repo_root}:/athena-code" + --workdir /athena-code +) + +if [[ -n "${DISPLAY:-}" && -d /tmp/.X11-unix ]]; then + docker_args+=( + --env "DISPLAY=${DISPLAY}" + --volume /tmp/.X11-unix:/tmp/.X11-unix:rw + ) +fi + +docker run "${docker_args[@]}" "${image_name}" "${command[@]}" diff --git a/.devcontainer/developer/Dockerfile b/.devcontainer/developer/Dockerfile new file mode 100644 index 00000000..5190d9b3 --- /dev/null +++ b/.devcontainer/developer/Dockerfile @@ -0,0 +1,154 @@ +FROM ubuntu:22.04 + +ARG DEBIAN_FRONTEND=noninteractive +ARG ROS_DISTRO=humble +ARG USERNAME=loop_dev +ARG USER_UID=1000 +ARG USER_GID=${USER_UID} +ARG MACHINE_TYPE=developer + +ENV ROS_DISTRO=${ROS_DISTRO} +ENV LANG=en_US.UTF-8 +ENV LC_ALL=en_US.UTF-8 +ENV SHELL=/bin/bash + +SHELL ["/bin/bash", "-c"] + +RUN apt-get update && apt-get install -y --no-install-recommends \ + ca-certificates \ + curl \ + gnupg \ + lsb-release \ + locales \ + software-properties-common \ + sudo \ + && locale-gen en_US en_US.UTF-8 \ + && update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 \ + && add-apt-repository universe \ + && 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 \ + && rm -rf /var/lib/apt/lists/* + +RUN apt-get update && apt-get install -y --no-install-recommends \ + bash-completion \ + build-essential \ + can-utils \ + clang-format \ + cmake \ + evtest \ + geographiclib-tools \ + git \ + git-lfs \ + gdb \ + htop \ + iproute2 \ + iputils-ping \ + libboost-chrono-dev \ + libboost-date-time-dev \ + libboost-filesystem-dev \ + libboost-program-options-dev \ + libboost-regex-dev \ + libboost-serialization-dev \ + libboost-system-dev \ + libboost-thread-dev \ + libboost-timer-dev \ + libgeographic-dev \ + libopencv-dev \ + libpcl-dev \ + libserial-dev \ + nano \ + net-tools \ + openssh-client \ + python3-dev \ + python3-colcon-common-extensions \ + python3-pip \ + python3-rosdep \ + python3-serial \ + python3-vcstool \ + python3-venv \ + ruby \ + tmux \ + tmuxinator \ + udev \ + unzip \ + usbutils \ + vim \ + wget \ + && apt-get install -y --no-install-recommends \ + ros-${ROS_DISTRO}-desktop \ + ros-${ROS_DISTRO}-moveit \ + ros-${ROS_DISTRO}-navigation2 \ + ros-${ROS_DISTRO}-nav2-bringup \ + ros-${ROS_DISTRO}-ros2-control \ + ros-${ROS_DISTRO}-ros2-controllers \ + ros-${ROS_DISTRO}-ros2-control-test-assets \ + ros-${ROS_DISTRO}-ros2controlcli \ + ros-${ROS_DISTRO}-control-msgs \ + ros-${ROS_DISTRO}-controller-manager \ + ros-${ROS_DISTRO}-cv-bridge \ + ros-${ROS_DISTRO}-eigen3-cmake-module \ + ros-${ROS_DISTRO}-generate-parameter-library \ + ros-${ROS_DISTRO}-geographic-msgs \ + ros-${ROS_DISTRO}-gps-msgs \ + ros-${ROS_DISTRO}-gtsam \ + ros-${ROS_DISTRO}-gz-ros2-control \ + ros-${ROS_DISTRO}-image-transport-plugins \ + ros-${ROS_DISTRO}-joint-state-broadcaster \ + ros-${ROS_DISTRO}-joint-state-publisher \ + ros-${ROS_DISTRO}-joint-state-publisher-gui \ + ros-${ROS_DISTRO}-joint-trajectory-controller \ + ros-${ROS_DISTRO}-kdl-parser \ + ros-${ROS_DISTRO}-moveit-configs-utils \ + ros-${ROS_DISTRO}-moveit-kinematics \ + ros-${ROS_DISTRO}-moveit-planners \ + ros-${ROS_DISTRO}-moveit-ros-planning-interface \ + ros-${ROS_DISTRO}-moveit-simple-controller-manager \ + ros-${ROS_DISTRO}-nmea-msgs \ + ros-${ROS_DISTRO}-pcl-conversions \ + ros-${ROS_DISTRO}-position-controllers \ + ros-${ROS_DISTRO}-realtime-tools \ + ros-${ROS_DISTRO}-robot-localization \ + ros-${ROS_DISTRO}-robot-state-publisher \ + ros-${ROS_DISTRO}-ros-gz \ + ros-${ROS_DISTRO}-ros-gz-bridge \ + ros-${ROS_DISTRO}-ros-gz-sim \ + ros-${ROS_DISTRO}-rosidl-default-generators \ + ros-${ROS_DISTRO}-tf2-eigen \ + ros-${ROS_DISTRO}-tf2-geometry-msgs \ + ros-${ROS_DISTRO}-tf2-ros \ + ros-${ROS_DISTRO}-tf2-sensor-msgs \ + ros-${ROS_DISTRO}-topic-tools \ + ros-${ROS_DISTRO}-velocity-controllers \ + ros-${ROS_DISTRO}-vision-msgs \ + ros-${ROS_DISTRO}-xacro \ + && rm -rf /var/lib/apt/lists/* + +COPY requirements.txt /tmp/requirements.txt +RUN python3 -m pip install --no-cache-dir --upgrade pip \ + && python3 -m pip install --no-cache-dir -r /tmp/requirements.txt + +COPY libmavsdk-dev_3.14.0_ubuntu22.04_amd64.deb /tmp/libmavsdk-dev_3.14.0_ubuntu22.04_amd64.deb +RUN dpkg -i /tmp/libmavsdk-dev_3.14.0_ubuntu22.04_amd64.deb \ + || (apt-get update && apt-get install -y --no-install-recommends -f && rm -rf /var/lib/apt/lists/*) \ + && rm -f /tmp/libmavsdk-dev_3.14.0_ubuntu22.04_amd64.deb + +RUN if ! getent group ${USER_GID} >/dev/null; then groupadd --gid ${USER_GID} ${USERNAME}; fi \ + && if ! id -u ${USERNAME} >/dev/null 2>&1; then useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} -s /bin/bash; fi \ + && usermod -aG sudo,dialout,video,plugdev ${USERNAME} \ + && echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} \ + && chmod 0440 /etc/sudoers.d/${USERNAME} + +RUN rosdep init || true + +COPY .devcontainer/loop_ascii.txt /etc/loop_ascii.txt + +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /etc/bash.bashrc + +USER ${USERNAME} +RUN touch /home/${USERNAME}/.hushlogin +RUN printf '\nif [[ $- == *i* ]] && [[ -r /etc/loop_ascii.txt ]]; then\n printf '"'"'%%b\\n'"'"' "$(cat /etc/loop_ascii.txt)"\nfi\n' \ + >> /home/${USERNAME}/.bashrc + +WORKDIR /athena-code diff --git a/.devcontainer/developer/build_docker.sh b/.devcontainer/developer/build_docker.sh new file mode 100755 index 00000000..606d0329 --- /dev/null +++ b/.devcontainer/developer/build_docker.sh @@ -0,0 +1,17 @@ +#!/usr/bin/env bash +set -euo pipefail + +config_dir="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)" +repo_root="$(cd -- "${config_dir}/../.." && pwd)" +machine_type="$(basename -- "${config_dir}")" +image_name="athena-code:${machine_type}" + +docker build \ + --file "${config_dir}/Dockerfile" \ + --build-arg "MACHINE_TYPE=${machine_type}" \ + --build-arg "USER_UID=$(id -u)" \ + --build-arg "USER_GID=$(id -g)" \ + --tag "${image_name}" \ + "${repo_root}" + +echo "Built ${image_name}" diff --git a/.devcontainer/developer/devcontainer.json b/.devcontainer/developer/devcontainer.json new file mode 100644 index 00000000..8e0a83d2 --- /dev/null +++ b/.devcontainer/developer/devcontainer.json @@ -0,0 +1,40 @@ +{ + "name": "Athena ROS 2 Humble (Developer)", + "build": { + "dockerfile": "Dockerfile", + "context": "../..", + "args": { + "MACHINE_TYPE": "developer" + } + }, + "remoteUser": "loop_dev", + "updateRemoteUserUID": true, + "workspaceFolder": "/athena-code", + "workspaceMount": "source=${localWorkspaceFolder},target=/athena-code,type=bind,consistency=cached", + "postCreateCommand": ".devcontainer/developer/post_create.sh", + "runArgs": [ + "--network=host", + "--ipc=host", + "--privileged" + ], + "containerEnv": { + "ROS_DISTRO": "humble", + "RMW_IMPLEMENTATION": "rmw_fastrtps_cpp", + "ATHENA_MACHINE_TYPE": "developer" + }, + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cpptools", + "ms-vscode.cmake-tools", + "ms-python.python", + "ms-iot.vscode-ros", + "openai.chatgpt", + "cweijan.vscode-office", + "esben.prettier-vscode", + "redhat.vscode-xml", + "tamasfe.even-better-toml" + ] + } + } +} diff --git a/.devcontainer/developer/post_create.sh b/.devcontainer/developer/post_create.sh new file mode 100755 index 00000000..7c0101a8 --- /dev/null +++ b/.devcontainer/developer/post_create.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +set +u +source "/opt/ros/${ROS_DISTRO}/setup.bash" +set -u + +sudo apt-get update +sudo rosdep update +sudo rosdep install -r --from-paths src --ignore-src --rosdistro "$ROS_DISTRO" -y + +colcon build --symlink-install --packages-ignore zed_components zed_debug diff --git a/.devcontainer/developer/run_docker.sh b/.devcontainer/developer/run_docker.sh new file mode 100755 index 00000000..04b61c4a --- /dev/null +++ b/.devcontainer/developer/run_docker.sh @@ -0,0 +1,33 @@ +#!/usr/bin/env bash +set -euo pipefail + +config_dir="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)" +repo_root="$(cd -- "${config_dir}/../.." && pwd)" +machine_type="$(basename -- "${config_dir}")" +image_name="athena-code:${machine_type}" +command=("$@") +if [[ ${#command[@]} -eq 0 ]]; then + command=(bash) +fi + +docker_args=( + --rm + --interactive + --tty + --network host + --ipc host + --privileged + --env "ROS_DISTRO=humble" + --env "RMW_IMPLEMENTATION=rmw_fastrtps_cpp" + --volume "${repo_root}:/athena-code" + --workdir /athena-code +) + +if [[ -n "${DISPLAY:-}" && -d /tmp/.X11-unix ]]; then + docker_args+=( + --env "DISPLAY=${DISPLAY}" + --volume /tmp/.X11-unix:/tmp/.X11-unix:rw + ) +fi + +docker run "${docker_args[@]}" "${image_name}" "${command[@]}" diff --git a/.devcontainer/jetson/Dockerfile b/.devcontainer/jetson/Dockerfile new file mode 100644 index 00000000..5190d9b3 --- /dev/null +++ b/.devcontainer/jetson/Dockerfile @@ -0,0 +1,154 @@ +FROM ubuntu:22.04 + +ARG DEBIAN_FRONTEND=noninteractive +ARG ROS_DISTRO=humble +ARG USERNAME=loop_dev +ARG USER_UID=1000 +ARG USER_GID=${USER_UID} +ARG MACHINE_TYPE=developer + +ENV ROS_DISTRO=${ROS_DISTRO} +ENV LANG=en_US.UTF-8 +ENV LC_ALL=en_US.UTF-8 +ENV SHELL=/bin/bash + +SHELL ["/bin/bash", "-c"] + +RUN apt-get update && apt-get install -y --no-install-recommends \ + ca-certificates \ + curl \ + gnupg \ + lsb-release \ + locales \ + software-properties-common \ + sudo \ + && locale-gen en_US en_US.UTF-8 \ + && update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 \ + && add-apt-repository universe \ + && 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 \ + && rm -rf /var/lib/apt/lists/* + +RUN apt-get update && apt-get install -y --no-install-recommends \ + bash-completion \ + build-essential \ + can-utils \ + clang-format \ + cmake \ + evtest \ + geographiclib-tools \ + git \ + git-lfs \ + gdb \ + htop \ + iproute2 \ + iputils-ping \ + libboost-chrono-dev \ + libboost-date-time-dev \ + libboost-filesystem-dev \ + libboost-program-options-dev \ + libboost-regex-dev \ + libboost-serialization-dev \ + libboost-system-dev \ + libboost-thread-dev \ + libboost-timer-dev \ + libgeographic-dev \ + libopencv-dev \ + libpcl-dev \ + libserial-dev \ + nano \ + net-tools \ + openssh-client \ + python3-dev \ + python3-colcon-common-extensions \ + python3-pip \ + python3-rosdep \ + python3-serial \ + python3-vcstool \ + python3-venv \ + ruby \ + tmux \ + tmuxinator \ + udev \ + unzip \ + usbutils \ + vim \ + wget \ + && apt-get install -y --no-install-recommends \ + ros-${ROS_DISTRO}-desktop \ + ros-${ROS_DISTRO}-moveit \ + ros-${ROS_DISTRO}-navigation2 \ + ros-${ROS_DISTRO}-nav2-bringup \ + ros-${ROS_DISTRO}-ros2-control \ + ros-${ROS_DISTRO}-ros2-controllers \ + ros-${ROS_DISTRO}-ros2-control-test-assets \ + ros-${ROS_DISTRO}-ros2controlcli \ + ros-${ROS_DISTRO}-control-msgs \ + ros-${ROS_DISTRO}-controller-manager \ + ros-${ROS_DISTRO}-cv-bridge \ + ros-${ROS_DISTRO}-eigen3-cmake-module \ + ros-${ROS_DISTRO}-generate-parameter-library \ + ros-${ROS_DISTRO}-geographic-msgs \ + ros-${ROS_DISTRO}-gps-msgs \ + ros-${ROS_DISTRO}-gtsam \ + ros-${ROS_DISTRO}-gz-ros2-control \ + ros-${ROS_DISTRO}-image-transport-plugins \ + ros-${ROS_DISTRO}-joint-state-broadcaster \ + ros-${ROS_DISTRO}-joint-state-publisher \ + ros-${ROS_DISTRO}-joint-state-publisher-gui \ + ros-${ROS_DISTRO}-joint-trajectory-controller \ + ros-${ROS_DISTRO}-kdl-parser \ + ros-${ROS_DISTRO}-moveit-configs-utils \ + ros-${ROS_DISTRO}-moveit-kinematics \ + ros-${ROS_DISTRO}-moveit-planners \ + ros-${ROS_DISTRO}-moveit-ros-planning-interface \ + ros-${ROS_DISTRO}-moveit-simple-controller-manager \ + ros-${ROS_DISTRO}-nmea-msgs \ + ros-${ROS_DISTRO}-pcl-conversions \ + ros-${ROS_DISTRO}-position-controllers \ + ros-${ROS_DISTRO}-realtime-tools \ + ros-${ROS_DISTRO}-robot-localization \ + ros-${ROS_DISTRO}-robot-state-publisher \ + ros-${ROS_DISTRO}-ros-gz \ + ros-${ROS_DISTRO}-ros-gz-bridge \ + ros-${ROS_DISTRO}-ros-gz-sim \ + ros-${ROS_DISTRO}-rosidl-default-generators \ + ros-${ROS_DISTRO}-tf2-eigen \ + ros-${ROS_DISTRO}-tf2-geometry-msgs \ + ros-${ROS_DISTRO}-tf2-ros \ + ros-${ROS_DISTRO}-tf2-sensor-msgs \ + ros-${ROS_DISTRO}-topic-tools \ + ros-${ROS_DISTRO}-velocity-controllers \ + ros-${ROS_DISTRO}-vision-msgs \ + ros-${ROS_DISTRO}-xacro \ + && rm -rf /var/lib/apt/lists/* + +COPY requirements.txt /tmp/requirements.txt +RUN python3 -m pip install --no-cache-dir --upgrade pip \ + && python3 -m pip install --no-cache-dir -r /tmp/requirements.txt + +COPY libmavsdk-dev_3.14.0_ubuntu22.04_amd64.deb /tmp/libmavsdk-dev_3.14.0_ubuntu22.04_amd64.deb +RUN dpkg -i /tmp/libmavsdk-dev_3.14.0_ubuntu22.04_amd64.deb \ + || (apt-get update && apt-get install -y --no-install-recommends -f && rm -rf /var/lib/apt/lists/*) \ + && rm -f /tmp/libmavsdk-dev_3.14.0_ubuntu22.04_amd64.deb + +RUN if ! getent group ${USER_GID} >/dev/null; then groupadd --gid ${USER_GID} ${USERNAME}; fi \ + && if ! id -u ${USERNAME} >/dev/null 2>&1; then useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} -s /bin/bash; fi \ + && usermod -aG sudo,dialout,video,plugdev ${USERNAME} \ + && echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" > /etc/sudoers.d/${USERNAME} \ + && chmod 0440 /etc/sudoers.d/${USERNAME} + +RUN rosdep init || true + +COPY .devcontainer/loop_ascii.txt /etc/loop_ascii.txt + +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /etc/bash.bashrc + +USER ${USERNAME} +RUN touch /home/${USERNAME}/.hushlogin +RUN printf '\nif [[ $- == *i* ]] && [[ -r /etc/loop_ascii.txt ]]; then\n printf '"'"'%%b\\n'"'"' "$(cat /etc/loop_ascii.txt)"\nfi\n' \ + >> /home/${USERNAME}/.bashrc + +WORKDIR /athena-code diff --git a/.devcontainer/jetson/build_docker.sh b/.devcontainer/jetson/build_docker.sh new file mode 100755 index 00000000..606d0329 --- /dev/null +++ b/.devcontainer/jetson/build_docker.sh @@ -0,0 +1,17 @@ +#!/usr/bin/env bash +set -euo pipefail + +config_dir="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)" +repo_root="$(cd -- "${config_dir}/../.." && pwd)" +machine_type="$(basename -- "${config_dir}")" +image_name="athena-code:${machine_type}" + +docker build \ + --file "${config_dir}/Dockerfile" \ + --build-arg "MACHINE_TYPE=${machine_type}" \ + --build-arg "USER_UID=$(id -u)" \ + --build-arg "USER_GID=$(id -g)" \ + --tag "${image_name}" \ + "${repo_root}" + +echo "Built ${image_name}" diff --git a/.devcontainer/jetson/devcontainer.json b/.devcontainer/jetson/devcontainer.json new file mode 100644 index 00000000..5f81b21b --- /dev/null +++ b/.devcontainer/jetson/devcontainer.json @@ -0,0 +1,40 @@ +{ + "name": "Athena ROS 2 Humble (Jetson)", + "build": { + "dockerfile": "Dockerfile", + "context": "../..", + "args": { + "MACHINE_TYPE": "jetson" + } + }, + "remoteUser": "loop_dev", + "updateRemoteUserUID": true, + "workspaceFolder": "/athena-code", + "workspaceMount": "source=${localWorkspaceFolder},target=/athena-code,type=bind,consistency=cached", + "postCreateCommand": ".devcontainer/jetson/post_create.sh", + "runArgs": [ + "--network=host", + "--ipc=host", + "--privileged" + ], + "containerEnv": { + "ROS_DISTRO": "humble", + "RMW_IMPLEMENTATION": "rmw_fastrtps_cpp", + "ATHENA_MACHINE_TYPE": "jetson" + }, + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cpptools", + "ms-vscode.cmake-tools", + "ms-python.python", + "ms-iot.vscode-ros", + "openai.chatgpt", + "cweijan.vscode-office", + "esben.prettier-vscode", + "redhat.vscode-xml", + "tamasfe.even-better-toml" + ] + } + } +} diff --git a/.devcontainer/jetson/post_create.sh b/.devcontainer/jetson/post_create.sh new file mode 100755 index 00000000..6a2e0ee0 --- /dev/null +++ b/.devcontainer/jetson/post_create.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +set +u +source "/opt/ros/${ROS_DISTRO}/setup.bash" +set -u + +sudo apt-get update +sudo rosdep update +sudo rosdep install -r --from-paths src --ignore-src --rosdistro "$ROS_DISTRO" -y + +colcon build --symlink-install diff --git a/.devcontainer/jetson/run_docker.sh b/.devcontainer/jetson/run_docker.sh new file mode 100755 index 00000000..04b61c4a --- /dev/null +++ b/.devcontainer/jetson/run_docker.sh @@ -0,0 +1,33 @@ +#!/usr/bin/env bash +set -euo pipefail + +config_dir="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)" +repo_root="$(cd -- "${config_dir}/../.." && pwd)" +machine_type="$(basename -- "${config_dir}")" +image_name="athena-code:${machine_type}" +command=("$@") +if [[ ${#command[@]} -eq 0 ]]; then + command=(bash) +fi + +docker_args=( + --rm + --interactive + --tty + --network host + --ipc host + --privileged + --env "ROS_DISTRO=humble" + --env "RMW_IMPLEMENTATION=rmw_fastrtps_cpp" + --volume "${repo_root}:/athena-code" + --workdir /athena-code +) + +if [[ -n "${DISPLAY:-}" && -d /tmp/.X11-unix ]]; then + docker_args+=( + --env "DISPLAY=${DISPLAY}" + --volume /tmp/.X11-unix:/tmp/.X11-unix:rw + ) +fi + +docker run "${docker_args[@]}" "${image_name}" "${command[@]}" diff --git a/.devcontainer/loop_ascii.txt b/.devcontainer/loop_ascii.txt new file mode 100644 index 00000000..42131f85 --- /dev/null +++ b/.devcontainer/loop_ascii.txt @@ -0,0 +1,5 @@ +\033[31m ██ ██ ███ ██▒ ██████▌ \033[37m ██▌ ▄▄██▄▄ ▄▄███▄ █████▄ +\033[31m ▓██ ██▌████ ███ ██ ▀███\033[37m▐██ ██▀▀▀███▄█▀▀▀▀██ ▐██ ██ +\033[31m ██▓ ▓██ ████████ ███ ██ \033[37m███ ▐██ ▒███ ██▌██████▀ +\033[31m ██████ ▒█ ▀▀ ██ ▒███████ \033[37m█████▌ ▀██████ ▀██████▀ ██▌ +We make kickass engineers! diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 00000000..9561dc44 --- /dev/null +++ b/.dockerignore @@ -0,0 +1,11 @@ +.git +.agents +.codex +build +install +log +**/__pycache__ +**/.pytest_cache +**/.mypy_cache +**/.ruff_cache +*.pyc diff --git a/.gitmodules b/.gitmodules index e8b4a8ef..2629fe2b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ [submodule "nav2_config"] - path = nav2_config + path = src/subsystems/navigation/nav2_config url = https://github.com/sutharsan-311/nav2_config.git diff --git a/README.md b/README.md index 50d79492..be0126d9 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,50 @@ # Athena Code +![Alt text](docs/title_picture.JPG) + This repository contains all the code for UMDLoop's 2025-26 rover, Athena. When cloning, be sure to use `git clone --recursive [URL to Git repository]` to pull in the submodules found in `src/third-party`. Alternatively, run `git submodule update --init --recursive` if you have already cloned. ## How To Use +### Docker setup + +*Prior to building:* + +Plug in all necessary usb devices. If a new one is plugged in, rebuild the devcontainer. + +*devcontainer:* + +`Ctrl+Shift+P` + +Dev Containers: Rebuild and Reopen in Container + +Choose based on what computer this repository is running on. Developer is general use, the other two are for competition specific systems. + +*no vscode available:* + +1. Navigate to desired system directory (base_station, developer, or jetson) +2. Build the docker + +```bash +./build_docker.sh +docker images +``` + +3. Run the docker + +```bash +./run_docker.sh +docker ps +``` + +*Quick hardware test (joystick controller):* + +```bash +sudo evtest +``` + ### Hardware Setup - Ensure CAN Bus is connected and hardware is configured to the correct IDs @@ -14,136 +53,85 @@ When cloning, be sure to use `git clone --recursive [URL to Git repository]` to ### CAN Setup _Hardware:_ + ```bash -./src/athena-code/src/tools/scripts/can_setup.sh +./src/tools/scripts/can_setup.sh ``` _Virtual:_ + ```bash -./src/athena-code/src/tools/scripts/virtual_can_setup.sh +./src/tools/scripts/virtual_can_setup.sh ``` Use `ip link` to verify that `can0` or `vcan0` is up. +--- + ### Building 1. Install required dependencies: + ```bash rosdep install --from-paths src -y --ignore-src ``` 2. Build the workspace: -```bash -colcon build --symlink-install -``` -3. Source the workspace: ```bash -source install/setup.bash +colcon build --symlink-install ``` ---- - -## Controller Switching - -All subsystems use a controller switching service. Open a new terminal, source the workspace, and call: +3. Source the workspace in every terminal you use files specific to this workspace: ```bash source install/setup.bash -ros2 service call /set_controller msgs/srv/SetController "{controller_names: [CONTROLLER_NAME]}" ``` -Only one motion controller should be active at a time (alongside the `joint_state_broadcaster`). +4. Launch your desired subsystem. --- -## Testing +## Subsystems ### Arm -#### Launch +See the [Arm subsystem README](src/subsystems/arm/README.md). -_Hardware:_ -```bash -ros2 launch arm_bringup athena_arm.launch.py use_mock_hardware:=false -``` +### Drive -_Mock (no hardware):_ -```bash -ros2 launch arm_bringup athena_arm.launch.py use_mock_hardware:=true -``` +See the [Drive subsystem README](src/subsystems/drive/README.md). -#### PS4 Controller +### Science -- **Base Yaw**: Left/Right on Left Joystick (Axes 0) -- **Shoulder Pitch**: Up/Down on Left Joystick (Axes 1) -- **Elbow Pitch**: Up/Down on Right Joystick (Axes 3) -- **Wrist Pitch**: Up/Down on Left Joystick AND O button (Axes 1) -- **Wrist Roll**: Left/Right on Left Joystick AND O button (Axes 0) -- **Open Claw (max speed)**: Left Bumper (Button 5) -- **Close Claw (max speed)**: Right Bumper (Button 4) -- **Open Claw (speed control)**: Left Trigger (Axes 5) -- **Close Claw (speed control)**: Right Trigger (Axes 4) +See the [Science subsystem README](src/subsystems/science/README.md). -#### Velocity Testing +### Navigation -1. Switch to velocity controller using the controller switching service -2. In a new terminal: -```bash -ros2 topic pub /velocity_controller/commands std_msgs/msg/Float64MultiArray "{data: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}" -``` +See the [Navigation subsystem README](src/subsystems/navigation/README.md). -#### Joint Trajectory Testing +### General -1. Switch to joint trajectory controller using the controller switching service -2. In a new terminal: -```bash -ros2 action send_goal /joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory "{ \ - trajectory:{ \ - joint_names: ['base_yaw', 'shoulder_pitch', 'elbow_pitch', 'wrist_pitch', 'wrist_roll', 'gripper_claw', 'actuator'], \ - points: [ \ - {positions: [0.0, -1.0, -2.0, -1.0, -0.16, 0.0, 0.0], time_from_start: {sec: 3, nanosec: 0}}, \ - ] \ - } \ -}" -``` +See the [General subsystem README](src/subsystems/general/README.md). --- -### Drive - -#### Launch - -_Hardware:_ -```bash -ros2 launch drive_bringup athena_drive.launch.py use_mock_hardware:=false -``` - -_Mock (no hardware):_ -```bash -ros2 launch drive_bringup athena_drive.launch.py use_mock_hardware:=true -``` +## Hardware Interfaces -The default controller is `single_ackermann_controller`. To use the built-in ackermann steering controller instead: -```bash -ros2 launch drive_bringup athena_drive.launch.py robot_controller:=ackermann_steering_controller -``` +For a list of available hardware interfaces, see the [Hardware Interfaces README](src/hardware_interfaces/README.md) --- -### Science +## Controller Switching -#### Launch +All subsystems use a controller switching service. Open a new terminal, source the workspace, and call: -_Hardware:_ ```bash -ros2 launch science_bringup athena_science.launch.py use_mock_hardware:=false +source install/setup.bash +ros2 service call /set_controller msgs/srv/SetController "{controller_names: [CONTROLLER_NAME]}" ``` -_Mock (no hardware):_ -```bash -ros2 launch science_bringup athena_science.launch.py use_mock_hardware:=true -``` +Only one motion controller should be active at a time (alongside the `joint_state_broadcaster`). --- @@ -151,7 +139,7 @@ ros2 launch science_bringup athena_science.launch.py use_mock_hardware:=true ### Learning Git -If you're new to Git and GitHub, start with this beginner-friendly tutorial: [GitHub's Hello World Guide](https://docs.github.com/en/get-started/quickstart/hello-world) +If you're new to Git and GitHub, start with this beginner-friendly tutorial: [GitHub's Hello World Guide](https://docs.github.com/en/get-started/quickstart/hello-world) For a more comprehensive introduction, check out: [Git and GitHub Tutorial for Beginners](https://www.freecodecamp.org/news/git-and-github-for-beginners/) @@ -175,17 +163,16 @@ To securely authenticate with GitHub without entering your password each time, s Once you're set up, here's how to contribute your changes: 1. **Fork the repository** - Create your own copy of the repository: [Fork a repo](https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/working-with-forks/fork-a-repo) - 2. **Clone your fork** - Download your forked repository to your local machine using `git clone`. - 3. **Create a feature branch** - Make a new branch for your specific feature or fix: + ```bash git checkout -b feature/your-feature-name ``` 4. **Make your changes** - Write your code, test it thoroughly, and commit your changes - 5. **Push to your fork** - Upload your feature branch to your GitHub fork + ```bash git push origin feature/your-feature-name ``` @@ -193,3 +180,35 @@ git push origin feature/your-feature-name 6. **Open a Pull Request** - Submit your changes for review: [Creating a pull request from a fork](https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/proposing-changes-to-your-work-with-pull-requests/creating-a-pull-request-from-a-fork) Your PR will be reviewed by your lead, and once approved, it will be merged into the main codebase! + +--- + +### Enabling Real Time ros2_control + +```bash +sudo addgroup realtime +sudo usermod -a -G realtime $(whoami) +``` + +Afterwards, add the following limits to the realtime group in `/etc/security/limits.conf`: + +``` +@realtime soft rtprio 99 +@realtime soft priority 99 +@realtime soft memlock unlimited +@realtime hard rtprio 99 +@realtime hard priority 99 +@realtime hard memlock unlimited +``` + +--- + +## Future Work + +- Test docker setup and documentation branch changes on actual rover. +- Add navigation readme. +- Modify `.devcontainer/jetson/Dockerfile` for the Jetson platform and its hardware-specific dependencies. +- Modify `.devcontainer/base_station/Dockerfile` for the base-station machine and its runtime requirements. +- Validate both images on their target machines so they can replace the shared developer setup. +- Test Docker hardware access with a CANable adapter when one is available. +- Fix joystick publisher. diff --git a/docs/arm_breakdown.png b/docs/arm_breakdown.png new file mode 100644 index 00000000..896ac890 Binary files /dev/null and b/docs/arm_breakdown.png differ diff --git a/docs/arm_controls_2dof_wrist_jbj.png b/docs/arm_controls_2dof_wrist_jbj.png new file mode 100644 index 00000000..673e08fb Binary files /dev/null and b/docs/arm_controls_2dof_wrist_jbj.png differ diff --git a/docs/arm_controls_3dof_wrist_jbj.png b/docs/arm_controls_3dof_wrist_jbj.png new file mode 100644 index 00000000..97937f31 Binary files /dev/null and b/docs/arm_controls_3dof_wrist_jbj.png differ diff --git a/docs/arm_controls_arm_jbj.png b/docs/arm_controls_arm_jbj.png new file mode 100644 index 00000000..55304455 Binary files /dev/null and b/docs/arm_controls_arm_jbj.png differ diff --git a/docs/arm_controls_cylindrical.png b/docs/arm_controls_cylindrical.png new file mode 100644 index 00000000..b78b577a Binary files /dev/null and b/docs/arm_controls_cylindrical.png differ diff --git a/docs/arm_controls_endeff_gripper_jbj.png b/docs/arm_controls_endeff_gripper_jbj.png new file mode 100644 index 00000000..27c4c37d Binary files /dev/null and b/docs/arm_controls_endeff_gripper_jbj.png differ diff --git a/docs/cylindrical_controller.gif b/docs/cylindrical_controller.gif new file mode 100644 index 00000000..d5184a98 Binary files /dev/null and b/docs/cylindrical_controller.gif differ diff --git a/docs/drive_controls_rear_ackermann.png b/docs/drive_controls_rear_ackermann.png new file mode 100644 index 00000000..8287314d Binary files /dev/null and b/docs/drive_controls_rear_ackermann.png differ diff --git a/docs/moveit_demo.gif b/docs/moveit_demo.gif new file mode 100644 index 00000000..7111a163 Binary files /dev/null and b/docs/moveit_demo.gif differ diff --git a/docs/science_controls_manual.png b/docs/science_controls_manual.png new file mode 100644 index 00000000..cbdb903e Binary files /dev/null and b/docs/science_controls_manual.png differ diff --git a/docs/title_picture.JPG b/docs/title_picture.JPG new file mode 100644 index 00000000..8d7b90ad Binary files /dev/null and b/docs/title_picture.JPG differ diff --git a/src/README.md b/src/README.md deleted file mode 100644 index 1b0720b9..00000000 --- a/src/README.md +++ /dev/null @@ -1,41 +0,0 @@ -# monorepo-test - -### Install Gazebo -```bash -sudo apt-get install ros-${ROS_DISTRO}-ros-gz -``` - -### Enabling Real Time ros2_control -```bash -sudo addgroup realtime -sudo usermod -a -G realtime $(whoami) -``` - -Afterwards, add the following limits to the realtime group in `/etc/security/limits.conf`: -``` -@realtime soft rtprio 99 -@realtime soft priority 99 -@realtime soft memlock unlimited -@realtime hard rtprio 99 -@realtime hard priority 99 -@realtime hard memlock unlimited -``` - -### Moveit Installs -Install rviz moveit package: -```bash -sudo apt install ros-humble-moveit-ros-visualization -``` - -Download source code for the rest of MoveIt -```bash -cd src -vcs import < moveit2_tutorials/moveit2_tutorials.repos -``` - -Rosdep install -```bash -cd .. -sudo apt update && rosdep install -r --from-paths . --ignore-src --rosdistro $ROS_DISTRO -y -``` - diff --git a/src/bringup/README.md b/src/bringup/README.md index b3b50a9d..ad6c5841 100644 --- a/src/bringup/README.md +++ b/src/bringup/README.md @@ -4,11 +4,11 @@ Mission-level launch files for Athena. Each file composes subsystem launches for ## Modes -| Mode | What runs | -|---|---| -| `jetson` | Hardware and compute nodes — run on the rover | -| `base_station` | Operator teleop nodes — run on the laptop | -| `standalone` | Everything on one machine (default, for development) | +| Mode | What runs | +| ---------------- | ---------------------------------------------------- | +| `jetson` | Hardware and compute nodes — run on the rover | +| `base_station` | Operator teleop nodes — run on the laptop | +| `standalone` | Everything on one machine (default, for development) | --- @@ -18,32 +18,37 @@ Drive + Arm + GPS + Heading ```bash # Rover +source install/setup.bash ros2 launch bringup delivery.launch.py mode:=jetson # Operator laptop +source install/setup.bash ros2 launch bringup delivery.launch.py mode:=base_station # Single machine +source install/setup.bash ros2 launch bringup delivery.launch.py # Mock hardware (no physical rover) +source install/setup.bash ros2 launch bringup delivery.launch.py use_mock_hardware:=true use_sim:=true # 3 DOF wrist +source install/setup.bash ros2 launch bringup delivery.launch.py use_3dof:=true ``` ### Parameters -| Parameter | Default | Choices | Description | -|---|---|---|---| -| `mode` | `standalone` | `standalone` `jetson` `base_station` | Deployment target | -| `use_sim` | `false` | `true` `false` | Launch RViz2 | -| `use_mock_hardware` | `false` | `true` `false` | Mock hardware for drive and arm | -| `mock_sensor_commands` | `false` | `true` `false` | Mock sensor command interfaces (requires `use_mock_hardware`) | -| `robot_controller` | `rear_ackermann_controller` | `rear_ackermann_controller` `front_ackermann_controller` `ackermann_steering_controller` | Drive controller to start | -| `use_3dof` | `false` | `true` `false` | Enable 3 DOF wrist joints on arm | -| `deactivate_talon` | `false` | `true` `false` | Deactivate talon joints in URDF (use with `use_mock_hardware` to prevent CAN traffic) | +| Parameter | Default | Choices | Description | +| ------------------------ | ----------------------------- | ---------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------- | +| `mode` | `standalone` | `standalone` `jetson` `base_station` | Deployment target | +| `use_sim` | `false` | `true` `false` | Launch RViz2 | +| `use_mock_hardware` | `false` | `true` `false` | Mock hardware for drive and arm | +| `mock_sensor_commands` | `false` | `true` `false` | Mock sensor command interfaces (requires`use_mock_hardware`) | +| `robot_controller` | `rear_ackermann_controller` | `rear_ackermann_controller` `front_ackermann_controller` `ackermann_steering_controller` | Drive controller to start | +| `use_3dof` | `false` | `true` `false` | Enable 3 DOF wrist joints on arm | +| `deactivate_talon` | `false` | `true` `false` | Deactivate talon joints in URDF (use with`use_mock_hardware` to prevent CAN traffic) | --- @@ -53,44 +58,51 @@ Drive + Nav2 + ZED + GPS + Localizer + Heading ```bash # Rover +source install/setup.bash ros2 launch bringup autonav.launch.py mode:=jetson # Operator laptop +source install/setup.bash ros2 launch bringup autonav.launch.py mode:=base_station # Single machine +source install/setup.bash ros2 launch bringup autonav.launch.py # Use EKF localizer instead of ZED spatial localization +source install/setup.bash ros2 launch bringup autonav.launch.py use_zed_localizer:=false # Disable GNSS fusion in ZED +source install/setup.bash ros2 launch bringup autonav.launch.py enable_gnss:=false # Custom Nav2 params +source install/setup.bash ros2 launch bringup autonav.launch.py params_file:=/path/to/params.yaml # Debug nav2 verbosity +source install/setup.bash ros2 launch bringup autonav.launch.py log_level:=debug ``` ### Parameters -| Parameter | Default | Choices | Description | -|---|---|---|---| -| `mode` | `standalone` | `standalone` `jetson` `base_station` | Deployment target | -| `use_sim` | `false` | `true` `false` | Enable sim time and RViz2 | -| `use_mock_hardware` | `false` | `true` `false` | Mock hardware for drive | -| `mock_sensor_commands` | `false` | `true` `false` | Mock sensor command interfaces (requires `use_mock_hardware`) | -| `robot_controller` | `rear_ackermann_controller` | `rear_ackermann_controller` `front_ackermann_controller` `ackermann_steering_controller` | Drive controller to start | -| `use_zed_localizer` | `true` | `true` `false` | Use ZED spatial localization instead of EKF localizer | -| `enable_gnss` | `true` | `true` `false` | Enable GNSS fusion inside the ZED camera | -| `use_minimal` | `false` | `true` `false` | Use minimal Nav2 config (empty costmaps, basic BT) for ackermann testing | -| `params_file` | `nav2_params.yaml` (or `nav2_params_minimal.yaml` when `use_minimal:=true`) | — | Full path to Nav2 params YAML; overrides `use_minimal` if set explicitly | -| `use_dem` | `false` | `true` `false` | Enable DEM costmap layer | -| `use_respawn` | `false` | `true` `false` | Respawn nav2 nodes on crash | -| `log_level` | `info` | — | Log level for nav2 nodes | -| `use_config` | `false` | `true` `false` | Launch the Nav2 config GUI | +| Parameter | Default | Choices | Description | +| ------------------------ | --------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------- | +| `mode` | `standalone` | `standalone` `jetson` `base_station` | Deployment target | +| `use_sim` | `false` | `true` `false` | Enable sim time and RViz2 | +| `use_mock_hardware` | `false` | `true` `false` | Mock hardware for drive | +| `mock_sensor_commands` | `false` | `true` `false` | Mock sensor command interfaces (requires`use_mock_hardware`) | +| `robot_controller` | `rear_ackermann_controller` | `rear_ackermann_controller` `front_ackermann_controller` `ackermann_steering_controller` | Drive controller to start | +| `use_zed_localizer` | `true` | `true` `false` | Use ZED spatial localization instead of EKF localizer | +| `enable_gnss` | `true` | `true` `false` | Enable GNSS fusion inside the ZED camera | +| `use_minimal` | `false` | `true` `false` | Use minimal Nav2 config (empty costmaps, basic BT) for ackermann testing | +| `params_file` | `nav2_params.yaml` (or `nav2_params_minimal.yaml` when `use_minimal:=true`) | — | Full path to Nav2 params YAML; overrides`use_minimal` if set explicitly | +| `use_dem` | `false` | `true` `false` | Enable DEM costmap layer | +| `use_respawn` | `false` | `true` `false` | Respawn nav2 nodes on crash | +| `log_level` | `info` | — | Log level for nav2 nodes | +| `use_config` | `false` | `true` `false` | Launch the Nav2 config GUI | --- @@ -100,32 +112,77 @@ Drive + Arm + GPS + Heading ```bash # Rover +source install/setup.bash ros2 launch bringup equipment_servicing.launch.py mode:=jetson # Operator laptop +source install/setup.bash ros2 launch bringup equipment_servicing.launch.py mode:=base_station # Single machine +source install/setup.bash ros2 launch bringup equipment_servicing.launch.py # Mock hardware (no physical rover) +source install/setup.bash ros2 launch bringup equipment_servicing.launch.py use_mock_hardware:=true use_sim:=true # 3 DOF wrist +source install/setup.bash ros2 launch bringup equipment_servicing.launch.py use_3dof:=true ``` ### Parameters -| Parameter | Default | Choices | Description | -|---|---|---|---| -| `mode` | `standalone` | `standalone` `jetson` `base_station` | Deployment target | -| `use_sim` | `false` | `true` `false` | Launch RViz2 | -| `use_mock_hardware` | `false` | `true` `false` | Mock hardware for drive and arm | -| `mock_sensor_commands` | `false` | `true` `false` | Mock sensor command interfaces (requires `use_mock_hardware`) | -| `robot_controller` | `rear_ackermann_controller` | `rear_ackermann_controller` `front_ackermann_controller` `ackermann_steering_controller` | Drive controller to start | -| `use_3dof` | `false` | `true` `false` | Enable 3 DOF wrist joints on arm | -| `deactivate_talon` | `false` | `true` `false` | Deactivate talon joints in URDF (use with `use_mock_hardware` to prevent CAN traffic) | +| Parameter | Default | Choices | Description | +| ------------------------ | ----------------------------- | ---------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------- | +| `mode` | `standalone` | `standalone` `jetson` `base_station` | Deployment target | +| `use_sim` | `false` | `true` `false` | Launch RViz2 | +| `use_mock_hardware` | `false` | `true` `false` | Mock hardware for drive and arm | +| `mock_sensor_commands` | `false` | `true` `false` | Mock sensor command interfaces (requires`use_mock_hardware`) | +| `robot_controller` | `rear_ackermann_controller` | `rear_ackermann_controller` `front_ackermann_controller` `ackermann_steering_controller` | Drive controller to start | +| `use_3dof` | `false` | `true` `false` | Enable 3 DOF wrist joints on arm | +| `deactivate_talon` | `false` | `true` `false` | Deactivate talon joints in URDF (use with`use_mock_hardware` to prevent CAN traffic) | + +--- + +## Science + +Drive + Science + GPS + Heading + LED Indicator + +```bash +# Rover +source install/setup.bash +ros2 launch bringup science.launch.py mode:=jetson + +# Operator laptop +source install/setup.bash +ros2 launch bringup science.launch.py mode:=base_station + +# Single machine +source install/setup.bash +ros2 launch bringup science.launch.py + +# Mock hardware (no physical rover) +source install/setup.bash +ros2 launch bringup science.launch.py use_mock_hardware:=true use_sim:=true + +# Custom CAN interface +source install/setup.bash +ros2 launch bringup science.launch.py can_interface:=vcan0 +``` + +### Parameters + +| Parameter | Default | Choices | Description | +| ------------------------ | ----------------------------- | ---------------------------------------------------------------------------------------------- | ------------------------------------------------------------------ | +| `mode` | `standalone` | `standalone` `jetson` `base_station` | Deployment target | +| `use_sim` | `false` | `true` `false` | Launch RViz2 and use simulated GPS | +| `use_mock_hardware` | `false` | `true` `false` | Mock hardware for drive and science | +| `mock_sensor_commands` | `false` | `true` `false` | Mock sensor command interfaces (requires`use_mock_hardware`) | +| `robot_controller` | `rear_ackermann_controller` | `rear_ackermann_controller` `front_ackermann_controller` `ackermann_steering_controller` | Drive controller to start | +| `deactivate_talon` | `false` | `true` `false` | Deactivate Talon joints in the science URDF to prevent CAN traffic | +| `can_interface` | `can1` | — | CAN interface used by drive and science hardware | --- @@ -133,24 +190,54 @@ ros2 launch bringup equipment_servicing.launch.py use_3dof:=true ### Delivery / Equipment Servicing -| Node group | jetson | base_station | standalone | -|---|:---:|:---:|:---:| -| Drive hardware + controllers | ✓ | | ✓ | -| Drive teleop (joystick) | | ✓ | ✓ | -| Arm hardware + controllers | ✓ | | ✓ | -| Arm teleop (joystick) | | ✓ | ✓ | -| GPS (athena_gps / pixhawk) | ✓ | | ✓ | -| Magnetometer heading | ✓ | | ✓ | +| Node group | jetson | base_station | standalone | +| ---------------------------- | :----: | :----------: | :--------: | +| Drive hardware + controllers | ✓ | | ✓ | +| Drive teleop (joystick) | | ✓ | ✓ | +| Arm hardware + controllers | ✓ | | ✓ | +| Arm teleop (joystick) | | ✓ | ✓ | +| GPS (athena_gps / pixhawk) | ✓ | | ✓ | +| Magnetometer heading | ✓ | | ✓ | ### Autonomous Navigation -| Node group | jetson | base_station | standalone | -|---|:---:|:---:|:---:| -| Drive hardware + controllers | ✓ | | ✓ | -| Drive teleop (joystick) | | ✓ | ✓ | -| ZED camera | ✓ | | ✓ | -| GPS (athena_gps / pixhawk) | ✓ | | ✓ | -| ZED spatial localizer (default) | ✓ | | ✓ | -| EKF localizer (`use_zed_localizer:=false`) | ✓ | | ✓ | -| Nav2 stack | ✓ | | ✓ | -| Magnetometer heading | ✓ | | ✓ | +| Node group | jetson | base_station | standalone | +| -------------------------------------------- | :----: | :----------: | :--------: | +| Drive hardware + controllers | ✓ | | ✓ | +| Drive teleop (joystick) | | ✓ | ✓ | +| ZED camera | ✓ | | ✓ | +| GPS (athena_gps / pixhawk) | ✓ | | ✓ | +| ZED spatial localizer (default) | ✓ | | ✓ | +| EKF localizer (`use_zed_localizer:=false`) | ✓ | | ✓ | +| Nav2 stack | ✓ | | ✓ | +| Magnetometer heading | ✓ | | ✓ | + +### Science + +| Node group | jetson | base_station | standalone | +| ------------------------------ | :----: | :----------: | :--------: | +| Drive hardware + controllers | ✓ | | ✓ | +| Drive teleop (joystick) | | ✓ | ✓ | +| Science hardware + controllers | ✓ | | ✓ | +| Science teleop (joystick) | | ✓ | ✓ | +| GPS (athena_gps / pixhawk) | ✓ | | ✓ | +| Magnetometer heading | ✓ | | ✓ | +| LED indicator | ✓ | | ✓ | + +### Namespace CLI + +#### How do use arm controller manager cli + +During the delivery or equipment servicing missions, the arm controller manager is in a different namespace, so to monitor the controller and hardware status, use this formatting: + +```bash +ros2 control -c /arm/controller_manager +``` + +#### How do use science controller manager cli + +During the science mission, the science controller manager is in a different namespace, so to monitor the controller and hardware status, use this formatting: + +```bash +ros2 control -c /science/controller_manager +``` diff --git a/src/bringup/launch/delivery.launch.py b/src/bringup/launch/delivery.launch.py index eb46eba9..aeeab520 100644 --- a/src/bringup/launch/delivery.launch.py +++ b/src/bringup/launch/delivery.launch.py @@ -94,8 +94,13 @@ def launch_setup(context, *args, **kwargs): # Arm: inlined, namespaced under /arm so its CM/topics don't clash with drive. # The arm subsystem files (URDF, controllers YAML, joystick YAML) are reused as-is. # ------------------------------------------------------------------ - arm_urdf_xacro = os.path.join(description_share, 'urdf', 'athena_arm.urdf.xacro') - arm_controllers_yaml = os.path.join(arm_bringup_share, 'config', 'athena_arm_controllers.yaml') + arm_urdf_xacro = os.path.join(description_share, 'urdf', 'arm', 'athena_arm.urdf.xacro') + use_3dof_bool = use_3dof.lower() == 'true' + arm_controllers_yaml = os.path.join( + arm_bringup_share, + 'config', + 'athena_arm_controllers_3dof.yaml' if use_3dof_bool else 'athena_arm_controllers_2dof.yaml', + ) arm_joystick_yaml = os.path.join(arm_bringup_share, 'config', 'joystick.yaml') arm_urdf = Command([ @@ -109,22 +114,17 @@ def launch_setup(context, *args, **kwargs): ' can_interface:=', can_interface, ' ', ]) - use_3dof_bool = use_3dof.lower() == 'true' - wrist_active = 'manual_3dof_wrist_joint_by_joint_controller' if use_3dof_bool else 'manual_2dof_wrist_joint_by_joint_controller' - wrist_inactive = 'manual_2dof_wrist_joint_by_joint_controller' if use_3dof_bool else 'manual_3dof_wrist_joint_by_joint_controller' - jt_controller = 'threedof_joint_trajectory_controller' if use_3dof_bool else 'twodof_joint_trajectory_controller' - active_controllers = [ 'manual_arm_joint_by_joint_controller', - wrist_active, + 'manual_wrist_joint_by_joint_controller', 'manual_end_effector_gripper_claw_controller', ] inactive_controllers = [ - wrist_inactive, - 'manual_arm_cylindrical_controller', - jt_controller, + 'joint_trajectory_controller', 'arm_velocity_controller', ] + if not use_3dof_bool: + inactive_controllers.insert(0, 'manual_arm_cylindrical_controller') arm_cm = Node( package='controller_manager', diff --git a/src/bringup/launch/equipment_servicing.launch.py b/src/bringup/launch/equipment_servicing.launch.py index 11df3e20..5a56dd36 100644 --- a/src/bringup/launch/equipment_servicing.launch.py +++ b/src/bringup/launch/equipment_servicing.launch.py @@ -94,8 +94,13 @@ def launch_setup(context, *args, **kwargs): # Arm: inlined, namespaced under /arm so its CM/topics don't clash with drive. # The arm subsystem files (URDF, controllers YAML, joystick YAML) are reused as-is. # ------------------------------------------------------------------ - arm_urdf_xacro = os.path.join(description_share, 'urdf', 'athena_arm.urdf.xacro') - arm_controllers_yaml = os.path.join(arm_bringup_share, 'config', 'athena_arm_controllers.yaml') + arm_urdf_xacro = os.path.join(description_share, 'urdf', 'arm', 'athena_arm.urdf.xacro') + use_3dof_bool = use_3dof.lower() == 'true' + arm_controllers_yaml = os.path.join( + arm_bringup_share, + 'config', + 'athena_arm_controllers_3dof.yaml' if use_3dof_bool else 'athena_arm_controllers_2dof.yaml', + ) arm_joystick_yaml = os.path.join(arm_bringup_share, 'config', 'joystick.yaml') arm_urdf = Command([ @@ -109,22 +114,17 @@ def launch_setup(context, *args, **kwargs): ' can_interface:=', can_interface, ' ', ]) - use_3dof_bool = use_3dof.lower() == 'true' - wrist_active = 'manual_3dof_wrist_joint_by_joint_controller' if use_3dof_bool else 'manual_2dof_wrist_joint_by_joint_controller' - wrist_inactive = 'manual_2dof_wrist_joint_by_joint_controller' if use_3dof_bool else 'manual_3dof_wrist_joint_by_joint_controller' - jt_controller = 'threedof_joint_trajectory_controller' if use_3dof_bool else 'twodof_joint_trajectory_controller' - active_controllers = [ 'manual_arm_joint_by_joint_controller', - wrist_active, + 'manual_wrist_joint_by_joint_controller', 'manual_end_effector_gripper_claw_controller', ] inactive_controllers = [ - wrist_inactive, - 'manual_arm_cylindrical_controller', - jt_controller, + 'joint_trajectory_controller', 'arm_velocity_controller', ] + if not use_3dof_bool: + inactive_controllers.insert(0, 'manual_arm_cylindrical_controller') arm_cm = Node( package='controller_manager', diff --git a/src/bringup/launch/science.launch.py b/src/bringup/launch/science.launch.py index 29c6ea79..595c17a3 100644 --- a/src/bringup/launch/science.launch.py +++ b/src/bringup/launch/science.launch.py @@ -96,7 +96,7 @@ def launch_setup(context, *args, **kwargs): # Science: inlined, namespaced under /science so its CM/topics don't clash with drive. # The science subsystem files (URDF, controllers YAML, joystick YAML) are reused as-is. # ------------------------------------------------------------------ - science_urdf_xacro = os.path.join(description_share, 'urdf', 'athena_science.urdf.xacro') + science_urdf_xacro = os.path.join(description_share, 'urdf', 'science', 'athena_science.urdf.xacro') science_controllers_yaml = os.path.join(science_bringup_share, 'config', 'athena_science_controllers.yaml') science_joystick_yaml = os.path.join(science_bringup_share, 'config', 'joystick.yaml') @@ -196,4 +196,4 @@ def spawner(name, inactive=False): elif mode == 'base_station': return [drive, science_joystick] else: # standalone - return [drive, science_hardware, science_joystick, gps, heading, led] \ No newline at end of file + return [drive, science_hardware, science_joystick, gps, heading, led] diff --git a/src/bringup/scripts/controller_switcher.py b/src/bringup/scripts/controller_switcher.py index 2f8c3736..80c0fc76 100755 --- a/src/bringup/scripts/controller_switcher.py +++ b/src/bringup/scripts/controller_switcher.py @@ -40,7 +40,12 @@ def __init__(self): self.client_cb_group = MutuallyExclusiveCallbackGroup() # Controllers to always keep active or ignore - self.always_active = ["joint_state_broadcaster", "motor_status_broadcaster"] + self.always_active = ["joint_state_broadcaster", "motor_status_controller"] + if self.subsystem == 'arm': + self.always_active.extend([ + "cam_position_controller", + "rotary_encoder_state_request_controller", + ]) self.ignore_controllers = [] # Lock to prevent concurrent service processing @@ -149,9 +154,10 @@ def _handle_set_controller(self, request, response): controllers_to_deactivate = [] - # Always ensure joint_state_broadcaster is active - if "joint_state_broadcaster" not in active_controllers and "joint_state_broadcaster" not in controllers_to_activate: - controllers_to_activate.append("joint_state_broadcaster") + # Ensure every loaded protected controller remains active. + for controller in self.always_active: + if controller in all_controllers and controller not in active_controllers: + controllers_to_activate.append(controller) def controller_handler(controller, subsystem_controllers): @@ -325,4 +331,4 @@ def main(args=None): if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/src/description/config/odrive.ros2_control.xacro b/src/description/config/odrive.ros2_control.xacro deleted file mode 100644 index 901cc064..00000000 --- a/src/description/config/odrive.ros2_control.xacro +++ /dev/null @@ -1,42 +0,0 @@ - - - - - - - - odrive_ros2_control_plugin/ODriveHardwareInterface - ${can_interface} - - - - - - - - diff --git a/src/description/launch/display.launch.py b/src/description/launch/display.launch.py index 60abf239..bbff0b0a 100644 --- a/src/description/launch/display.launch.py +++ b/src/description/launch/display.launch.py @@ -21,6 +21,7 @@ def generate_launch_description(): robot_description_path = PathJoinSubstitution([ FindPackageShare("description"), "urdf", + "arm", "athena_arm.urdf.xacro" ]) diff --git a/src/description/ros2_control/arm/arm.mock.ros2_control.xacro b/src/description/ros2_control/arm/arm.mock.ros2_control.xacro new file mode 100644 index 00000000..18183062 --- /dev/null +++ b/src/description/ros2_control/arm/arm.mock.ros2_control.xacro @@ -0,0 +1,251 @@ + + + + + + + + mock_components/GenericSystem + true + + false + false + true + 0.0 + + + + + + + + + + + + 0.0 + + + + + + + + + + + + + + + + + 0.0 + + + + + + + + + + + + + + + + + 0.0 + + + + + + + + + + + + + + + + + 0.0 + + + + + + + + + + + + + + + + + 0.0 + + + + + + + + + + + + + 0.0 + + + + + + + + + + + + + 0.0 + + + + + + + + + + + + + 0.0 + + + + + + + + + + + + + 0.0 + + + + + + + + + + + + + + + + + 0.0 + + + + + + + + + + + + + + + 0.0 + + + + + + + + + + + + + + + 0.0 + + + + + + + + + 0.0 + + + + + + + + 0.0 + + + + + + + + 0.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro b/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro index 4dc31b49..4914cb5d 100644 --- a/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro @@ -1,8 +1,6 @@ - - - + ${deactivate_odrive} @@ -18,36 +16,65 @@ - - 7 - 26 - - - - - - - - - + + + 6 + 26 + + + + + + + + + - - 5 - 26 - - - - - - - - - + + 4 + 26 + + + + + + + + + + - + + + 7 + 26 + + + + + + + + + + + 5 + 26 + + + + + + + + + + + - - + \ No newline at end of file diff --git a/src/description/ros2_control/science/science.dc.ros2_control.xacro b/src/description/ros2_control/science/science.dc.ros2_control.xacro index 32f3bc06..001be4c9 100644 --- a/src/description/ros2_control/science/science.dc.ros2_control.xacro +++ b/src/description/ros2_control/science/science.dc.ros2_control.xacro @@ -1,10 +1,7 @@ - - - dc_ros2_control/DCHardwareInterface 20 @@ -53,9 +50,6 @@ --> - - - diff --git a/src/description/ros2_control/science/science.laser.ros2_control.xacro b/src/description/ros2_control/science/science.laser.ros2_control.xacro index 6fea9233..f8217dae 100644 --- a/src/description/ros2_control/science/science.laser.ros2_control.xacro +++ b/src/description/ros2_control/science/science.laser.ros2_control.xacro @@ -1,10 +1,7 @@ - - - laser_ros2_control/LaserHardwareInterface ${can_interface} @@ -22,9 +19,6 @@ - - - diff --git a/src/description/ros2_control/science/science.mock.ros2_control.xacro b/src/description/ros2_control/science/science.mock.ros2_control.xacro index 2a25a88a..ec94c917 100644 --- a/src/description/ros2_control/science/science.mock.ros2_control.xacro +++ b/src/description/ros2_control/science/science.mock.ros2_control.xacro @@ -1,8 +1,6 @@ - - mock_components/GenericSystem @@ -85,9 +83,6 @@ - - - + - \ No newline at end of file diff --git a/src/description/ros2_control/science/science.stepper.ros2_control.xacro b/src/description/ros2_control/science/science.stepper.ros2_control.xacro index 135152c6..e34bcdee 100644 --- a/src/description/ros2_control/science/science.stepper.ros2_control.xacro +++ b/src/description/ros2_control/science/science.stepper.ros2_control.xacro @@ -1,10 +1,7 @@ - - - stepper_ros2_control/STEPPERHardwareInterface 10 @@ -47,9 +44,6 @@ - - - diff --git a/src/description/urdf/athena_arm.urdf.xacro b/src/description/urdf/arm/athena_arm.urdf.xacro similarity index 93% rename from src/description/urdf/athena_arm.urdf.xacro rename to src/description/urdf/arm/athena_arm.urdf.xacro index 187bc098..57abd793 100644 --- a/src/description/urdf/athena_arm.urdf.xacro +++ b/src/description/urdf/arm/athena_arm.urdf.xacro @@ -14,10 +14,10 @@ - + - + @@ -53,7 +53,7 @@ - + diff --git a/src/description/urdf/athena_arm_description.urdf.xacro b/src/description/urdf/arm/athena_arm_description.urdf.xacro similarity index 100% rename from src/description/urdf/athena_arm_description.urdf.xacro rename to src/description/urdf/arm/athena_arm_description.urdf.xacro diff --git a/src/description/urdf/depth_camera.xacro b/src/description/urdf/components/depth_camera.xacro similarity index 100% rename from src/description/urdf/depth_camera.xacro rename to src/description/urdf/components/depth_camera.xacro diff --git a/src/description/urdf/gps.xacro b/src/description/urdf/components/gps.xacro similarity index 100% rename from src/description/urdf/gps.xacro rename to src/description/urdf/components/gps.xacro diff --git a/src/description/urdf/ground_truth_odom.xacro b/src/description/urdf/components/ground_truth_odom.xacro similarity index 100% rename from src/description/urdf/ground_truth_odom.xacro rename to src/description/urdf/components/ground_truth_odom.xacro diff --git a/src/description/urdf/imu.xacro b/src/description/urdf/components/imu.xacro similarity index 100% rename from src/description/urdf/imu.xacro rename to src/description/urdf/components/imu.xacro diff --git a/src/description/urdf/common/inertials.xacro b/src/description/urdf/components/inertials.xacro similarity index 100% rename from src/description/urdf/common/inertials.xacro rename to src/description/urdf/components/inertials.xacro diff --git a/src/description/urdf/magnetometer.xacro b/src/description/urdf/components/magnetometer.xacro similarity index 100% rename from src/description/urdf/magnetometer.xacro rename to src/description/urdf/components/magnetometer.xacro diff --git a/src/description/urdf/common/materials.xacro b/src/description/urdf/components/materials.xacro similarity index 100% rename from src/description/urdf/common/materials.xacro rename to src/description/urdf/components/materials.xacro diff --git a/src/description/urdf/zed_camera.xacro b/src/description/urdf/components/zed_camera.xacro similarity index 96% rename from src/description/urdf/zed_camera.xacro rename to src/description/urdf/components/zed_camera.xacro index b8d44419..34749fe6 100644 --- a/src/description/urdf/zed_camera.xacro +++ b/src/description/urdf/components/zed_camera.xacro @@ -1,7 +1,7 @@ - + @@ -30,7 +31,7 @@ - + @@ -78,7 +79,11 @@ - + diff --git a/src/description/urdf/athena_drive_description.urdf.xacro b/src/description/urdf/drive/athena_drive_description.urdf.xacro similarity index 100% rename from src/description/urdf/athena_drive_description.urdf.xacro rename to src/description/urdf/drive/athena_drive_description.urdf.xacro diff --git a/src/description/urdf/athena_drive/athena_drive_macro.ros2_control.xacro b/src/description/urdf/drive/athena_drive_macro.ros2_control.xacro similarity index 100% rename from src/description/urdf/athena_drive/athena_drive_macro.ros2_control.xacro rename to src/description/urdf/drive/athena_drive_macro.ros2_control.xacro diff --git a/src/description/urdf/athena_drive/athena_drive_macro.xacro b/src/description/urdf/drive/athena_drive_macro.xacro similarity index 98% rename from src/description/urdf/athena_drive/athena_drive_macro.xacro rename to src/description/urdf/drive/athena_drive_macro.xacro index e64baee7..ba86d452 100644 --- a/src/description/urdf/athena_drive/athena_drive_macro.xacro +++ b/src/description/urdf/drive/athena_drive_macro.xacro @@ -1,8 +1,8 @@ - - + + diff --git a/src/description/urdf/athena_science.urdf.xacro b/src/description/urdf/science/athena_science.urdf.xacro similarity index 100% rename from src/description/urdf/athena_science.urdf.xacro rename to src/description/urdf/science/athena_science.urdf.xacro diff --git a/src/hardware_interfaces/README.md b/src/hardware_interfaces/README.md index 60fea846..1daf93bc 100644 --- a/src/hardware_interfaces/README.md +++ b/src/hardware_interfaces/README.md @@ -4,12 +4,15 @@ This directory contains ROS2 Control hardware interface implementations for vari ## Available Interfaces -- `power_module_ros2_control/` - PowerModule hardware interface -- `laser_ros2_control/` - Laser hardware interface -- `led_ros2_control/` - LED hardware interface -- `rmd_ros2_control/` - RMD motor controller interface -- `ros_odrive/` - ODrive motor controller interface -- `servo_ros2_control/` - Servo motor controller interface -- `smc_ros2_control/` - SMC motor controller interface -- `stepper_ros2_control/` - Stepper motor controller interface -- `talon_ros2_control/` - Talon motor controller interface +- `dc_ros2_control` - DC Motor hardware interface +- `laser_ros2_control` - Laser hardware interface +- `led_ros2_control` - LED hardware interface +- `limit_switch_ros2_control` - Limit Switch hardware interface +- `power_module_ros2_control` - PowerModule hardware interface +- `rmd_ros2_control` - RMD motor controller hardware interface +- `ros_odrive` - ODrive motor controller interface. Reference https://github.com/odriverobotics/ros_odrive +- `rotary_encoder_ros2_control` - rs485 rotary encoder hardware interface +- `servo_ros2_control` - Servo motor hardware interface +- `smc_ros2_control` - SMC motor controller hardware interface +- `stepper_ros2_control` - Stepper motor hardware interface +- `talon_ros2_control` - Talon motor controller hardware interface \ No newline at end of file diff --git a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp index de6d4d0b..61cd42fb 100644 --- a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp +++ b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp @@ -25,6 +25,7 @@ namespace rmd_ros2_control // -- HELPER FUNCTIONS -- // +// TODO: Remove the 0.01 dps conversion factor, we are not using double RMDHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ // Converts from 0.01 deg to deg to radians/s return (motor_position * 0.01 * (M_PI/180.0))/gear_ratio; diff --git a/src/infrastructure/umdloop_can/setup.py b/src/infrastructure/umdloop_can/setup.py index 7f6264b3..ded9f5f8 100644 --- a/src/infrastructure/umdloop_can/setup.py +++ b/src/infrastructure/umdloop_can/setup.py @@ -20,7 +20,6 @@ maintainer_email='loop@adriancires.com', description='ROS2 CAN Package for UMD Loop', license='Apache-2.0', - tests_require=['pytest'], entry_points={ 'console_scripts': [ 'can_node = umdloop_can.can_node:main', diff --git a/src/simulation/launch/spawn.launch.py b/src/simulation/launch/spawn.launch.py index adee51c6..e1aa788b 100644 --- a/src/simulation/launch/spawn.launch.py +++ b/src/simulation/launch/spawn.launch.py @@ -31,7 +31,7 @@ def generate_launch_description(): pkg_description = get_package_share_directory('description') pkg_sim = get_package_share_directory('simulation') - urdf_file = os.path.join(pkg_description, 'urdf', 'athena_drive.urdf.xacro') + urdf_file = os.path.join(pkg_description, 'urdf', 'drive', 'athena_drive.urdf.xacro') controllers_file = os.path.join(pkg_description, 'config', 'athena_drive_sim_controllers.yaml') rviz_config_file = os.path.join(pkg_sim, 'rviz', 'sim.rviz') diff --git a/src/subsystems/arm/README.md b/src/subsystems/arm/README.md index a1a91fc3..a96c1364 100644 --- a/src/subsystems/arm/README.md +++ b/src/subsystems/arm/README.md @@ -1,164 +1,219 @@ -# ROS2 workspace for UMD Loop's Arm software integration +# UMD Loop's Arm software integration -## Current package list in arm-control: +## Current package list in arm subsystem:
    -
  • actuator_ros2_control: contains ros2_control hardware interface for servo motors. Currently Empty.
    • -
  • athena_arm_bringup: contains launch file for ros2_control and additional config files for rviz and controllers
    • -
  • athena_arm_controllers: contains athena arm manual controller
    • -
  • athena_arm_description: contains the full urdf file for the arm including ros2_control urdfs
    • +
  • athena_arm_bringup: contains launch files for ros2_control, teleop, rviz, and moveit with additional launch config files
    • +
  • athena_arm_controllers: contains custom arm controllers
  • athena_arm_moveit: contains athena arm moveit setup
    • -
  • athena_arm_msgs: contains messages used for athena mainly for logging
    • -
  • manual_control: testing setup to control each motor on the arm with a PS4 controller. We now have a controller in the ros2_control setup that does the same thing
    • -
  • ros_odrive: contains node and ros2_control hardware interface for ODrives.
    • -
  • smc_ros2_control: contains ros2_control hardware interface for SMC motors
    • -
  • talon_ros2_control: contains ros2_control hardware interface for Pololu motors using Talon motor controller
    • -
  • umdloop_can: contains can_node
    • -
  • umdloop_theseus_can_messages: contains custom messages for can_node
-## How to use: - -### Keep in Mind -- Data should be passed in meters for prismatic joints, radians for revolute -- ENSURE THAT THE AMOUNT OF VALUES PASSED IN DATA MATCHES THE AMOUNT OF INTERFACES AKA JOINTS -- Home position (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) has the arm outstretched parallel to the ground -- Safe position: (0.0 -1.0 -1.8 -0.55 0.0) -- Don't activate multiple controllers at once, you should only have one controller and the joint state broadcaster active at a given time - --- -### Hardware Setup: +### Launch Modes -- Ensure CAN Bus and controller (if using manual control) are connected -- Make sure hardware is configured to the correct ids - - Manual: go into config/manual_control.yaml, ensure all the ids are correct - - ros2_control: go into athena_arm_description/ros2_control and check that each of the ros2_control files have the correct ids inside of the parameter "node_id" for the appropriate joint +`athena_arm.launch.py` accepts a `mode` argument that controls which nodes are started. ---- +By default (`mode:=standalone`), all nodes run on a single machine. For competition, split across two machines using the `mode` argument: -### Software Setup: +- `standalone` (default): starts all nodes on one machine (control + joystick) +- `jetson`: starts only the control and hardware nodes, to be run on the rover +- `base_station`: starts only the joystick node, to be run on the base station -#### Workspace setup: ```bash -./submodule_commands.sh -./clean.sh +source install/setup.bash +ros2 launch arm_bringup athena_arm.launch.py ``` -#### Live Hardware setup: ```bash -./can_setup.sh +source install/setup.bash +ros2 launch arm_bringup athena_arm.launch.py mode:=jetson can_interface:=can1 ``` -#### Virtual setup: ```bash -./virtual_can_setup.sh +source install/setup.bash +ros2 launch arm_bringup athena_arm.launch.py mode:=base_station ``` -Use *ip link* to check that there is a can0/vcan0. - --- +### Other Launch Parameters -### Launch Modes - -`athena_arm.launch.py` accepts a `mode` argument that controls which nodes are started. +#### Mock Hardware -By default (`mode:=standalone`), all nodes run on a single machine. For competition, split across two machines using the `mode` argument: +Allows user to implement mock hardware components which will allow for virtual testing. -- `standalone` (default): starts all nodes on one machine (control + joystick) -- `jetson`: starts only the control and hardware nodes, to be run on the rover -- `base_station`: starts only the joystick node, to be run on the base station +_Hardware:_ ```bash -ros2 launch arm_bringup athena_arm.launch.py +source install/setup.bash +ros2 launch arm_bringup athena_arm.launch.py use_mock_hardware:=false ``` +_Mock (no hardware):_ + ```bash -ros2 launch arm_bringup athena_arm.launch.py mode:=jetson +source install/setup.bash +ros2 launch arm_bringup athena_arm.launch.py use_mock_hardware:=true ``` +#### Simulation Mode (WIP) + +Currently this repository does not support a fully implemented simulated hardware environment for the entire rover. The `use_sim` argument controls only the activation of RViz. + +_Deactivates RViz:_ + ```bash -ros2 launch arm_bringup athena_arm.launch.py mode:=base_station +source install/setup.bash +ros2 launch arm_bringup athena_arm.launch.py use_sim:=false ``` ---- +#### Talon Deactivation + +The Talon motor controllers overflow the CANBus, it can be helpful to debug with them off. -### How to use Control Switching +_Deactivates Talons:_ -**Open another terminal, source the workspace, and call the service to set controllers:** ```bash source install/setup.bash -ros2 service call /set_controller msgs/srv/SetController "{controller_names: [INCLUDE CONTROLLER(S) YOU WANT WITHIN BRACKETS]}" +ros2 launch arm_bringup athena_arm.launch.py deactivate_talon:=false ``` -Example: `ros2 service call /set_controller msgs/srv/SetController "{controller_names: [joint_trajectory_controller, arm_velocity_controller]}"` ---- +#### 3 Degrees of Freedom Mode -### Testing: +There are currently 2 variations of the wrist, 2DOF and 3DOF. Each require a different hardware configuration and load up different controllers as a result. Currently, 2DOF is the default implementation. -#### Manual testing: +_3DOF Mode:_ -From the workspace source, run: ```bash -./manual.sh +source install/setup.bash +ros2 launch arm_bringup athena_arm.launch.py use_3dof:=true ``` -#### PS4 Controller: +#### CAN Interface + +The Jetson currently uses a CANable to implement CAN, not its own CAN controller. This results in 2 can interfaces, can0 and can1, with the current working implementation using the latter. + +_CAN Interface:_ -- **Base Yaw**: Left/Right on Left Joystick (Axes 0) -- **Shoulder Pitch**: Up/Down on Left Joystick (Axes 1) -- **Elbow Pitch**: Up/Down on Right Joystick (Axes 3) +```bash +source install/setup.bash +ros2 launch arm_bringup athena_arm.launch.py can_interface:=can1 +``` -- **Wrist Pitch**: Up/Down on Left Joystick AND O button (Axes 1) -- **Wrist Roll**: Left/Right on Left Joystick AND O button (Axes 0) +### Arm Breakdown -- **Open Claw (max speed)**: Left Bumper (Button 5) -- **Close Claw (max speed)**: Right Bumper (Button 4) -- **Open Claw (speed control)**: Left Trigger (Axes 5) -- **Close Claw (speed control)**: Right Trigger (Axes 4) +![Alt text](../../../docs/arm_breakdown.png) - -#### Thrustmaster (not tested): +### Testing -- **Base Yaw**: Rotate Stick (Axes 0) -- **Shoulder Pitch**: Pull Back Stick (Axes 1) -- **Elbow Pitch**: Pull Back Stick AND press trigger (Axes 1 and Button 0) +#### Manual Controller Testing + +1. Make sure to plug in joystick controller +2. Open a new terminal +3. Switch to desired controller + - Joint by Joint + - Cylindrical + - Joint Trajectory -- **Wrist Pitch**: Pull Back Stick AND press top button on stick (Axes 1) -- **Wrist Roll**: Left/Right on Stick (Axes 2) +##### ROS2 Controller: Arm - Joint by Joint -- **Close Claw**: Left Side, Top Left Button (Button 4) -- **Open Claw**: Right Side, Top Middle Button (Button 5) +![Alt text](../../../docs/arm_controls_arm_jbj.png) +##### ROS2 Controller: Wrist - 2DOF - Joint by Joint +![Alt text](../../../docs/arm_controls_2dof_wrist_jbj.png) -#### ROS2 control testing: +##### ROS2 Controller: End Effector - Gripper Claw - Joint by Joint -*Hardware*: ```bash source install/setup.bash -ros2 launch athena_arm_bringup athena_arm_ros2_control.launch.py use_mock_hardware:=False +ros2 service call /set_controller msgs/srv/SetController "{ + controller_names: [ + manual_arm_joint_by_joint_controller, + manual_wrist_joint_by_joint_controller, + manual_end_effector_gripper_claw_controller + ] +}" ``` -*Mock*: +##### ROS2 Controller: Cylindrical (Work in Progress) + +**Concept Demo:** + +![Alt text](../../../docs/cylindrical_controller.gif) + +**Controls:** +![Alt text](../../../docs/arm_controls_cylindrical.png) + ```bash source install/setup.bash -ros2 launch athena_arm_bringup athena_arm_ros2_control.launch.py use_mock_hardware:=True +ros2 service call /set_controller msgs/srv/SetController "{ + controller_names: [ + manual_arm_cylindrical_controller + ] +}" +``` + +##### ROS2 Controller: Wrist - 3DOF - Joint by Joint + +![Alt text](../../../docs/arm_controls_3dof_wrist_jbj.png) + +``` +source install/setup.bash +ros2 service call /set_controller msgs/srv/SetController "{ + controller_names: [ + manual_arm_joint_by_joint_controller, + manual_wrist_joint_by_joint_controller, + manual_end_effector_gripper_claw_controller + ] +}" +``` + +#### Autonomous Controller Testing + +##### Velocity Testing + +1. Launch (Virtual Example): + +``` +./src/tools/scripts/virtual_can_setup.sh +source install/setup.bash +ros2 launch arm_bringup athena_arm.launch.py can_interface:=vcan0 use_mock_hardware:=true +``` + +2. Open a separate terminal and switch to arm velocity controller using the controller switching service: + +``` +source install/setup.bash +ros2 service call /set_controller msgs/srv/SetController "{controller_names: [arm_velocity_controller]}" ``` -#### Velocity Testing: -1. Open a new terminal -2. Switch to velocity controller +3. In this same terminal: + ```bash -ros2 topic pub /velocity_controller/commands std_msgs/msg/Float64MultiArray "{data: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}" +ros2 topic pub --rate 20 /arm_velocity_controller/commands std_msgs/msg/Float64MultiArray "{data: [0.05, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0]}" +``` + +##### Joint Trajectory Testing + +1. Launch (Virtual example): + +``` +./src/tools/scripts/virtual_can_setup.sh +source install/setup.bash +ros2 launch arm_bringup athena_arm.launch.py can_interface:=vcan0 use_mock_hardware:=true +``` + +2. Open a separate terminal and switch to joint trajectory controller using the controller switching service: + +``` +source install/setup.bash +ros2 service call /set_controller msgs/srv/SetController "{controller_names: [joint_trajectory_controller]}" ``` +3. In this same terminal: -#### Joint Trajectory Testing: -1. Open a new terminal -2. Switch to joint trajectory controller -3. Use the below command ```bash ros2 action send_goal /joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory "{ \ trajectory:{ \ @@ -170,13 +225,23 @@ ros2 action send_goal /joint_trajectory_controller/follow_joint_trajectory contr }" ``` -#### Manual Controller Testing +##### Virtual MoveIt Testing: -1. Make sure to plug in joystick controller -2. Open a new terminal -3. Switch to desired controller - - Joint by Joint - - Cylindrical +![Alt text](../../../docs/moveit_demo.gif) ---- +1. Launch: + +``` +./src/tools/scripts/virtual_can_setup.sh +source install/setup.bash +ros2 launch arm_bringup athena_arm.launch.py use_moveit:=true can_interface:=vcan0 use_mock_hardware:=true +``` +2. Open a separate terminal and switch to joint trajectory controller using the controller switching service: + +``` +source install/setup.bash +ros2 service call /set_controller msgs/srv/SetController "{controller_names: [joint_trajectory_controller]}" +``` + +--- diff --git a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers_2dof.yaml similarity index 63% rename from src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml rename to src/subsystems/arm/arm_bringup/config/athena_arm_controllers_2dof.yaml index 6426cee0..77d0dcb3 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers_2dof.yaml @@ -5,11 +5,9 @@ joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - twodof_joint_trajectory_controller: + joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController - threedof_joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController arm_velocity_controller: type: velocity_controllers/JointGroupVelocityController @@ -20,29 +18,41 @@ manual_arm_joint_by_joint_controller: type: arm_controllers/ManualArmJointByJointController - manual_2dof_wrist_joint_by_joint_controller: + manual_wrist_joint_by_joint_controller: type: arm_controllers/Manual2DOFWristJointByJointController - manual_3dof_wrist_joint_by_joint_controller: - type: arm_controllers/Manual3DOFWristJointByJointController manual_end_effector_gripper_claw_controller: type: arm_controllers/ManualEndEffectorGripperClawController - two_dof_motor_status_controller: + motor_status_controller: type: general_controllers/MotorStatusController - three_dof_motor_status_controller: - type: general_controllers/MotorStatusController cam_position_controller: type: position_controllers/JointGroupPositionController rotary_encoder_state_request_controller: type: forward_command_controller/ForwardCommandController - -/**/twodof_joint_trajectory_controller: +/**/joint_state_broadcaster: + ros__parameters: + # Only publish joints represented in the kinematic URDF. Hardware-only + # resources (cameras, encoders, and limit switches) are exposed by their + # dedicated controllers and must not be sent to MoveIt's state monitor. + joints: + - base_yaw + - shoulder_pitch + - elbow_pitch + - wrist_pitch + - wrist_roll + - gripper_claw + - actuator + interfaces: + - position + - velocity + +/**/joint_trajectory_controller: ros__parameters: joints: - base_yaw @@ -94,41 +104,6 @@ # trajectory: 0.05 # goal: 0.03 -/**/threedof_joint_trajectory_controller: - ros__parameters: - joints: - - base_yaw - - shoulder_pitch - - elbow_pitch - - wrist_motor_a - - wrist_motor_b - - wrist_motor_c - - gripper_claw - - actuator - - command_interfaces: - - position - - state_interfaces: - - position - - velocity - - action_monitor_rate: 20.0 # Rate to monitor status changes when the controller is executing action - - allow_partial_joints_goal: true - - # Open loop:The controller ignores the states provided by hardware interface but using - # last commands as states for starting the trajectory interpolation - # deactivates the feedback control - open_loop_control: true - constraints: - # Velocity tolerance for at the end of the trajectory that indicates that controlled system is stopped. - stopped_velocity_tolerance: 0.01 - - # Time tolerance for achieving trajectory goal before or after commanded time. - # If set to zero, the controller will wait a potentially infinite amount of time. - goal_time: 10.0 - /**/arm_velocity_controller: ros__parameters: joints: @@ -185,7 +160,7 @@ - 0.05148721 virtual_four_bar_coupling_ratio: 1.0 -/**/manual_2dof_wrist_joint_by_joint_controller: +/**/manual_wrist_joint_by_joint_controller: ros__parameters: joints: - wrist_pitch @@ -195,18 +170,6 @@ - 0.785398 # 45 dps - 0.785398 -/**/manual_3dof_wrist_joint_by_joint_controller: - ros__parameters: - joints: - - wrist_motor_a - - wrist_motor_b - - wrist_motor_c - # interface_name: velocity - joint_max_velocities: - - 0.785398 # TODO - - 0.785398 # TODO - - 0.785398 # TODO - /**/manual_end_effector_gripper_claw_controller: ros__parameters: velocity_joints: @@ -231,7 +194,7 @@ # - motor_temperature # - torque_current # - status -/**/two_dof_motor_status_controller: +/**/motor_status_controller: ros__parameters: joints: - base_yaw @@ -294,77 +257,3 @@ interfaces: - status publish_rate: 10.0 - -/**/three_dof_motor_status_controller: - ros__parameters: - joints: - - base_yaw - - shoulder_pitch - - elbow_pitch - - wrist_motor_a - - wrist_motor_b - - wrist_motor_c - - actuator - - gripper_claw - - cam_0 - - cam_1 - gpios: - - limit_switch_0 - - limit_switch_1 - - limit_switch_2 - - limit_switch_3 - - limit_switch_4 - command_interfaces: - gripper_claw: - interfaces: - - status_request - wrist_motor_a: - interfaces: - - status_request - wrist_motor_b: - interfaces: - - status_request - wrist_motor_c: - interfaces: - - status_request - limit_switch_0: - interfaces: - - status_request - limit_switch_1: - interfaces: - - status_request - limit_switch_2: - interfaces: - - status_request - limit_switch_3: - interfaces: - - status_request - limit_switch_4: - interfaces: - - status_request - state_interfaces: - actuator: - interfaces: - - status - cam_0: - interfaces: - - status - cam_1: - interfaces: - - status - limit_switch_0: - interfaces: - - status - limit_switch_1: - interfaces: - - status - limit_switch_2: - interfaces: - - status - limit_switch_3: - interfaces: - - status - limit_switch_4: - interfaces: - - status - publish_rate: 10.0 diff --git a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers_3dof.yaml b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers_3dof.yaml new file mode 100644 index 00000000..ebfb9b25 --- /dev/null +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers_3dof.yaml @@ -0,0 +1,238 @@ +/**/controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + arm_velocity_controller: + type: velocity_controllers/JointGroupVelocityController + + + manual_arm_joint_by_joint_controller: + type: arm_controllers/ManualArmJointByJointController + + + manual_wrist_joint_by_joint_controller: + type: arm_controllers/Manual3DOFWristJointByJointController + + manual_end_effector_gripper_claw_controller: + type: arm_controllers/ManualEndEffectorGripperClawController + + + motor_status_controller: + type: general_controllers/MotorStatusController + + cam_position_controller: + type: position_controllers/JointGroupPositionController + + rotary_encoder_state_request_controller: + type: forward_command_controller/ForwardCommandController + +/**/joint_state_broadcaster: + ros__parameters: + # Only publish joints represented in the kinematic URDF. Hardware-only + # resources (cameras, encoders, and limit switches) are exposed by their + # dedicated controllers and must not be sent to MoveIt's state monitor. + joints: + - base_yaw + - shoulder_pitch + - elbow_pitch + - wrist_motor_a + - wrist_motor_b + - wrist_motor_c + - gripper_claw + - actuator + interfaces: + - position + - velocity + +/**/joint_trajectory_controller: + ros__parameters: + joints: + - base_yaw + - shoulder_pitch + - elbow_pitch + - wrist_motor_a + - wrist_motor_b + - wrist_motor_c + - gripper_claw + - actuator + + command_interfaces: + - position + + state_interfaces: + - position + - velocity + + action_monitor_rate: 20.0 # Rate to monitor status changes when the controller is executing action + + allow_partial_joints_goal: true + + # Open loop:The controller ignores the states provided by hardware interface but using + # last commands as states for starting the trajectory interpolation + # deactivates the feedback control + open_loop_control: true + constraints: + # Velocity tolerance for at the end of the trajectory that indicates that controlled system is stopped. + stopped_velocity_tolerance: 0.01 + + # Time tolerance for achieving trajectory goal before or after commanded time. + # If set to zero, the controller will wait a potentially infinite amount of time. + goal_time: 10.0 + +/**/arm_velocity_controller: + ros__parameters: + joints: + - base_yaw + - shoulder_pitch + - elbow_pitch + - wrist_motor_a + - wrist_motor_b + - wrist_motor_c + - gripper_claw + - actuator + interface_name: velocity + +/**/cam_position_controller: + ros__parameters: + joints: + - cam_0 + - cam_1 + interface_name: position + +/**/rotary_encoder_state_request_controller: + ros__parameters: + joints: + - base_yaw_encoder + - elbow_pitch_encoder + - shoulder_pitch_encoder + interface_name: state_request + +/**/manual_arm_joint_by_joint_controller: + ros__parameters: + joints: + - base_yaw + - shoulder_pitch + - elbow_pitch + # interface_name: velocity + joint_max_velocities: + - 0.08 # base_yaw + - 0.05148721 # 2.95 dps + - 0.05148721 + virtual_four_bar_coupling_ratio: 1.0 + +/**/manual_wrist_joint_by_joint_controller: + ros__parameters: + joints: + - wrist_motor_a + - wrist_motor_b + - wrist_motor_c + # interface_name: velocity + joint_max_velocities: + - 0.785398 # TODO + - 0.785398 # TODO + - 0.785398 # TODO + +/**/manual_end_effector_gripper_claw_controller: + ros__parameters: + velocity_joints: + - gripper_claw + position_joints: + - actuator + # interface_name: velocity + joint_max_velocities: + - 0.005 # TODO: Find this + - 0.03 # Actuator max displacement + joint_max_positions: + - 0.03 # Actuator max displacement + +# Default command interfaces: +# - status_request +# - maintenance_request +# - maintenance_frame_high +# - maintenance_frame_low +# - maintenance_data_count + +# Default state interfaces: +# - motor_temperature +# - torque_current +# - status +/**/motor_status_controller: + ros__parameters: + joints: + - base_yaw + - shoulder_pitch + - elbow_pitch + - wrist_motor_a + - wrist_motor_b + - wrist_motor_c + - actuator + - gripper_claw + - cam_0 + - cam_1 + gpios: + - limit_switch_0 + - limit_switch_1 + - limit_switch_2 + - limit_switch_3 + - limit_switch_4 + command_interfaces: + gripper_claw: + interfaces: + - status_request + wrist_motor_a: + interfaces: + - status_request + wrist_motor_b: + interfaces: + - status_request + wrist_motor_c: + interfaces: + - status_request + limit_switch_0: + interfaces: + - status_request + limit_switch_1: + interfaces: + - status_request + limit_switch_2: + interfaces: + - status_request + limit_switch_3: + interfaces: + - status_request + limit_switch_4: + interfaces: + - status_request + state_interfaces: + actuator: + interfaces: + - status + cam_0: + interfaces: + - status + cam_1: + interfaces: + - status + limit_switch_0: + interfaces: + - status + limit_switch_1: + interfaces: + - status + limit_switch_2: + interfaces: + - status + limit_switch_3: + interfaces: + - status + limit_switch_4: + interfaces: + - status + publish_rate: 10.0 diff --git a/src/subsystems/arm/arm_bringup/config/controller_switcher.yaml b/src/subsystems/arm/arm_bringup/config/controller_switcher_2dof.yaml similarity index 58% rename from src/subsystems/arm/arm_bringup/config/controller_switcher.yaml rename to src/subsystems/arm/arm_bringup/config/controller_switcher_2dof.yaml index 8bc999c5..4bdd38fa 100644 --- a/src/subsystems/arm/arm_bringup/config/controller_switcher.yaml +++ b/src/subsystems/arm/arm_bringup/config/controller_switcher_2dof.yaml @@ -4,12 +4,10 @@ full_arm: - manual_arm_cylindrical_controller - arm_velocity_controller - - twodof_joint_trajectory_controller - - threedof_joint_trajectory_controller + - joint_trajectory_controller arm: - manual_arm_joint_by_joint_controller wrist: - - manual_2dof_wrist_joint_by_joint_controller - - manual_3dof_wrist_joint_by_joint_controller + - manual_wrist_joint_by_joint_controller end_effector: - manual_end_effector_gripper_claw_controller diff --git a/src/subsystems/arm/arm_bringup/config/controller_switcher_3dof.yaml b/src/subsystems/arm/arm_bringup/config/controller_switcher_3dof.yaml new file mode 100644 index 00000000..eaab9903 --- /dev/null +++ b/src/subsystems/arm/arm_bringup/config/controller_switcher_3dof.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + subsystem: arm + full_arm: + - arm_velocity_controller + - joint_trajectory_controller + arm: + - manual_arm_joint_by_joint_controller + wrist: + - manual_wrist_joint_by_joint_controller + end_effector: + - manual_end_effector_gripper_claw_controller diff --git a/src/subsystems/arm/arm_bringup/launch/athena_arm.jetson.launch.py b/src/subsystems/arm/arm_bringup/launch/athena_arm.jetson.launch.py index 31024964..bd09ec1a 100644 --- a/src/subsystems/arm/arm_bringup/launch/athena_arm.jetson.launch.py +++ b/src/subsystems/arm/arm_bringup/launch/athena_arm.jetson.launch.py @@ -1,7 +1,7 @@ from launch import LaunchDescription, LaunchContext -from launch.actions import RegisterEventHandler, DeclareLaunchArgument, TimerAction +from launch.actions import RegisterEventHandler, DeclareLaunchArgument, TimerAction, OpaqueFunction from launch.event_handlers import OnProcessExit, OnProcessStart -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration, PythonExpression +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -18,8 +18,8 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="athena_arm_controllers.yaml", - description="YAML file with the controllers configuration.", + default_value="", + description="Optional controller YAML override. The wrist-specific configuration is selected by default.", ) ) declared_arguments.append( @@ -32,7 +32,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_file", - default_value="athena_arm.urdf.xacro", + default_value="arm/athena_arm.urdf.xacro", description="URDF/XACRO description file with the robot.", ) ) @@ -78,6 +78,11 @@ def generate_launch_description(): description="CAN interface to use for hardware interfaces.", ) ) + + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + + +def launch_setup(context, *args, **kwargs): # -- Initialize Arguments -- runtime_config_package = LaunchConfiguration("runtime_config_package") controllers_file = LaunchConfiguration("controllers_file") @@ -86,20 +91,30 @@ def generate_launch_description(): use_mock_hardware = LaunchConfiguration("use_mock_hardware") mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") use_3dof = LaunchConfiguration("use_3dof") + using_3dof = use_3dof.perform(context).lower() in ("true", "1", "yes") deactivate_talon = LaunchConfiguration("deactivate_talon") can_interface = LaunchConfiguration("can_interface") # -- Building Path Files -- robot_description_path = PathJoinSubstitution( - [FindPackageShare("description"), "urdf", "athena_arm.urdf.xacro"] + [FindPackageShare(description_package), "urdf", LaunchConfiguration("description_file")] ) + controllers_file_name = controllers_file.perform(context) + if not controllers_file_name: + controllers_file_name = ( + "athena_arm_controllers_3dof.yaml" + if using_3dof + else "athena_arm_controllers_2dof.yaml" + ) robot_controllers = PathJoinSubstitution( - [FindPackageShare(runtime_config_package), "config", controllers_file] + [FindPackageShare(runtime_config_package), "config", controllers_file_name] ) - controller_switcher_config = PathJoinSubstitution( - [FindPackageShare("bringup"), "config", "controller_switcher.yaml"] - ) + controller_switcher_config = PathJoinSubstitution([ + FindPackageShare(runtime_config_package), + "config", + "controller_switcher_3dof.yaml" if using_3dof else "controller_switcher_2dof.yaml", + ]) # -- Additional Configuration Setup -- robot_description_content = Command( @@ -154,15 +169,9 @@ def generate_launch_description(): arguments=["joint_state_broadcaster"], ) - wrist_controller = PythonExpression([ - '"manual_3dof_wrist_joint_by_joint_controller" if "', - use_3dof, - '" == "true" else "manual_2dof_wrist_joint_by_joint_controller"' - ]) - robot_controller_names = [ "manual_arm_joint_by_joint_controller", - wrist_controller, + "manual_wrist_joint_by_joint_controller", "manual_end_effector_gripper_claw_controller", ] robot_controller_spawners = [] @@ -175,23 +184,12 @@ def generate_launch_description(): ) ] - joint_trajectory_controller = PythonExpression([ - '"threedof_joint_trajectory_controller" if "', - use_3dof, - '" == "true" else "twodof_joint_trajectory_controller"' - ]) - - inactive_controller = PythonExpression([ - '"manual_2dof_wrist_joint_by_joint_controller" if "', - use_3dof, - '" == "true" else "manual_3dof_wrist_joint_by_joint_controller"', - ]) inactive_robot_controller_names = [ - inactive_controller, - "manual_arm_cylindrical_controller", - joint_trajectory_controller, + "joint_trajectory_controller", "arm_velocity_controller", ] + if not using_3dof: + inactive_robot_controller_names.insert(0, "manual_arm_cylindrical_controller") inactive_robot_controller_spawners = [] for controller in inactive_robot_controller_names: inactive_robot_controller_spawners += [ @@ -264,8 +262,7 @@ def generate_launch_description(): ) ] - return LaunchDescription( - declared_arguments + + return ( [ control_node, robot_state_pub_node, diff --git a/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py b/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py index c15397fc..c8a40a1e 100644 --- a/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py +++ b/src/subsystems/arm/arm_bringup/launch/athena_arm.launch.py @@ -1,15 +1,12 @@ -from launch import LaunchDescription, LaunchContext +from launch import LaunchDescription from launch.actions import RegisterEventHandler, DeclareLaunchArgument, TimerAction, OpaqueFunction -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit, OnProcessStart -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration, PythonExpression +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from moveit_configs_utils import MoveItConfigsBuilder - - def generate_launch_description(): declared_arguments = [] declared_arguments.append( @@ -32,6 +29,13 @@ def generate_launch_description(): description="Start RViz2 automatically with this launch file.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "use_moveit", + default_value="false", + description="Start the MoveIt move_group and hello_moveit nodes.", + ) + ) declared_arguments.append( DeclareLaunchArgument( "runtime_config_package", @@ -43,8 +47,8 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "controllers_file", - default_value="athena_arm_controllers.yaml", - description="YAML file with the controllers configuration.", + default_value="", + description="Optional controller YAML override. The wrist-specific configuration is selected by default.", ) ) declared_arguments.append( @@ -58,7 +62,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_file", - default_value="athena_arm.urdf.xacro", + default_value="arm/athena_arm.urdf.xacro", description="URDF/XACRO description file with the robot.", ) ) @@ -129,16 +133,18 @@ def generate_launch_description(): def launch_setup(context, *args, **kwargs): mode = LaunchConfiguration("mode").perform(context) use_sim = LaunchConfiguration("use_sim") + use_moveit = LaunchConfiguration("use_moveit") runtime_config_package = LaunchConfiguration("runtime_config_package") controllers_file = LaunchConfiguration("controllers_file") description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") rviz_file = LaunchConfiguration("rviz_file") - moveit_package = LaunchConfiguration("moveit_package") prefix = LaunchConfiguration("prefix") use_mock_hardware = LaunchConfiguration("use_mock_hardware") + using_mock_hardware = use_mock_hardware.perform(context).lower() in ("true", "1", "yes") mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") use_3dof = LaunchConfiguration("use_3dof") + using_3dof = use_3dof.perform(context).lower() in ("true", "1", "yes") deactivate_talon = LaunchConfiguration("deactivate_talon") can_interface = LaunchConfiguration("can_interface") @@ -147,37 +153,27 @@ def launch_setup(context, *args, **kwargs): # This is creating a terminal command that essentially expands all macros in this file # and creates the FULL URDF robot_description_path = PathJoinSubstitution( - [FindPackageShare("description"), "urdf", "athena_arm.urdf.xacro"] - ) + [FindPackageShare(description_package), "urdf", description_file] + ) + controllers_file_name = controllers_file.perform(context) + if not controllers_file_name: + controllers_file_name = ( + "athena_arm_controllers_3dof.yaml" + if using_3dof + else "athena_arm_controllers_2dof.yaml" + ) robot_controllers = PathJoinSubstitution( - [FindPackageShare(runtime_config_package), "config", controllers_file] - ) - joint_state_yaml = PathJoinSubstitution( - [FindPackageShare(runtime_config_package), "config", "initial_joint_states.yaml"] - ) - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", rviz_file] + [FindPackageShare(runtime_config_package), "config", controllers_file_name] ) joystick_config_file = PathJoinSubstitution( [FindPackageShare(runtime_config_package), 'config', 'joystick.yaml'] ) - controller_switcher_config = PathJoinSubstitution( - [FindPackageShare(runtime_config_package), "config", "controller_switcher.yaml"] - ) - - # MoveIt Config Setup (TODO: Currently not using Launch Configuration for description and these configs because moveit - # config builder happens at launch time. Is there a way to keep these Launch configs when building file path?) - robot_semantic_path = PathJoinSubstitution( - [FindPackageShare("arm_moveit"), "srdf", "athena_arm.srdf"] - ) - robot_kinematics_path = PathJoinSubstitution( - [FindPackageShare("arm_moveit"), "config", "kinematics.yaml"] - ) - - moveit_controllers_config_path = PathJoinSubstitution( - [FindPackageShare("arm_moveit"), "config", "moveit_controllers_2dof.yaml"] - ) + controller_switcher_config = PathJoinSubstitution([ + FindPackageShare(runtime_config_package), + "config", + "controller_switcher_3dof.yaml" if using_3dof else "controller_switcher_2dof.yaml", + ]) robot_description_content = Command( [ @@ -206,22 +202,6 @@ def launch_setup(context, *args, **kwargs): ) robot_description = {"robot_description": robot_description_content} - moveit_config = ( - MoveItConfigsBuilder("athena_arm", package_name="arm_moveit") - .robot_description(file_path=robot_description_path.perform(LaunchContext())) - .robot_description_semantic(file_path=robot_semantic_path.perform(LaunchContext())) - .robot_description_kinematics(file_path=robot_kinematics_path.perform(LaunchContext())) - .trajectory_execution(file_path=moveit_controllers_config_path.perform(LaunchContext())) - .planning_scene_monitor( - publish_robot_description=True, publish_robot_description_semantic=True - ) - .planning_pipelines( - pipelines=["ompl", "pilz_industrial_motion_planner"], - default_planning_pipeline="ompl", - ) - .to_moveit_configs() - ) - joystick_publisher = Node( package='teleop', executable='joystick', @@ -247,22 +227,6 @@ def launch_setup(context, *args, **kwargs): parameters=[robot_description], ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - condition=IfCondition(use_sim), - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - moveit_config.planning_pipelines, - moveit_config.joint_limits, - ], - ) - joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", @@ -272,27 +236,12 @@ def launch_setup(context, *args, **kwargs): motor_status_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=[ - PythonExpression([ - '"three_dof_motor_status_controller" if "', - use_3dof, - '" == "true" else "two_dof_motor_status_controller"' - ]), - "-c", - "/controller_manager", - ], + arguments=["motor_status_controller", "-c", "/controller_manager"], ) - - wrist_controller = PythonExpression([ - '"manual_3dof_wrist_joint_by_joint_controller" if "', - use_3dof, - '" == "true" else "manual_2dof_wrist_joint_by_joint_controller"' - ]) - robot_controller_names = [ "manual_arm_joint_by_joint_controller", - wrist_controller, + "manual_wrist_joint_by_joint_controller", "manual_end_effector_gripper_claw_controller", ] robot_controller_spawners = [] @@ -305,23 +254,12 @@ def launch_setup(context, *args, **kwargs): ) ] - joint_trajectory_controller = PythonExpression([ - '"threedof_joint_trajectory_controller" if "', - use_3dof, - '" == "true" else "twodof_joint_trajectory_controller"' - ]) - - - inactive_controller = PythonExpression([ - '"manual_2dof_wrist_joint_by_joint_controller" if "', - use_3dof, - '" == "true" else "manual_3dof_wrist_joint_by_joint_controller"', - ]) inactive_robot_controller_names = [ - inactive_controller, - "manual_arm_cylindrical_controller", - joint_trajectory_controller, - "arm_velocity_controller"] + "joint_trajectory_controller", + "arm_velocity_controller", + ] + if not using_3dof: + inactive_robot_controller_names.insert(0, "manual_arm_cylindrical_controller") inactive_robot_controller_spawners = [] for controller in inactive_robot_controller_names: inactive_robot_controller_spawners += [ @@ -332,10 +270,11 @@ def launch_setup(context, *args, **kwargs): ) ] - active_robot_controller_names = [ - "rotary_encoder_state_request_controller", - "cam_position_controller", - ] + active_robot_controller_names = ["cam_position_controller"] + if not using_mock_hardware: + active_robot_controller_names.insert( + 0, "rotary_encoder_state_request_controller" + ) for controller in active_robot_controller_names: robot_controller_spawners += [ Node( @@ -379,13 +318,6 @@ def launch_setup(context, *args, **kwargs): ) ) - delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[rviz_node], - ) - ) - delay_robot_controller_spawners_after_joint_state_broadcaster_spawner = [] for i, controller in enumerate(robot_controller_spawners): delay_robot_controller_spawners_after_joint_state_broadcaster_spawner += [ @@ -416,16 +348,83 @@ def launch_setup(context, *args, **kwargs): ) ] + standalone_visualization_actions = [] + if mode == "standalone": + from moveit_configs_utils import MoveItConfigsBuilder + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rviz", rviz_file] + ) + robot_semantic_path = PathJoinSubstitution( + [FindPackageShare("arm_moveit"), "srdf", "athena_arm.srdf"] + ) + robot_kinematics_path = PathJoinSubstitution( + [FindPackageShare("arm_moveit"), "config", "kinematics.yaml"] + ) + moveit_controllers_config_path = PathJoinSubstitution([ + FindPackageShare("arm_moveit"), + "config", + "moveit_controllers_3dof.yaml" if using_3dof else "moveit_controllers_2dof.yaml", + ]) + moveit_config = ( + MoveItConfigsBuilder("athena_arm", package_name="arm_moveit") + .robot_description(file_path=robot_description_path.perform(context)) + .robot_description_semantic(file_path=robot_semantic_path.perform(context)) + .robot_description_kinematics(file_path=robot_kinematics_path.perform(context)) + .trajectory_execution(file_path=moveit_controllers_config_path.perform(context)) + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .planning_pipelines( + pipelines=["ompl", "pilz_industrial_motion_planner"], + default_planning_pipeline="ompl", + ) + .to_moveit_configs() + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(use_sim), + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, + ], + ) + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + condition=IfCondition(use_moveit), + parameters=[moveit_config.to_dict()], + ) + standalone_visualization_actions.append( + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[rviz_node, move_group_node], + ) + ) + ) + jetson_actions = [ control_node, robot_state_pub_node, delay_joint_state_broadcaster_spawner_after_ros2_control_node, - delay_motor_status_controller_after_joint_state_broadcaster, - delay_rviz_after_joint_state_broadcaster_spawner, controller_switcher_node, ] + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner \ + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner + if not using_mock_hardware: + jetson_actions.append( + delay_motor_status_controller_after_joint_state_broadcaster + ) + base_station_actions = [ joystick_publisher, ] @@ -435,4 +434,4 @@ def launch_setup(context, *args, **kwargs): elif mode == "base_station": return base_station_actions else: - return jetson_actions + base_station_actions + return jetson_actions + base_station_actions + standalone_visualization_actions diff --git a/src/subsystems/arm/arm_bringup/package.xml b/src/subsystems/arm/arm_bringup/package.xml index bc813d56..6f10b22a 100644 --- a/src/subsystems/arm/arm_bringup/package.xml +++ b/src/subsystems/arm/arm_bringup/package.xml @@ -41,7 +41,9 @@ urdf xacro arm_controllers + arm_moveit general_controllers + moveit_ros_move_group rosidl_default_runtime builtin_interfaces diff --git a/src/subsystems/arm/arm_controllers/src/manual_arm_cylindrical_controller.cpp b/src/subsystems/arm/arm_controllers/src/manual_arm_cylindrical_controller.cpp index 8afa579b..aecf1c2d 100644 --- a/src/subsystems/arm/arm_controllers/src/manual_arm_cylindrical_controller.cpp +++ b/src/subsystems/arm/arm_controllers/src/manual_arm_cylindrical_controller.cpp @@ -244,11 +244,11 @@ controller_interface::return_type ManualArmCylindricalController::update( if (!std::isnan((*current_ref)->axes[0])) { //TODO: Get rid of the hardcoded rigamarole (translates to max vel of 1.5 inches/s) - command_velocities_[0] = (*current_ref)->axes[2]*0.174533; // l/r right joystick -> yaw 10 dps - command_velocities_[1] = (*current_ref)->axes[1]*6*0.00635; // u/d left joystick -> vy - command_velocities_[2] = (*current_ref)->axes[3]*6*0.00635; // u/d right joystick -> vz + command_velocities_[0] = (*current_ref)->axes[0]*0.174533; // l/r left joystick -> yaw 10 dps + command_velocities_[1] = (*current_ref)->axes[1]*6*0.00635; // u/d left joystick -> vx + command_velocities_[2] = (*current_ref)->axes[3]*6*0.00635; // u/d right joystick -> vy command_velocities_[3] = (*current_ref)->axes[1] * static_cast((*current_ref)->buttons[1]); // u/d left joystick & circle -> thetadot - command_velocities_[4] = 0.0; // (*current_ref)->axes[0]; // l/r left joystick -> wrist roll + command_velocities_[4] = (*current_ref)->axes[2]; // l/r right joystick -> wrist roll command_velocities_[5] = (*current_ref)->axes[4]; // left trigger -> open claw command_velocities_[6] = (*current_ref)->axes[5]; // right trigger -> close claw } diff --git a/src/subsystems/arm/arm_moveit/config/moveit_controllers.yaml b/src/subsystems/arm/arm_moveit/config/moveit_controllers.yaml index 76233e51..0073d69b 100644 --- a/src/subsystems/arm/arm_moveit/config/moveit_controllers.yaml +++ b/src/subsystems/arm/arm_moveit/config/moveit_controllers.yaml @@ -9,9 +9,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - twodof_joint_trajectory_controller + - joint_trajectory_controller - twodof_joint_trajectory_controller: + joint_trajectory_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true diff --git a/src/subsystems/arm/arm_moveit/config/moveit_controllers_2dof.yaml b/src/subsystems/arm/arm_moveit/config/moveit_controllers_2dof.yaml index 76233e51..0073d69b 100644 --- a/src/subsystems/arm/arm_moveit/config/moveit_controllers_2dof.yaml +++ b/src/subsystems/arm/arm_moveit/config/moveit_controllers_2dof.yaml @@ -9,9 +9,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - twodof_joint_trajectory_controller + - joint_trajectory_controller - twodof_joint_trajectory_controller: + joint_trajectory_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true diff --git a/src/subsystems/arm/arm_moveit/config/moveit_controllers_3dof.yaml b/src/subsystems/arm/arm_moveit/config/moveit_controllers_3dof.yaml index 41c2e8f5..d814559a 100644 --- a/src/subsystems/arm/arm_moveit/config/moveit_controllers_3dof.yaml +++ b/src/subsystems/arm/arm_moveit/config/moveit_controllers_3dof.yaml @@ -9,9 +9,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - threedof_joint_trajectory_controller + - joint_trajectory_controller - threedof_joint_trajectory_controller: + joint_trajectory_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true diff --git a/src/subsystems/arm/arm_moveit/launch/athena_arm_moveit.launch.py b/src/subsystems/arm/arm_moveit/launch/athena_arm_moveit.launch.py index c1ffb53d..9d61621b 100644 --- a/src/subsystems/arm/arm_moveit/launch/athena_arm_moveit.launch.py +++ b/src/subsystems/arm/arm_moveit/launch/athena_arm_moveit.launch.py @@ -1,5 +1,5 @@ -from launch import LaunchDescription, LaunchContext -from launch.actions import DeclareLaunchArgument +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration, PythonExpression from launch_ros.actions import Node @@ -10,7 +10,6 @@ def generate_launch_description(): - declared_arguments = [] declared_arguments.append( DeclareLaunchArgument( @@ -20,6 +19,10 @@ def generate_launch_description(): ) ) + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + + +def launch_setup(context, *args, **kwargs): use_3dof = LaunchConfiguration("use_3dof") joint_state_yaml = PathJoinSubstitution( @@ -32,6 +35,7 @@ def generate_launch_description(): robot_description_path = PathJoinSubstitution([FindPackageShare("description"), "urdf", + "arm", "athena_arm.urdf.xacro"]) robot_semantic_path = PathJoinSubstitution([FindPackageShare("arm_moveit"), "srdf", @@ -71,9 +75,9 @@ def generate_launch_description(): MoveItConfigsBuilder( "athena_arm", package_name="arm_moveit" ) - .robot_description(robot_description_path.perform(LaunchContext())) + .robot_description(robot_description_path.perform(context)) .robot_description_semantic("srdf/athena_arm.srdf") - .trajectory_execution(file_path=moveit_controllers_config_path.perform(LaunchContext())) + .trajectory_execution(file_path=moveit_controllers_config_path.perform(context)) .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True ) @@ -142,4 +146,4 @@ def generate_launch_description(): run_move_group_node, hello_moveit_node ] - return LaunchDescription(declared_arguments + nodes) \ No newline at end of file + return nodes diff --git a/src/subsystems/drive/README.md b/src/subsystems/drive/README.md index 68b07b3a..20a13793 100644 --- a/src/subsystems/drive/README.md +++ b/src/subsystems/drive/README.md @@ -1,86 +1,173 @@ -# athena_drive +# UMD Loop's Drive software integration -This repository contains all packages for the drive subteam for UMD Loop's 2024-25 rover, Athena. +## Current package list in drive subsystem: ------ +
    +
  • drive_bringup: contains launch files and configuration for drive ros2_control, teleop, RViz, and controllers
    • +
  • drive_controllers: contains custom drive controllers
    • +
-## Guidelines -* Fork this repository for development, you can open a pull request to this repo to add your changes for everyone. -* Do NOT force push. -* Follow best practices (descriptive commit messages, well documented code, etc.) +--- -## Versions -This code is written for an Ubuntu 22.04 system with ROS2 Humble Hawksbill and Gazebo Harmonic. +### Launch Modes -## Building +`athena_drive.launch.py` accepts a `mode` argument that controls which nodes are started. -Note that to build this repository, you must have a [workspace](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html). You can clone this repository into the src/ folder. +By default (`mode:=standalone`), all nodes run on a single machine. For competition, split across two machines using the `mode` argument: -### Installing Dependencies +- `standalone` (default): starts all nodes on one machine (control + teleop) +- `jetson`: starts only the control and hardware nodes, to be run on the rover +- `base_station`: starts only the teleop nodes (joystick + teleop_twist_joy), to be run on the base station -Inside of your workspace, run: ```bash -rosdep install --from-paths src -y --ignore-src +source install/setup.bash +ros2 launch drive_bringup athena_drive.launch.py ``` -This will search through the `package.xml` files and install the necessary packages. Read more about rosdep and how to properly configure your packages [here](https://docs.ros.org/en/humble/Tutorials/Intermediate/Rosdep.html). -### Building +```bash +source install/setup.bash +ros2 launch drive_bringup athena_drive.launch.py mode:=jetson +``` -Inside of your workspace, run: ```bash -colcon build --packages-select +source install/setup.bash +ros2 launch drive_bringup athena_drive.launch.py mode:=base_station ``` -You can also do ``colcon build`` to build all packages. +--- + +### Other Launch Parameters + +#### Mock Hardware + +Allows user to implement mock hardware components which will allow for virtual testing. + +_Hardware:_ -After building, source your install with: ```bash source install/setup.bash +ros2 launch drive_bringup athena_drive.launch.py use_mock_hardware:=false ``` -Velocity Testing, data is in rad/s: +_Mock (no hardware):_ + +```bash +source install/setup.bash +ros2 launch drive_bringup athena_drive.launch.py use_mock_hardware:=true ``` -ros2 control switch_controllers --deactivate ackermann_steering_controller --deactivate drive_position_controller -ros2 control switch_controllers --activate drive_velocity_controller -ros2 topic pub /drive_velocity_controller/commands std_msgs/msg/Float64MultiArray "{data: [0.0, 0.0, 0.0, 0.0]}" + +#### Simulation Mode (WIP) + +Currently this repository does not support a fully implemented simulated hardware environment for the entire rover. The `use_sim` argument controls only the activation of RViz. + +_Deactivates RViz:_ + +```bash +source install/setup.bash +ros2 launch drive_bringup athena_drive.launch.py use_sim:=false ``` +#### ODrive Deactivation -### Launch Modes +ODrives can cloud the CANBus just like the Talons. They are easier to moderate but this is a quick deactivation parameter. -`athena_drive.launch.py` accepts a `mode` argument that controls which nodes are started. +_Deactivates ODrives:_ -By default (`mode:=standalone`), all nodes run on a single machine. For competition, split across two machines using the `mode` argument: +```bash +source install/setup.bash +ros2 launch drive_bringup athena_drive.launch.py deactivate_odrive:=true +``` -- `standalone` (default): starts all nodes on one machine (control + teleop) -- `jetson`: starts only the control and hardware nodes, to be run on the rover -- `base_station`: starts only the teleop nodes (joystick + teleop_twist_joy), to be run on the base station +#### General Controls + +![Alt text](../../../docs/drive_controls_rear_ackermann.png) + +#### CAN Interface + +The Jetson currently uses a CANable to implement CAN, not its own CAN controller. This results in 2 can interfaces, can0 and can1, with the current working implementation using the latter. + +_CAN Interface:_ ```bash -ros2 launch drive_bringup athena_drive.launch.py +source install/setup.bash +ros2 launch drive_bringup athena_drive.launch.py can_interface:=can1 ``` +### Testing + +#### Manual Controller Testing + +1. Make sure to plug in joystick controller +2. Open a new terminal +3. Switch to desired controller + - Rear Ackermann + - Front Ackermann + - Double Ackermann + - Ackermann Steering (ros2_control provided) + +##### Rear Ackermann Controller: + ```bash -ros2 launch drive_bringup athena_drive.launch.py mode:=jetson +source install/setup.bash +ros2 launch drive_bringup athena_drive.launch.py robot_controller:=rear_ackermann_controller ``` +##### Front Ackermann Controller: + ```bash -ros2 launch drive_bringup athena_drive.launch.py mode:=base_station +source install/setup.bash +ros2 launch drive_bringup athena_drive.launch.py robot_controller:=front_ackermann_controller ``` ---- +##### Double Ackermann Controller: -### How to use Control Switching -**Open another terminal, source the workspace, and call the service to set controllers:** ```bash source install/setup.bash -ros2 service call /set_controller athena_drive_msgs/srv/SetController "{controller_names: [INCLUDE CONTROLLER(S) YOU WANT WITHIN BRACKETS]}" +ros2 launch drive_bringup athena_drive.launch.py robot_controller:=double_ackermann_controller +``` + +##### Ackermann Steering Controller: + +```bash +source install/setup.bash +ros2 launch drive_bringup athena_drive.launch.py robot_controller:=ackermann_steering_controller +``` + +##### Power Gpio Controller: + +```bash +ros2 topic pub --once /power_module_gpio_controller/commands control_msgs/msg/DynamicInterfaceGroupValues " +interface_groups: +- drive_power_module +interface_values: +- interface_names: + - kill_jetson + values: + - 1.0 +" +``` + +##### LED Gpio Controller: + +```bash +ros2 topic pub --once /drive_led_gpio_controller/commands control_msgs/msg/DynamicInterfaceGroupValues " +interface_groups: +- drive_led +interface_values: +- interface_names: + - red + values: + - 255.0 +" +``` + +#### Autonomous Controller Testing + +##### Velocity Testing + +1. Switch to velocity controller using the controller switching service +2. In a new terminal: + +```bash +ros2 topic pub /drive_velocity_controller/commands std_msgs/msg/Float64MultiArray "{data: [0.0, 0.0, 0.0, 0.0]}" ``` -Example: `ros2 service call /set_controller athena_drive_msgs/srv/SetController "{controller_names: [drive_velocity_controller]}"` - -ros2 topic pub --once /power_module_gpio_controller/commands control_msgs/msg/DynamicInterfaceGroupValues " -interface_groups: ['drive_power_module'] -interface_values: -- interface_names: ['kill_jetson'] - values: [1.0] -" \ No newline at end of file diff --git a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml index 27319c2c..fc41bb9f 100644 --- a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml +++ b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml @@ -1,24 +1,3 @@ -# Copyright (c) 20224 Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# -# Author: Dr. Denis -# - controller_manager: ros__parameters: update_rate: 30 # Hz @@ -31,6 +10,15 @@ controller_manager: front_ackermann_controller: type: athena_drive_controllers/FrontAckermannController + + crab_steering_controller: + type: athena_drive_controllers/CrabSteeringController + + double_ackermann_controller: + type: athena_drive_controllers/DoubleAckermannController + + swerve_drive_controller: + type: athena_drive_controllers/SwerveDriveController drive_velocity_controller: type: velocity_controllers/JointGroupVelocityController @@ -38,9 +26,6 @@ controller_manager: drive_position_controller: type: position_controllers/JointGroupPositionController - # swerve_drive_controller: - # type: drive_controllers/SwerveDriveController - # LED GPIO Controller drive_led_gpio_controller: type: gpio_controllers/GpioCommandController @@ -89,6 +74,46 @@ front_ackermann_controller: base_frame_id: "base_footprint" velocity_rolling_window_size: 10 +crab_steering_controller: + ros__parameters: + front_left_drive_joint: propulsion_fl_joint + front_right_drive_joint: propulsion_fr_joint + rear_left_drive_joint: propulsion_bl_joint + rear_right_drive_joint: propulsion_br_joint + front_left_swerve_joint: steer_fl_joint + front_right_swerve_joint: steer_fr_joint + rear_left_swerve_joint: steer_bl_joint + rear_right_swerve_joint: steer_br_joint + wheel_radius: 0.125 + +double_ackermann_controller: + ros__parameters: + front_left_drive_joint: propulsion_fl_joint + front_right_drive_joint: propulsion_fr_joint + rear_left_drive_joint: propulsion_bl_joint + rear_right_drive_joint: propulsion_br_joint + front_left_swerve_joint: steer_fl_joint + front_right_swerve_joint: steer_fr_joint + rear_left_swerve_joint: steer_bl_joint + rear_right_swerve_joint: steer_br_joint + track_width: 0.6604 + wheelbase: 0.8382 + wheel_radius: 0.125 + +swerve_drive_controller: + ros__parameters: + front_left_drive_joint: propulsion_fl_joint + front_right_drive_joint: propulsion_fr_joint + rear_left_drive_joint: propulsion_bl_joint + rear_right_drive_joint: propulsion_br_joint + front_left_swerve_joint: steer_fl_joint + front_right_swerve_joint: steer_fr_joint + rear_left_swerve_joint: steer_bl_joint + rear_right_swerve_joint: steer_br_joint + track_width: 0.6604 + wheelbase: 0.8382 + wheel_radius: 0.125 + drive_velocity_controller: ros__parameters: joints: diff --git a/src/subsystems/drive/drive_bringup/config/test_goal_publishers_config.yaml b/src/subsystems/drive/drive_bringup/config/test_goal_publishers_config.yaml deleted file mode 100644 index 9d83c239..00000000 --- a/src/subsystems/drive/drive_bringup/config/test_goal_publishers_config.yaml +++ /dev/null @@ -1,57 +0,0 @@ -# Copyright (c) 20224 Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# - -publisher_forward_position_controller: - ros__parameters: - - controller_name: "forward_position_controller" - wait_sec_between_publish: 5 - - goal_names: ["pos1", "pos2", "pos3", "pos4"] - pos1: [0.785, 0.785, 0.785, 0.785, 0.785, 0.785] - pos2: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - pos3: [-0.785, -0.785, -0.785, -0.785, -0.785, -0.785] - pos4: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - -publisher_joint_trajectory_controller: - ros__parameters: - - controller_name: "joint_trajectory_controller" - wait_sec_between_publish: 6 - repeat_the_same_goal: 1 # useful to simulate continuous inputs - - goal_time_from_start: 3.0 - goal_names: ["pos1", "pos2", "pos3", "pos4"] - pos1: - positions: [0.785, 0.785, 0.785, 0.785, 0.785, 0.785] - pos2: - positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - pos3: - positions: [-0.785, -0.785, -0.785, -0.785, -0.785, -0.785] - pos4: - positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - joints: - - suspension_left_joint - - steer_fl_joint - - propulsion_fl_joint - - swerve_module_back_joint - - propulsion_bl_joint - - suspension_right_joint diff --git a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py index 91999751..9a32165b 100644 --- a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py +++ b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py @@ -69,7 +69,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_file", - default_value="athena_drive.urdf.xacro", + default_value="drive/athena_drive.urdf.xacro", description="URDF/XACRO description file with the robot.", ) ) @@ -108,7 +108,14 @@ def generate_launch_description(): DeclareLaunchArgument( "robot_controller", default_value="rear_ackermann_controller", - choices=["front_ackermann_controller", "ackermann_steering_controller", "rear_ackermann_controller"], + choices=[ + "front_ackermann_controller", + "ackermann_steering_controller", + "rear_ackermann_controller", + "crab_steering_controller", + "double_ackermann_controller", + "swerve_drive_controller", + ], description="Robot controller to start.", ) ) @@ -119,6 +126,14 @@ def generate_launch_description(): description="Deactivate the ODrive joints in the URDF when using mock hardware to prevent excessive CAN flow.", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "steering_config", + default_value="rear", + choices=["front", "rear", "all"], + description="Steering joints to load in the ODrive ros2_control description.", + ) + ) declared_arguments.append( DeclareLaunchArgument( "can_interface", @@ -144,7 +159,9 @@ def launch_setup(context, *args, **kwargs): use_mock_hardware = LaunchConfiguration("use_mock_hardware") mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") robot_controller = LaunchConfiguration("robot_controller") + robot_controller_name = robot_controller.perform(context) deactivate_odrive = LaunchConfiguration("deactivate_odrive") + steering_config = LaunchConfiguration("steering_config") can_interface = LaunchConfiguration("can_interface") robot_description_path = PathJoinSubstitution( @@ -186,6 +203,9 @@ def launch_setup(context, *args, **kwargs): "deactivate_odrive:=", deactivate_odrive, " ", + "steering_config:=", + steering_config, + " ", "can_interface:=", can_interface, ] @@ -212,7 +232,7 @@ def launch_setup(context, *args, **kwargs): output='screen', parameters=[teleop_twist_config], remappings=[ - ('/cmd_vel', '/rear_ackermann_controller/reference'), + ('/cmd_vel', f'/{robot_controller_name}/reference'), ], ) @@ -224,7 +244,6 @@ def launch_setup(context, *args, **kwargs): remappings=[ ("~/robot_description", "/robot_description"), ("/front_ackermann_controller/tf_odometry", "/tf"), - ("/ackermann_steering_controller/reference", "/cmd_vel"), ], ) @@ -256,7 +275,7 @@ def launch_setup(context, *args, **kwargs): arguments=["motor_status_controller", "-c", "/controller_manager"], ) - robot_controller_names = [robot_controller] + robot_controller_names = [robot_controller_name] robot_controller_spawners = [] for controller in robot_controller_names: robot_controller_spawners += [ @@ -278,7 +297,19 @@ def launch_setup(context, *args, **kwargs): ) ] - inactive_robot_controller_names = ["ackermann_steering_controller", "drive_velocity_controller", "drive_position_controller"] + selectable_robot_controller_names = [ + "front_ackermann_controller", + "ackermann_steering_controller", + "rear_ackermann_controller", + "crab_steering_controller", + "double_ackermann_controller", + "swerve_drive_controller", + ] + inactive_robot_controller_names = [ + controller + for controller in selectable_robot_controller_names + if controller != robot_controller_name + ] + ["drive_velocity_controller", "drive_position_controller"] inactive_robot_controller_spawners = [] for controller in inactive_robot_controller_names: inactive_robot_controller_spawners += [ diff --git a/src/subsystems/drive/drive_bringup/launch/test_forward_position_controller.launch.py b/src/subsystems/drive/drive_bringup/launch/test_forward_position_controller.launch.py deleted file mode 100644 index 15d48329..00000000 --- a/src/subsystems/drive/drive_bringup/launch/test_forward_position_controller.launch.py +++ /dev/null @@ -1,46 +0,0 @@ -# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# -# Author: Dr. Denis -# - -from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - position_goals = PathJoinSubstitution( - [FindPackageShare("drive_bringup"), "config", "test_goal_publishers_config.yaml"] - ) - - return LaunchDescription( - [ - Node( - package="ros2_controllers_test_nodes", - executable="publisher_forward_position_controller", - name="publisher_forward_position_controller", - parameters=[position_goals], - output={ - "stdout": "screen", - "stderr": "screen", - }, - ) - ] - ) diff --git a/src/subsystems/drive/drive_controllers/CMakeLists.txt b/src/subsystems/drive/drive_controllers/CMakeLists.txt index 20bc96b9..12358b02 100644 --- a/src/subsystems/drive/drive_controllers/CMakeLists.txt +++ b/src/subsystems/drive/drive_controllers/CMakeLists.txt @@ -61,6 +61,7 @@ add_library( SHARED src/swerve_drive_controller.cpp src/double_ackermann_controller.cpp + src/crab_steering_controller.cpp src/front_ackermann_controller.cpp src/rear_ackermann_controller.cpp ) diff --git a/src/subsystems/drive/drive_controllers/athena_drive_controllers.xml b/src/subsystems/drive/drive_controllers/athena_drive_controllers.xml index f4810ca6..3c4a0245 100644 --- a/src/subsystems/drive/drive_controllers/athena_drive_controllers.xml +++ b/src/subsystems/drive/drive_controllers/athena_drive_controllers.xml @@ -1,23 +1,3 @@ - - diff --git a/src/subsystems/drive/drive_controllers/config/crab_steering_controller.yaml b/src/subsystems/drive/drive_controllers/config/crab_steering_controller.yaml index 749b7a50..0f38fc49 100644 --- a/src/subsystems/drive/drive_controllers/config/crab_steering_controller.yaml +++ b/src/subsystems/drive/drive_controllers/config/crab_steering_controller.yaml @@ -1,118 +1,61 @@ -# Copyright (c) 20224 Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# - crab_steering_controller: state_joints: { type: string_array, default_value: [], description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", read_only: true, - validation: { - unique<>: null, - } } front_left_drive_joint: { type: string, default_value: "", description: "Specifies the front left drive joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } front_right_drive_joint: { type: string, default_value: "", description: "Specifies the front right drive joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } rear_left_drive_joint: { type: string, default_value: "", description: "Specifies the rear left drive joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } rear_right_drive_joint: { type: string, default_value: "", description: "Specifies the rear right drive joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } front_left_swerve_joint: { type: string, default_value: "", description: "Specifies the front left swerve joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } front_right_swerve_joint: { type: string, default_value: "", description: "Specifies the front right swerve joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } rear_left_swerve_joint: { type: string, default_value: "", description: "Specifies the rear left swerve joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } rear_right_swerve_joint: { type: string, default_value: "", description: "Specifies the rear right swerve joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } wheel_radius: { type: double, default_value: 0.254, description: "Specifies wheel radius calculation purposes. ", read_only: true, - validation: { - not_empty<>: null, - } } diff --git a/src/subsystems/drive/drive_controllers/config/double_ackermann_controller.yaml b/src/subsystems/drive/drive_controllers/config/double_ackermann_controller.yaml index e3a208e2..a60feae2 100644 --- a/src/subsystems/drive/drive_controllers/config/double_ackermann_controller.yaml +++ b/src/subsystems/drive/drive_controllers/config/double_ackermann_controller.yaml @@ -1,136 +1,73 @@ -# Copyright (c) 20224 Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# - double_ackermann_controller: state_joints: { type: string_array, default_value: [], description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", read_only: true, - validation: { - unique<>: null, - } } front_left_drive_joint: { type: string, default_value: "", description: "Specifies the front left drive joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } front_right_drive_joint: { type: string, default_value: "", description: "Specifies the front right drive joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } rear_left_drive_joint: { type: string, default_value: "", description: "Specifies the rear left drive joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } rear_right_drive_joint: { type: string, default_value: "", description: "Specifies the rear right drive joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } front_left_swerve_joint: { type: string, default_value: "", description: "Specifies the front left swerve joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } front_right_swerve_joint: { type: string, default_value: "", description: "Specifies the front right swerve joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } rear_left_swerve_joint: { type: string, default_value: "", description: "Specifies the rear left swerve joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } rear_right_swerve_joint: { type: string, default_value: "", description: "Specifies the rear right swerve joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } track_width: { type: double, default_value: 0.6604, description: "Specifies track width, or the distance in meters between left and right wheel for calculation purposes. ", read_only: true, - validation: { - not_empty<>: null, - } } wheelbase: { type: double, default_value: 0.8382, description: "Specifies wheelbase, or the distance in meters between front and back wheels for calculation purposes. ", read_only: true, - validation: { - not_empty<>: null, - } } wheel_radius: { type: double, default_value: 0.254, description: "Specifies wheel radius calculation purposes. ", read_only: true, - validation: { - not_empty<>: null, - } } diff --git a/src/subsystems/drive/drive_controllers/config/front_ackermann_controller.yaml b/src/subsystems/drive/drive_controllers/config/front_ackermann_controller.yaml index 77ae6d18..12c69c5e 100644 --- a/src/subsystems/drive/drive_controllers/config/front_ackermann_controller.yaml +++ b/src/subsystems/drive/drive_controllers/config/front_ackermann_controller.yaml @@ -1,4 +1,3 @@ -# Parameters for the FrontAckermannController front_ackermann_controller: drive_joints: { type: string_array, diff --git a/src/subsystems/drive/drive_controllers/config/rear_ackermann_controller.yaml b/src/subsystems/drive/drive_controllers/config/rear_ackermann_controller.yaml index 9483e9b2..b36cd365 100644 --- a/src/subsystems/drive/drive_controllers/config/rear_ackermann_controller.yaml +++ b/src/subsystems/drive/drive_controllers/config/rear_ackermann_controller.yaml @@ -1,4 +1,3 @@ -# Parameters for the RearAckermannController rear_ackermann_controller: drive_joints: { type: string_array, diff --git a/src/subsystems/drive/drive_controllers/config/swerve_drive_controller.yaml b/src/subsystems/drive/drive_controllers/config/swerve_drive_controller.yaml index f8030467..18e783f2 100644 --- a/src/subsystems/drive/drive_controllers/config/swerve_drive_controller.yaml +++ b/src/subsystems/drive/drive_controllers/config/swerve_drive_controller.yaml @@ -1,22 +1,3 @@ -# Copyright (c) 20224 Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# - swerve_drive_controller: state_joints: { type: string_array, @@ -32,105 +13,64 @@ swerve_drive_controller: default_value: "", description: "Specifies the front left drive joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } front_right_drive_joint: { type: string, default_value: "", description: "Specifies the front right drive joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } rear_left_drive_joint: { type: string, default_value: "", description: "Specifies the rear left drive joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } rear_right_drive_joint: { type: string, default_value: "", description: "Specifies the rear right drive joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } front_left_swerve_joint: { type: string, default_value: "", description: "Specifies the front left swerve joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } front_right_swerve_joint: { type: string, default_value: "", description: "Specifies the front right swerve joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } rear_left_swerve_joint: { type: string, default_value: "", description: "Specifies the rear left swerve joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } rear_right_swerve_joint: { type: string, default_value: "", description: "Specifies the rear right swerve joints used by the controller.", read_only: true, - validation: { - unique<>: null, - not_empty<>: null, - } } track_width: { type: double, default_value: 0.6604, description: "Specifies track width, or the distance in meters between left and right wheel for calculation purposes. ", read_only: true, - validation: { - not_empty<>: null, - } } wheelbase: { type: double, default_value: 0.8382, description: "Specifies wheelbase, or the distance in meters between front and back wheels for calculation purposes. ", read_only: true, - validation: { - not_empty<>: null, - } } wheel_radius: { type: double, default_value: 0.254, description: "Specifies wheel radius calculation purposes. ", read_only: true, - validation: { - not_empty<>: null, - } - } \ No newline at end of file + } diff --git a/src/subsystems/drive/drive_controllers/src/crab_steering_controller.cpp b/src/subsystems/drive/drive_controllers/src/crab_steering_controller.cpp index 905941da..821595d3 100644 --- a/src/subsystems/drive/drive_controllers/src/crab_steering_controller.cpp +++ b/src/subsystems/drive/drive_controllers/src/crab_steering_controller.cpp @@ -174,17 +174,7 @@ controller_interface::CallbackReturn CrabSteeringController::on_configure( void CrabSteeringController::reference_callback(const std::shared_ptr msg) { - // if (msg->joint_names.size() == joints.size()) - // { - // input_ref_.writeFromNonRT(msg); - // } - // else - // { - // RCLCPP_ERROR( - // get_node()->get_logger(), - // "Received %zu , but expected %zu joints in command. Ignoring message.", - // msg->joint_names.size(), joints.size()); - // } + input_ref_.writeFromNonRT(msg); } controller_interface::InterfaceConfiguration CrabSteeringController::command_interface_configuration() const diff --git a/src/subsystems/drive/drive_controllers/src/double_ackermann_controller.cpp b/src/subsystems/drive/drive_controllers/src/double_ackermann_controller.cpp index 1c3db9eb..13cbeb47 100644 --- a/src/subsystems/drive/drive_controllers/src/double_ackermann_controller.cpp +++ b/src/subsystems/drive/drive_controllers/src/double_ackermann_controller.cpp @@ -172,19 +172,9 @@ controller_interface::CallbackReturn DoubleAckermannController::on_configure( return controller_interface::CallbackReturn::SUCCESS; } -void DoubleAckermannController::reference_callback(const std::shared_ptr /*msg*/) +void DoubleAckermannController::reference_callback(const std::shared_ptr msg) { - // if (msg->joint_names.size() == joints.size()) - // { - // input_ref_.writeFromNonRT(msg); - // } - // else - // { - // RCLCPP_ERROR( - // get_node()->get_logger(), - // "Received %zu , but expected %zu joints in command. Ignoring message.", - // msg->joint_names.size(), joints.size()); - // } + input_ref_.writeFromNonRT(msg); } controller_interface::InterfaceConfiguration DoubleAckermannController::command_interface_configuration() const diff --git a/src/subsystems/drive/drive_controllers/src/swerve_drive_controller.cpp b/src/subsystems/drive/drive_controllers/src/swerve_drive_controller.cpp index 533adfea..deeccb4e 100644 --- a/src/subsystems/drive/drive_controllers/src/swerve_drive_controller.cpp +++ b/src/subsystems/drive/drive_controllers/src/swerve_drive_controller.cpp @@ -122,7 +122,7 @@ controller_interface::CallbackReturn SwerveDriveController::on_configure( // Reference Subscriber ref_subscriber_ = get_node()->create_subscription( - "/cmd_vel", subscribers_qos, + "~/reference", subscribers_qos, std::bind(&SwerveDriveController::reference_callback, this, std::placeholders::_1)); std::shared_ptr msg = std::make_shared(); @@ -175,17 +175,6 @@ controller_interface::CallbackReturn SwerveDriveController::on_configure( void SwerveDriveController::reference_callback(const std::shared_ptr msg) { - // if (msg->joint_names.size() == params_.joints.size()) - // { - // input_ref_.writeFromNonRT(msg); - // } - // else - // { - // RCLCPP_ERROR( - // get_node()->get_logger(), - // "Received %zu , but expected %zu joints in command. Ignoring message.", - // msg->joint_names.size(), params_.joints.size()); - // } input_ref_.writeFromNonRT(msg); } diff --git a/src/subsystems/general/README.md b/src/subsystems/general/README.md index cafe6892..b3f5c495 100644 --- a/src/subsystems/general/README.md +++ b/src/subsystems/general/README.md @@ -1,9 +1,92 @@ +# UMD Loop's General software integration + +The follow subsystem is shared across all actual robotic subsystems + +## Current package list in general: + +
    +
  • general_controllers: Contains custom controllers/broadcasters used across all subsystems
    • +
+ +--- + +## Motor Status Controller + +The motor status controller is intended to both allow for state interfaces that are not typically published by the joint state broadcaster (position, velocity, and effort) to be published on a separate topic. + +```bash +ros2 topic echo //motor_status +``` + +The controller also has some more complex functionality in that the user can directly make requests to the hardware to obtain status information about a particular piece of hardware. When prompting a `status request` for the RMDs for example, these commands would be called with the intention of a response frame which would be read in the RMD hardware interface. + +```bash + static constexpr std::array kStatusCommands = { + StatusCommands::READ_PID_CMD, + StatusCommands::READ_ACCELERATION_CMD, + StatusCommands::MOTOR_STATUS_1_CMD, + StatusCommands::MOTOR_STATUS_2_CMD + }; +``` + +Once the request is sent, the motor status controller would publish the updated motor status. This request functionality allows status information for hardware to be updated on an as-needed basis rather than a constant stream which would overflow the CANBus. Here is an example command with drive: + +```bash ros2 service call /motor_status_controller/status_request msgs/srv/StatusReq "{joint_name: propulsion_fl_joint, request_rate: 0}" +``` + +Additionally, the user can make `maintenance requests` as well. These are intended to be quick requests for particular information from the hardware such as obtaining PID parameters. This is an example for the SMC motors used in the arm in particular. + +`request_rate`: + +``` + +-1: once + +0: stop + +> 0: send command at request_rate Hz +``` + +`data`: CAN Frame data +``` +i32_data: 32-bit data + +i16_data: 16-bit data + +u8_data: 8-bit data +``` + +For these fields specifically, the user must be familiar with the CAN protocol for the specific piece of hardware. As long as the data inserted (like multiple different 8 bit data fields) are kept in order, the hardware interface will sort out the proper order for the frame as well as do the endianess and hex conversions. + +```bash +ros2 service call /motor_status_controller/maintenance_request msgs/srv/MaintenanceReq "{ + joint_name: wrist_roll, + request_rate: -1, + command_id: 0x40, + i32_data: [], + i16_data: [], + u8_data: [150] +}" +``` + +Expected output: + +```Shell + vcan0 145 [8] 40 96 00 00 00 00 00 00 +``` +Science examples: + +```bash ros2 service call /motor_status_controller/status_request msgs/srv/StatusReq "{joint_name: stepper_motor_a, request_rate: -1}" +``` +```bash ros2 service call /motor_status_controller/status_request msgs/srv/StatusReq "{joint_name: dc_auger, request_rate: -1}" +``` +```bash ros2 service call /motor_status_controller/maintenance_request msgs/srv/MaintenanceReq "{ joint_name: stepper_motor_a, request_rate: 1, @@ -12,7 +95,9 @@ ros2 service call /motor_status_controller/maintenance_request msgs/srv/Maintena i16_data: [], u8_data: [1] }" +``` +```bash ros2 service call /motor_status_controller/maintenance_request msgs/srv/MaintenanceReq "{ joint_name: dc_auger, request_rate: 1, @@ -21,7 +106,11 @@ ros2 service call /motor_status_controller/maintenance_request msgs/srv/Maintena i16_data: [], u8_data: [2] }" +``` + +Drive examples: +```bash ros2 service call /motor_status_controller/maintenance_request msgs/srv/MaintenanceReq "{ joint_name: propulsion_fl_joint, request_rate: -1, @@ -30,32 +119,307 @@ ros2 service call /motor_status_controller/maintenance_request msgs/srv/Maintena i16_data: [], u8_data: [] }" +``` -ros2 service call /motor_status_controller/maintenance_request msgs/srv/MaintenanceReq "{ - joint_name: wrist_roll, - request_rate: -1, - command_id: 0x40, - i32_data: [], - i16_data: [], - u8_data: [150] -}" +## Hardware functions + +`format_status_command`: Function in the hardware interfaces that creates the status request CAN frames + +`format_maintenance_command`: Function in the hardware interfaces that creates the status request CAN frames + +## Available status commands + +### RMD + +```bash +READ_PID_CMD (0x30) +READ_ACCELERATION_CMD (0x42) +MOTOR_STATUS_1_CMD (0x9A) +MOTOR_STATUS_2_CMD (0x9C) +``` + +### SMC + +```bash +READ_ACCELERATION_CMD (0x33) +READ_ENCODER_CMD (0x90) +READ_ABS_ANGLE_CMD (0x92) +MOTOR_STATUS_1_CMD (0x9A) +MOTOR_STATUS_2_CMD (0x9C) +``` + +### Stepper + +The command byte is the base command ID plus the motor's node ID. + +```bash +MOTOR_STATE (0x40 + node ID) +MOTOR_STATUS (0x50 + node ID) +``` + +### Servo + +The command byte is the base command ID plus the motor's node ID. + +```bash +MOTOR_STATE (0x40 + node ID) +MOTOR_STATUS (0x50 + node ID) +``` + +### DC + +The command byte is the base command ID plus the motor's node ID. + +```bash +MOTOR_STATE (0x40 + node ID) +MOTOR_STATUS (0x50 + node ID) +``` + +## Available maintenance commands + +### RMD + +```bash +WRITE_PID_TO_RAM_CMD (0x31) +u8_data: +- Current Loop P gain +- Current Loop I gain +- Speed Loop P gain +- Speed Loop I gain +- Position Loop P gain +- Position Loop I gain +i16_data: [] +i32_data: [] +``` + +```bash +WRITE_PID_TO_ROM_CMD (0x32) +u8_data: +- Current Loop P gain +- Current Loop I gain +- Speed Loop P gain +- Speed Loop I gain +- Position Loop P gain +- Position Loop I gain +i16_data: [] +i32_data: [] +``` + +```bash +WRITE_ACCELERATION_CMD (0x43) +u8_data: +- Function index +i16_data: [] +i32_data: +- Acceleration +``` + +```bash +WRITE_ENCODER_MULTI_TURN_ZERO_CMD (0x63) +u8_data: [] +i16_data: [] +i32_data: +- Encoder offset +``` + +```bash +WRITE_CURRENT_MULTI_TURN_POS_ZERO_CMD (0x64) +u8_data: [] +i16_data: [] +i32_data: [] +``` + +```bash +SYSTEM_RESET_CMD (0x76) +u8_data: [] +i16_data: [] +i32_data: [] +``` + +```bash +BRAKE_RELEASE_CMD (0x77) +u8_data: [] +i16_data: [] +i32_data: [] +``` + +```bash +BRAKE_LOCK_CMD (0x78) +u8_data: [] +i16_data: [] +i32_data: [] +``` + +```bash +MOTOR_SHUTDOWN_CMD (0x80) +u8_data: [] +i16_data: [] +i32_data: [] +``` + +```bash +MOTOR_STOP_CMD (0x81) +u8_data: [] +i16_data: [] +i32_data: [] +``` + +### SMC + +```bash +WRITE_ACCELERATION_CMD (0x34) +u8_data: [] +i16_data: [] +i32_data: +- Acceleration +``` + +```bash +READ_SETTINGS_CMD (0x40) +u8_data: +- Parameter ID +i16_data: [] +i32_data: [] +``` + +```bash +WRITE_SETTINGS_TO_RAM_CMD (0x42) +u8_data: +- Parameter ID +i16_data: [] +i32_data: [] +``` + +```bash +WRITE_SETTINGS_TO_ROM_CMD (0x44) +u8_data: +- Parameter ID +i16_data: [] +i32_data: [] +``` + +```bash +MOTOR_SHUTDOWN_CMD (0x80) +u8_data: [] +i16_data: [] +i32_data: [] +``` + +```bash +MOTOR_STOP_CMD (0x81) +u8_data: [] +i16_data: [] +i32_data: [] +``` + +```bash +MOTOR_RUNNING_CMD (0x88) +u8_data: [] +i16_data: [] +i32_data: [] +``` + +```bash +CLEAR_MOTOR_ANGLE_CMD (0x95) +u8_data: [] +i16_data: [] +i32_data: [] +``` + +```bash +CLEAR_ERROR_CMD (0x9B) +u8_data: [] +i16_data: [] +i32_data: [] +``` + +### Stepper + +```bash +PCB_HEARTBEAT_CMD (0x10) +u8_data: +- Heartbeat value +i16_data: [] +i32_data: [] +``` + +```bash +MAINTENANCE_CMD (0x60) +u8_data: +- Maintenance command option: + - SET_CURRENT_MULTI_TURN_POS_ZERO_TO_ROM_CMD (0x00) + - MOTOR_STOP_CMD (0x01) + - MOTOR_SHUTDOWN_CMD (0x02) + - CLEAR_ERRORS_CMD (0x03) +i16_data: [] +i32_data: [] +``` + +```bash +STEPPER_SPECS_CMD (0x70) +u8_data: +- One placeholder byte (required by the formatter; value is ignored) +i16_data: [] +i32_data: [] +``` + +### Servo + +```bash +PCB_HEARTBEAT_CMD (0x10) +u8_data: +- Heartbeat value +i16_data: [] +i32_data: [] +``` + +```bash +MAINTENANCE_CMD (0x60) +u8_data: +- Maintenance command option: + - SET_CURRENT_MULTI_TURN_POS_ZERO_TO_ROM_CMD (0x00) + - MOTOR_STOP_CMD (0x01) + - MOTOR_SHUTDOWN_CMD (0x02) + - CLEAR_ERRORS_CMD (0x03) +i16_data: [] +i32_data: [] +``` + +```bash +SERVO_SPECS_CMD (0x70) +u8_data: +- One placeholder byte (required by the formatter; value is ignored) +i16_data: [] +i32_data: [] +``` + +### DC + +```bash +PCB_HEARTBEAT_CMD (0x10) +u8_data: +- Heartbeat value +i16_data: [] +i32_data: [] +``` + +```bash +MAINTENANCE_CMD (0x60) +u8_data: +- Maintenance command option: + - SET_CURRENT_MULTI_TURN_POS_ZERO_TO_ROM_CMD (0x00) + - REQUEST_VECTORS_CMD (0x01) + - MOTOR_STOP_CMD (0x02) + - MOTOR_SHUTDOWN_CMD (0x03) + - CLEAR_ERRORS_CMD (0x04) +i16_data: [] +i32_data: [] +``` -ros2 topic pub --once /power_module_gpio_controller/commands control_msgs/msg/DynamicInterfaceGroupValues " -interface_groups: -- drive_power_module -interface_values: -- interface_names: - - kill_jetson - values: - - 1.0 -" - -ros2 topic pub --once /drive_led_gpio_controller/commands control_msgs/msg/DynamicInterfaceGroupValues " -interface_groups: -- drive_led -interface_values: -- interface_names: - - red - values: - - 255.0 -" +```bash +DC_SPECS_CMD (0x70) +u8_data: +- One placeholder byte (required by the formatter; value is ignored) +i16_data: [] +i32_data: [] +``` diff --git a/src/subsystems/navigation/README.md b/src/subsystems/navigation/README.md index 3b25fdf5..c0ada358 100644 --- a/src/subsystems/navigation/README.md +++ b/src/subsystems/navigation/README.md @@ -3,6 +3,8 @@ ## Running In Simulation ```bash +source install/setup.bash ros2 launch simulation bringup.launch.py publish_ground_truth_tf:=true rviz:=true +source install/setup.bash ros2 launch athena_planner navigation.launch.py sim:=true ``` \ No newline at end of file diff --git a/src/subsystems/navigation/aruco_bt/package.xml b/src/subsystems/navigation/aruco_bt/package.xml index f96c2e8a..3b2fe38d 100644 --- a/src/subsystems/navigation/aruco_bt/package.xml +++ b/src/subsystems/navigation/aruco_bt/package.xml @@ -7,8 +7,6 @@ abhinavkota06 TODO: License declaration - ament_python - rclpy sensor_msgs geometry_msgs diff --git a/src/subsystems/navigation/aruco_bt/setup.py b/src/subsystems/navigation/aruco_bt/setup.py index eb6b067b..655833b0 100644 --- a/src/subsystems/navigation/aruco_bt/setup.py +++ b/src/subsystems/navigation/aruco_bt/setup.py @@ -19,7 +19,6 @@ maintainer_email='abhinav.kota06@gmail.com', description='Python package for aruco detection and pose estimation', license='TODO: License declaration', - tests_require=['pytest'], entry_points={ 'console_scripts': [ 'aruco_node = aruco_bt.aruco_node:main', diff --git a/src/subsystems/navigation/aruco_detection/setup.py b/src/subsystems/navigation/aruco_detection/setup.py index 6d0b311e..5e04d777 100644 --- a/src/subsystems/navigation/aruco_detection/setup.py +++ b/src/subsystems/navigation/aruco_detection/setup.py @@ -17,7 +17,6 @@ maintainer_email='abhinav.kota06@gmail.com', description='TODO: Package description', license='TODO: License declaration', - tests_require=['pytest'], entry_points={ 'console_scripts': [ "aruco_node = aruco_detection.aruco_node:main", diff --git a/src/subsystems/navigation/gps_goal/setup.py b/src/subsystems/navigation/gps_goal/setup.py index 264ff97e..7a1caf7f 100644 --- a/src/subsystems/navigation/gps_goal/setup.py +++ b/src/subsystems/navigation/gps_goal/setup.py @@ -21,7 +21,6 @@ maintainer_email='mdurrani808@gmail.com', description='GPS goal navigation action server for Nav2', license='MIT', - tests_require=['pytest'], entry_points={ 'console_scripts': [ 'gps_goal_server = gps_goal.gps_goal_server:main', diff --git a/src/subsystems/navigation/led_indicator/setup.py b/src/subsystems/navigation/led_indicator/setup.py index 53711e99..e0ba8187 100644 --- a/src/subsystems/navigation/led_indicator/setup.py +++ b/src/subsystems/navigation/led_indicator/setup.py @@ -19,7 +19,6 @@ maintainer_email="mdurrani808@gmail.com", description="LED status indicator via Adafruit Pixel Trinkey + NeoPixels", license="MIT", - tests_require=["pytest"], entry_points={ "console_scripts": [ "led_indicator_node = led_indicator.led_indicator_node:main", diff --git a/src/subsystems/navigation/mag_heading/setup.py b/src/subsystems/navigation/mag_heading/setup.py index e1cb0d41..a3fa9bcf 100644 --- a/src/subsystems/navigation/mag_heading/setup.py +++ b/src/subsystems/navigation/mag_heading/setup.py @@ -21,7 +21,6 @@ maintainer_email='mdurrani808@gmail.com', description='Magnetometer calibration and heading publisher for ZED camera IMU', license='MIT', - tests_require=['pytest'], entry_points={ 'console_scripts': [ 'mag_heading_node = mag_heading.mag_heading_node:main', diff --git a/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py b/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py index be0d4d56..d6ae91a2 100644 --- a/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py +++ b/src/subsystems/navigation/nav_bringup/launch/emmn.launch.py @@ -94,7 +94,7 @@ def launch_setup(context, *args, **kwargs): PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', PathJoinSubstitution([ - FindPackageShare('description'), 'urdf', 'athena_drive.urdf.xacro' + FindPackageShare('description'), 'urdf', 'drive', 'athena_drive.urdf.xacro' ]), ]) diff --git a/src/subsystems/navigation/nav_bringup/launch/nav_bringup.launch.py b/src/subsystems/navigation/nav_bringup/launch/nav_bringup.launch.py index 6ca05525..0fdb32fd 100644 --- a/src/subsystems/navigation/nav_bringup/launch/nav_bringup.launch.py +++ b/src/subsystems/navigation/nav_bringup/launch/nav_bringup.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', PathJoinSubstitution([ - FindPackageShare('description'), 'urdf', 'athena_drive.urdf.xacro' + FindPackageShare('description'), 'urdf', 'drive', 'athena_drive.urdf.xacro' ]), ' ', 'can_interface:=', diff --git a/src/subsystems/navigation/waypoint_manager/setup.py b/src/subsystems/navigation/waypoint_manager/setup.py index a71ee970..9f88c5a9 100644 --- a/src/subsystems/navigation/waypoint_manager/setup.py +++ b/src/subsystems/navigation/waypoint_manager/setup.py @@ -20,7 +20,6 @@ maintainer_email='abhinav.kota06@gmail.com', description='Waypoint management and navigation node for simulation', license='MIT', - tests_require=['pytest'], entry_points={ 'console_scripts': [ 'waypoint_manager_node = waypoint_manager.waypoint_manager_node:main', diff --git a/src/subsystems/navigation/yolo_ros_bt/package.xml b/src/subsystems/navigation/yolo_ros_bt/package.xml index 75ad3402..8df7902b 100644 --- a/src/subsystems/navigation/yolo_ros_bt/package.xml +++ b/src/subsystems/navigation/yolo_ros_bt/package.xml @@ -6,8 +6,6 @@ abdul MIT - ament_python - rclpy std_msgs geometry_msgs @@ -19,4 +17,4 @@ ament_python - \ No newline at end of file + diff --git a/src/subsystems/navigation/yolo_ros_bt/setup.py b/src/subsystems/navigation/yolo_ros_bt/setup.py index 20e5a857..1ddaee62 100644 --- a/src/subsystems/navigation/yolo_ros_bt/setup.py +++ b/src/subsystems/navigation/yolo_ros_bt/setup.py @@ -32,10 +32,9 @@ maintainer_email='abdul@example.com', description='YOLO ROS 2 perception node for Behavior Tree integration', license='Apache-2.0', - tests_require=['pytest'], entry_points={ 'console_scripts': [ 'yolo_ros_node = yolo_ros_bt.yolo_ros_node:main', ], }, -) \ No newline at end of file +) diff --git a/src/subsystems/science/README.md b/src/subsystems/science/README.md index 79281c4f..6ee13bd1 100644 --- a/src/subsystems/science/README.md +++ b/src/subsystems/science/README.md @@ -1,63 +1,106 @@ -# athena_science +# UMD Loop's Science software integration -This repository contains all packages for the Science subteam for UMD Loop's 2024-25 rover, Athena. +## Current package list in science subsystem: ------ +
    +
  • science_bringup: contains launch files, controller configuration, service definitions, and joystick configuration for science
    • +
  • science_controllers: contains custom science controllers and related scripts
    • +
-## Guidelines -* Fork this repository for development, you can open a pull request to this repo to add your changes for everyone. -* Do NOT force push. -* Follow best practices (descriptive commit messages, well documented code, etc.) +--- -## Versions -This code is written for an Ubuntu 22.04 system with ROS2 Humble Hawksbill and Gazebo Harmonic. +### Launch Modes -## Building +`athena_science.launch.py` accepts a `mode` argument that controls which nodes are started. -Note that to build this repository, you must have a [workspace](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html). You can clone this repository into the src/ folder. +By default (`mode:=standalone`), all nodes run on a single machine. For competition, split across two machines using the `mode` argument: -### Installing Dependencies +- `standalone` (default): starts all nodes on one machine (control + joystick) +- `jetson`: starts only the control and hardware nodes, to be run on the rover +- `base_station`: starts only the joystick node, to be run on the base stati + +`athena_science.launch.py` currently launches the science subsystem. -Inside of your workspace, run: ```bash -rosdep install --from-paths src -y --ignore-src +source install/setup.bash +ros2 launch science_bringup athena_science.launch.py ``` -This will search through the `package.xml` files and install the necessary packages. Read more about rosdep and how to properly configure your packages [here](https://docs.ros.org/en/humble/Tutorials/Intermediate/Rosdep.html). -### Building +```bash +source install/setup.bash +ros2 launch science_bringup athena_science.launch.py mode:=jetson can_interface:=can1 +``` -Inside of your workspace, run: ```bash -colcon build --packages-select +source install/setup.bash +ros2 launch science_bringup athena_science.launch.py mode:=base_station ``` -You can also do ``colcon build`` to build all packages. +--- + +### Other Launch Parameters + +#### Mock Hardware + +Allows user to implement mock hardware components which will allow for virtual testing. + +_Hardware:_ + +```bash +source install/setup.bash +ros2 launch science_bringup athena_science.launch.py use_mock_hardware:=false +``` -After building, source your install with: +_Mock (no hardware):_ ```bash source install/setup.bash +ros2 launch science_bringup athena_science.launch.py use_mock_hardware:=true ``` +#### Talon Deactivation +The Talon motor controllers overflow the CANBus, it can be helpful to debug with them off. -### How to use Control Switching +_Deactivates Talons:_ -1. **Build the package:** ```bash -colcon build --packages-select athena_science_bringup -./launch.sh source install/setup.bash +ros2 launch science_bringup athena_science.launch.py deactivate_talon:=true ``` -2. **Open a new terminal, source the workspace, and run the controller switcher:** + +#### Robot Controller + +Specify desired ROS2 controller. Currently, it is only the manual science controller. + +_Science Controller:_ + ```bash source install/setup.bash -ros2 run athena_science_bringup controller_switcher.py +ros2 launch science_bringup athena_science.launch.py robot_controller:=science_controller ``` -3. **Open another terminal, source the workspace, and call the service to set controllers:** +#### CAN Interface + +The Jetson currently uses a CANable to implement CAN, not its own CAN controller. This results in 2 can interfaces, can0 and can1, with the current working implementation using the latter. + +_CAN Interface:_ + ```bash source install/setup.bash -ros2 service call /set_controller athena_science_bringup/srv/SetController "{controller_names: [INCLUDE CONTROLLER(S) YOU WANT WITHIN BRACKETS]}" +ros2 launch science_bringup athena_science.launch.py can_interface:=can0 ``` -Example: `ros2 service call /set_controller athena_science_bringup/srv/SetController "{controller_names: [joint_group_position_controller, joint_group_velocity_controller]}"` + +### Testing + +#### Manual Controller Testing + +1. Make sure to plug in joystick controller +2. Open a new terminal +3. Switch to desired controller + - Manual Science Controller + +##### ROS2 Controller: Science + +![Alt text](../../../docs/science_controls_manual.png) +--- diff --git a/src/subsystems/science/science_bringup/launch/athena_science.launch.py b/src/subsystems/science/science_bringup/launch/athena_science.launch.py index fdf3be13..352bc68a 100644 --- a/src/subsystems/science/science_bringup/launch/athena_science.launch.py +++ b/src/subsystems/science/science_bringup/launch/athena_science.launch.py @@ -20,17 +20,37 @@ # from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction +from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler, TimerAction +from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit, OnProcessStart from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from launch.actions import TimerAction def generate_launch_description(): # Declare arguments declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "mode", + default_value="standalone", + choices=["standalone", "jetson", "base_station"], + description=( + "Deployment mode. " + "'standalone' (default) starts all nodes on a single machine. " + "'jetson' starts only the control/hardware nodes (run on the rover). " + "'base_station' starts only the teleop node (joystick)." + ), + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_sim", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) declared_arguments.append( DeclareLaunchArgument( "runtime_config_package", @@ -57,7 +77,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "description_file", - default_value="athena_science.urdf.xacro", + default_value="science/athena_science.urdf.xacro", description="URDF/XACRO description file with the robot.", ) ) @@ -108,7 +128,13 @@ def generate_launch_description(): ) ) + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + + +def launch_setup(context, *args, **kwargs): # Initialize Arguments + mode = LaunchConfiguration("mode").perform(context) + use_sim = LaunchConfiguration("use_sim") runtime_config_package = LaunchConfiguration("runtime_config_package") controllers_file = LaunchConfiguration("controllers_file") description_package = LaunchConfiguration("description_package") @@ -117,6 +143,7 @@ def generate_launch_description(): use_mock_hardware = LaunchConfiguration("use_mock_hardware") mock_sensor_commands = LaunchConfiguration("mock_sensor_commands") robot_controller = LaunchConfiguration("robot_controller") + robot_controller_name = robot_controller.perform(context) deactivate_talon = LaunchConfiguration("deactivate_talon") can_interface = LaunchConfiguration("can_interface") @@ -193,6 +220,7 @@ def generate_launch_description(): name="rviz2", output="log", arguments=["-d", rviz_config_file], + condition=IfCondition(use_sim), ) joint_state_broadcaster_spawner = Node( @@ -222,7 +250,7 @@ def generate_launch_description(): )''' # Active Spawners - robot_controller_names = ["science_controller"] # robot_controller + robot_controller_names = [robot_controller_name] robot_controller_spawners = [] for controller in robot_controller_names: robot_controller_spawners += [ @@ -310,6 +338,13 @@ def generate_launch_description(): ) ) + delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[rviz_node], + ) + ) + umdloop_can_node = Node( package='umdloop_can', executable='can_node', @@ -350,19 +385,28 @@ def generate_launch_description(): ) ] - return LaunchDescription( - declared_arguments - + [ - control_node, - robot_state_pub_node, - rviz_node, - delay_joint_state_broadcaster_spawner_after_ros2_control_node, - delay_motor_status_controller_after_joint_state_broadcaster, - # umdloop_can_node, - controller_switcher_node, - joystick_publisher, - ] - + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner - + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner - + delay_gpio_controller_spawners_after_joint_state_broadcaster_spawner - ) + jetson_actions = [ + control_node, + robot_state_pub_node, + delay_joint_state_broadcaster_spawner_after_ros2_control_node, + delay_motor_status_controller_after_joint_state_broadcaster, + # umdloop_can_node, + controller_switcher_node, + ] + delay_robot_controller_spawners_after_joint_state_broadcaster_spawner \ + + delay_inactive_robot_controller_spawners_after_joint_state_broadcaster_spawner \ + + delay_gpio_controller_spawners_after_joint_state_broadcaster_spawner + + base_station_actions = [ + joystick_publisher, + ] + + standalone_visualization_actions = [ + delay_rviz_after_joint_state_broadcaster_spawner, + ] + + if mode == "jetson": + return jetson_actions + elif mode == "base_station": + return base_station_actions + else: + return jetson_actions + base_station_actions + standalone_visualization_actions diff --git a/src/subsystems/science/science_bringup/launch/test_forward_position_controller.launch.py b/src/subsystems/science/science_bringup/launch/test_forward_position_controller.launch.py deleted file mode 100644 index 8cb14c77..00000000 --- a/src/subsystems/science/science_bringup/launch/test_forward_position_controller.launch.py +++ /dev/null @@ -1,46 +0,0 @@ -# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) -# -# 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. - -# -# Source of this file are templates in -# [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -# -# Author: Dr. Denis -# - -from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - position_goals = PathJoinSubstitution( - [FindPackageShare("science_bringup"), "config", "test_goal_publishers_config.yaml"] - ) - - return LaunchDescription( - [ - Node( - package="ros2_controllers_test_nodes", - executable="publisher_forward_position_controller", - name="publisher_forward_position_controller", - parameters=[position_goals], - output={ - "stdout": "screen", - "stderr": "screen", - }, - ) - ] - ) diff --git a/src/teleop/teleop/setup.py b/src/teleop/teleop/setup.py index a8fae374..2d074759 100644 --- a/src/teleop/teleop/setup.py +++ b/src/teleop/teleop/setup.py @@ -17,7 +17,6 @@ maintainer_email='duttaishan01@todo.todo', description='TODO: Package description', license='TODO: License declaration', - tests_require=['pytest'], entry_points={ 'console_scripts': [ 'joystick = teleop.joystick_publisher:main' diff --git a/src/third-party/zed-ros2-wrapper/README.md b/src/third-party/zed-ros2-wrapper/README.md index e83d2055..c284019d 100644 --- a/src/third-party/zed-ros2-wrapper/README.md +++ b/src/third-party/zed-ros2-wrapper/README.md @@ -91,6 +91,7 @@ source ~/.bashrc To start the ZED node, open a bash terminal and use the [CLI](https://index.ros.org/doc/ros2/Tutorials/Introspection-with-command-line-tools/) command `ros2 launch`: ```bash +source install/setup.bash ros2 launch zed_wrapper zed_camera.launch.py camera_model:= ``` @@ -104,7 +105,9 @@ A Robot State Publisher node is started to publish the camera static links and j You can get the list of all the available launch parameters by using the `-s` launch option: ```bash +source install/setup.bash ros2 launch zed_wrapper zed_camera.launch.py -s +source install/setup.bash ros2 launch zed_display_rviz2 display_zed_cam.launch.py -s ``` @@ -123,6 +126,7 @@ You can also quickly check that your depth data is correctly retrieved in RViz w Launch a standalone ZED ROS 2 node with simulated ZED data as input by using the following command: ```bash +source install/setup.bash ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zedx sim_mode:=true ``` @@ -137,6 +141,7 @@ Launch options: You can also start a preconfigured instance of `rviz2` to visualize all the information available in the simulation by using the command: ```bash +source install/setup.bash ros2 launch zed_display_rviz2 display_zed_cam.launch.py camera_model:=zedx sim_mode:=true ``` diff --git a/src/third-party/zed-ros2-wrapper/zed_debug/README.md b/src/third-party/zed-ros2-wrapper/zed_debug/README.md index a151a97a..992fe05c 100644 --- a/src/third-party/zed-ros2-wrapper/zed_debug/README.md +++ b/src/third-party/zed-ros2-wrapper/zed_debug/README.md @@ -67,6 +67,7 @@ The `zed_camera_debug.launch.py` launch file has been modified to accept a `cmd_ Run the launch file with the `gdbserver` prefix: ```bash +source install/setup.bash ros2 launch zed_debug zed_camera_debug.launch.py camera_model:= cmd_prefix:='gdbserver localhost:3000' ``` @@ -75,6 +76,7 @@ ros2 launch zed_debug zed_camera_debug.launch.py camera_model:= cmd_prefix:='gdb --args' ``` @@ -87,6 +89,7 @@ You can also run the ZED ROS2 nodes with `valgrind` to check for memory leaks an Run the launch file with the `valgrind` prefix: ```bash +source install/setup.bash ros2 launch zed_debug zed_camera_debug.launch.py camera_model:= cmd_prefix:='valgrind --leak-check=full --track-origins=yes' ```