diff --git a/.github/workflows/catkin.yml b/.github/workflows/catkin.yml
deleted file mode 100644
index e3e599cbf..000000000
--- a/.github/workflows/catkin.yml
+++ /dev/null
@@ -1,26 +0,0 @@
-name: CI
-on:
- pull_request:
- branches:
- - master
-jobs:
- build:
- name: Build and Test
- runs-on: ubuntu-latest
- container:
- image: umrover1/ros:latest
- options: --user root
- if: github.event.pull_request.draft == false
- steps:
- - uses: actions/checkout@v3
- with:
- lfs: "true"
- path: "src"
- - name: Initialize catkin workspace
- run: . /opt/ros/noetic/setup.sh && catkin init
- - name: Build
- run: . /opt/ros/noetic/setup.sh && catkin build -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_CLANG_TIDY=clang-tidy-12
- - name: Python requirements
- run: pip3 install -r $GITHUB_WORKSPACE/src/requirements.txt
- - name: Test
- run: . /opt/ros/noetic/setup.sh && . $GITHUB_WORKSPACE/devel/setup.sh && catkin test -j1
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
new file mode 100644
index 000000000..d8585ce5d
--- /dev/null
+++ b/.github/workflows/ci.yml
@@ -0,0 +1,43 @@
+name: CI
+on:
+ pull_request:
+ branches:
+ - master
+ push:
+ branches:
+ - master
+jobs:
+ ci:
+ name: CI
+ runs-on: ubuntu-latest
+ container:
+ image: umrover1/ros:latest
+ options: --user root
+ steps:
+ - uses: actions/checkout@v4
+ with:
+ lfs: "true"
+ # This makes sure that $GITHUB_WORKSPACE is the catkin workspace path
+ path: "src/mrover"
+ - name: Ensure Python Requirements
+ run: . /home/mrover/catkin_ws/src/mrover/venv/bin/activate && pip install -e "$GITHUB_WORKSPACE/src/mrover[dev]"
+ - name: Style Check
+ run: . /home/mrover/catkin_ws/src/mrover/venv/bin/activate && cd $GITHUB_WORKSPACE/src/mrover/ && ./style.sh
+ - name: Update ROS APT
+ if: github.event.pull_request.draft == false
+ run: runuser -u mrover -- rosdep update
+ - name: Ensure ROS APT Requirements
+ if: github.event.pull_request.draft == false
+ run: runuser -u mrover -- rosdep install --from-paths "$GITHUB_WORKSPACE/src" --ignore-src -r -y --rosdistro noetic
+ - name: Copy Catkin Profiles
+ if: github.event.pull_request.draft == false
+ run: rsync -r $GITHUB_WORKSPACE/src/mrover/ansible/roles/build/files/profiles $GITHUB_WORKSPACE/.catkin_tools
+ - name: Initialize
+ if: github.event.pull_request.draft == false
+ run: . /opt/ros/noetic/setup.sh && catkin init && catkin profile set ci
+ - name: Build
+ if: github.event.pull_request.draft == false
+ run: . /opt/ros/noetic/setup.sh && . /home/mrover/catkin_ws/src/mrover/venv/bin/activate && catkin build
+ - name: Test
+ if: github.event.pull_request.draft == false
+ run: . /opt/ros/noetic/setup.sh && . /home/mrover/catkin_ws/src/mrover/venv/bin/activate && . $GITHUB_WORKSPACE/devel/setup.sh && catkin test -j1
diff --git a/.github/workflows/style.yml b/.github/workflows/style.yml
deleted file mode 100644
index 545203560..000000000
--- a/.github/workflows/style.yml
+++ /dev/null
@@ -1,19 +0,0 @@
-name: Style
-on:
- pull_request:
- branches:
- - master
- push:
- branches:
- - master
-jobs:
- code-style:
- name: Code Style Check
- runs-on: ubuntu-latest
- container:
- image: umrover1/ros:latest
- options: --user root
- steps:
- - uses: actions/checkout@v3
- - name: Style check C++/Python
- run: $GITHUB_WORKSPACE/style.sh
diff --git a/.gitignore b/.gitignore
index 4cfeae4cf..b33a46b98 100644
--- a/.gitignore
+++ b/.gitignore
@@ -3,34 +3,34 @@ __pycache__/
*.py[cod]
*$py.class
.mypy_cache/
+*.egg-info/
# Common IDE's, ideally this should be in global gitignore per user
-**/.vscode/
-**/.idea/
-cmake-build*/
+.vscode/
+.idea/
+/cmake-build*/
#GUI Files
+node_modules/
/src/teleop/gui/dist/
-/src/teleop/gui/node_modules/
/src/teleop/gui/src/static/map
-**/keys.json*
-**/yarn.lock*
-**/yarn-error.log*
+keys.json*
+yarn.lock*
+yarn-error.log*
# Bag Files
-bags/
-
-# Bag Files
-bags/
+/bags/
# Catkin
-.catkin_tools/
-build/
-devel/
-logs/
+/build/
+/devel/
+/logs/
# Moteus
moteus-cal*
# CSV
-*.csv
\ No newline at end of file
+*.csv
+
+# Virtual Environment
+/venv/
diff --git a/.venv b/.venv
new file mode 100644
index 000000000..5ceb3864c
--- /dev/null
+++ b/.venv
@@ -0,0 +1 @@
+venv
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 81ee62d4a..b682be343 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,11 +1,12 @@
cmake_minimum_required(VERSION 3.16)
-project(mrover VERSION 1.0.0 LANGUAGES CXX)
+project(mrover VERSION 2024.0.0 LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 23)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
+if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+ add_link_options(-fuse-ld=lld-16)
+endif ()
set(MROVER_CPP_COMPILE_OPTIONS -Wall -Wextra -Werror -pedantic)
-add_link_options(-fuse-ld=lld-16)
-#add_compile_definitions(_LIBCPP_ENABLE_CXX17_REMOVED_FEATURES _LIBCPP_ENABLE_CXX20_REMOVED_FEATURES)
# Generate compile_commands.json for clangd
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
@@ -151,7 +152,7 @@ macro(add_tests_macro)
endmacro()
# Subdirectories before message declarations
-set(CMAKE_SUBDIRS "")
+set(MROVER_CMAKE_INCLUDES "")
# -=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
@@ -201,31 +202,11 @@ else ()
rosbridge_server
teleop_twist_joy
gazebo_ros
- moveit_commander
- moveit_core
- moveit_fake_controller_manager
- moveit_kinematics
- moveit_msgs
- moveit_planners_chomp
- moveit_planners_ompl
- moveit_ros_benchmarks
- moveit_ros_control_interface
- moveit_ros_manipulation
- moveit_ros_move_group
- moveit_ros_occupancy_map_monitor
- moveit_ros_perception
- moveit_ros_planning
- moveit_ros_planning_interface
- moveit_ros_robot_interaction
- moveit_ros_visualization
- moveit_ros_warehouse
- moveit_setup_assistant
- moveit_simple_controller_manager
)
# append subdirectories
- list(APPEND CMAKE_SUBDIRS
- starter_project/teleop
+ list(APPEND MROVER_CMAKE_INCLUDES
+ starter_project/autonomy/AutonomyStarterProject.cmake
)
# -=-=-=-=-
@@ -237,7 +218,7 @@ else ()
find_package(OpenCV REQUIRED COMPONENTS core aruco)
find_package(gazebo REQUIRED)
find_package(Eigen3 REQUIRED)
- find_package(ZED 2)
+ find_package(ZED 2 QUIET)
if (ZED_FOUND)
set(CMAKE_CUDA_SEPARABLE_COMPILATION ON)
set(CMAKE_CUDA_STANDARD 17)
@@ -298,12 +279,10 @@ endif ()
catkin_python_setup()
-# 4.5. Subdirectories before message declarations
-if (NOT "${CMAKE_SUBDIRS}" MATCHES "")
- add_subdirectory(
- ${CMAKE_SUBDIRS}
- )
-endif ()
+# 4.5. CMake includes before message declarations
+foreach (CMAKE_INCLUDE ${MROVER_CMAKE_INCLUDES})
+ include(${CMAKE_INCLUDE})
+endforeach ()
# 5. Message Generators (add_xxx)
diff --git a/Dockerfile b/Dockerfile
index 6d02b1f80..cd65b024e 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -1,33 +1,34 @@
FROM ros:noetic
-RUN apt-get update && apt-get upgrade -y
-# Add apt repo for latest version of Git
-RUN apt-get install software-properties-common -y && add-apt-repository ppa:git-core/ppa -y
-RUN apt-get update && apt-get install -y \
- zsh neovim sudo git git-lfs \
- clang-format-12 clang-tidy-12 \
- python3-catkin-tools python3-pip
-RUN DEBIAN_FRONTEND=noninteractive apt-get install keyboard-configuration -y
+# DEBIAN_FRONTEND=noninteractive and keyboard-configuration are needed to prevent stdin prompting later on
+# This was super annoying to figure out because otherwise the build would hang
+# software-properties-common is needed for apt-add-repository
+RUN apt-get update -y && apt-get upgrade -y && DEBIAN_FRONTEND=noninteractive apt-get install software-properties-common keyboard-configuration -y
+RUN apt-add-repository ppa:ansible/ansible -y
+RUN apt-add-repository ppa:git-core/ppa -y
+RUN apt-get install -y ansible git git-lfs
RUN useradd --create-home --groups sudo --shell /bin/zsh mrover
# Give mrover user sudo access with no password
RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers
USER mrover
-RUN mkdir -p ~/catkin_ws/src/mrover
+RUN mkdir -p /home/mrover/catkin_ws/src/mrover
WORKDIR /home/mrover/catkin_ws/src/mrover
-# ROS package manager (rosdep) reads this file to install dependencies
+# Defines the APT packages that need to be installed
+# rosdep is called from Ansible to install them
ADD ./package.xml .
-# Python package manager (pip) reads this file to install dependencies
-ADD ./requirements.txt .
-# Install ROS packages
-RUN rosdep update && rosdep install --from-paths . --ignore-src -y --rosdistro=noetic
+# Defines the Python packages that need to be installed
+# pip is called from Ansible to install them
+ADD ./pyproject.toml .
+# Copy over all Ansible files
+ADD ./ansible ./ansible
+ADD ./ansible.sh .
+RUN ./ansible.sh build.yml
USER root
# Remove apt cache to free up space in the image
RUN apt-get clean && rm -rf /var/lib/apt/lists/*
-# Install Python packags, sudo so it is a global install
-RUN pip3 install -r ./requirements.txt
USER mrover
ENTRYPOINT [ "/bin/zsh" ]
diff --git a/ansible.sh b/ansible.sh
index 5cb4af68e..f37d0b3cc 100755
--- a/ansible.sh
+++ b/ansible.sh
@@ -9,6 +9,6 @@ fi
sudo -v # Ensure Ansible has sudo permission
-MROVER_PATH=$(dirname "$0")
-CATKIN_WORKSPACE_PATH=$(realpath "${MROVER_PATH}"/../..)
+readonly MROVER_PATH=$(dirname "$0")
+readonly CATKIN_WORKSPACE_PATH=$(realpath "${MROVER_PATH}"/../..)
ansible-playbook -i "localhost," -c local "${MROVER_PATH}"/ansible/"$1" --extra-vars "catkin_workspace=${CATKIN_WORKSPACE_PATH}"
diff --git a/ansible/build.yml b/ansible/build.yml
new file mode 100644
index 000000000..09aeea3c3
--- /dev/null
+++ b/ansible/build.yml
@@ -0,0 +1,7 @@
+---
+- hosts: all
+ vars:
+ ros_distro: noetic
+ ubuntu_release: focal
+ roles:
+ - build
diff --git a/ansible/dev.yml b/ansible/dev.yml
index 0c1e2164b..3ef85f32c 100644
--- a/ansible/dev.yml
+++ b/ansible/dev.yml
@@ -4,4 +4,5 @@
ros_distro: noetic
ubuntu_release: focal
roles:
+ - build
- dev
diff --git a/ansible/jetson.yml b/ansible/jetson.yml
index 444c73478..9aa5b9b83 100644
--- a/ansible/jetson.yml
+++ b/ansible/jetson.yml
@@ -4,8 +4,7 @@
ros_distro: noetic
ubuntu_release: focal
roles:
- - jetson_dev
- dev
+ - jetson_dev
- jetson_networks
- jetson_services
-
diff --git a/ansible/roles/basestation_networks/tasks/main.yml b/ansible/roles/basestation_networks/tasks/main.yml
index dd62adb00..dded708bd 100644
--- a/ansible/roles/basestation_networks/tasks/main.yml
+++ b/ansible/roles/basestation_networks/tasks/main.yml
@@ -2,13 +2,14 @@
become: True
nmcli:
conn_name: MRover
+ state: present
type: ethernet
ifname: enp5s0
- # Share connection to the Internet
- method4: shared
- state: present
autoconnect: yes
ip4: 10.0.0.2/24
+ # Share connection to the Internet
+ method4: shared
+ method6: disabled
- name: Jetson SSH Config
community.general.ssh_config:
diff --git a/ansible/roles/build/files/profiles/ci/config.yaml b/ansible/roles/build/files/profiles/ci/config.yaml
new file mode 100644
index 000000000..44661fe8f
--- /dev/null
+++ b/ansible/roles/build/files/profiles/ci/config.yaml
@@ -0,0 +1,27 @@
+authors: [ ]
+blacklist: [ ]
+build_space: build
+catkin_make_args: [ ]
+cmake_args:
+ - -DCMAKE_BUILD_TYPE=Release
+ - -DCMAKE_CXX_FLAGS=-pipe
+ - -DCMAKE_C_COMPILER=clang-16
+ - -DCMAKE_CXX_COMPILER=clang++-16
+ - -DCMAKE_CXX_CLANG_TIDY=clang-tidy-16
+devel_layout: linked
+devel_space: devel
+extend_path: null
+extends: null
+install: false
+install_space: install
+isolate_install: false
+jobs_args: [ ]
+licenses:
+ - TODO
+log_space: logs
+maintainers: [ ]
+make_args: [ ]
+source_space: src
+use_env_cache: false
+use_internal_make_jobserver: true
+whitelist: [ ]
diff --git a/ansible/roles/build/files/profiles/debug/config.yaml b/ansible/roles/build/files/profiles/debug/config.yaml
new file mode 100644
index 000000000..004623cb6
--- /dev/null
+++ b/ansible/roles/build/files/profiles/debug/config.yaml
@@ -0,0 +1,28 @@
+authors: [ ]
+blacklist: [ ]
+build_space: build
+catkin_make_args: [ ]
+cmake_args:
+ - -DCMAKE_BUILD_TYPE=Debug
+ - -DCMAKE_CXX_FLAGS=-pipe
+ - -DCMAKE_C_COMPILER=clang-16
+ - -DCMAKE_CXX_COMPILER=clang++-16
+ - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
+ - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache
+devel_layout: linked
+devel_space: devel
+extend_path: null
+extends: null
+install: false
+install_space: install
+isolate_install: false
+jobs_args: [ ]
+licenses:
+ - TODO
+log_space: logs
+maintainers: [ ]
+make_args: [ ]
+source_space: src
+use_env_cache: false
+use_internal_make_jobserver: true
+whitelist: [ ]
diff --git a/ansible/roles/build/files/profiles/profiles.yaml b/ansible/roles/build/files/profiles/profiles.yaml
new file mode 100644
index 000000000..36cbb5fa7
--- /dev/null
+++ b/ansible/roles/build/files/profiles/profiles.yaml
@@ -0,0 +1 @@
+active: debug
\ No newline at end of file
diff --git a/ansible/roles/build/files/profiles/release/config.yaml b/ansible/roles/build/files/profiles/release/config.yaml
new file mode 100644
index 000000000..d85e121df
--- /dev/null
+++ b/ansible/roles/build/files/profiles/release/config.yaml
@@ -0,0 +1,29 @@
+authors: [ ]
+blacklist: [ ]
+build_space: build
+catkin_make_args: [ ]
+cmake_args:
+ - -DCMAKE_BUILD_TYPE=Release
+ - -DCMAKE_CXX_FLAGS=-march=native -pipe
+ - -DCMAKE_C_COMPILER=clang-16
+ - -DCMAKE_CXX_COMPILER=clang++-16
+ - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
+ - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache
+ - -DCMAKE_CUDA_HOST_COMPILER=g++-9
+devel_layout: linked
+devel_space: devel
+extend_path: null
+extends: null
+install: false
+install_space: install
+isolate_install: false
+jobs_args: [ ]
+licenses:
+ - TODO
+log_space: logs
+maintainers: [ ]
+make_args: [ ]
+source_space: src
+use_env_cache: false
+use_internal_make_jobserver: true
+whitelist: [ ]
diff --git a/ansible/roles/build/tasks/main.yml b/ansible/roles/build/tasks/main.yml
new file mode 100644
index 000000000..170e25ba5
--- /dev/null
+++ b/ansible/roles/build/tasks/main.yml
@@ -0,0 +1,162 @@
+# Allows us to install Python versions newer than 3.8
+- name: Add Python PPA
+ become: True
+ apt_repository:
+ repo: ppa:deadsnakes/ppa
+
+# Allows us to install G++13, so we can use an updated libstdc++ which provides more standard library features (C++20)
+- name: Add GCC PPA
+ become: True
+ apt_repository:
+ repo: ppa:ubuntu-toolchain-r/test
+
+- name: Add LLVM APT Key
+ become: True
+ apt_key:
+ url: https://apt.llvm.org/llvm-snapshot.gpg.key
+
+# Allows us to install Clang 16 and other LLVM tools
+- name: Add LLVM APT List
+ become: True
+ apt_repository:
+ repo: deb http://apt.llvm.org/{{ ubuntu_release }}/ llvm-toolchain-{{ ubuntu_release }}-16 main
+ filename: llvm
+
+- name: Add CMake APT Key
+ become: True
+ apt_key:
+ url: https://apt.kitware.com/keys/kitware-archive-latest.asc
+ keyring: /usr/share/keyrings/kitware-archive-keyring.gpg
+
+# Allows us to install CMake versions newer than 3.16
+- name: Add CMake APT List
+ become: True
+ apt_repository:
+ repo: deb [signed-by=/usr/share/keyrings/kitware-archive-keyring.gpg] https://apt.kitware.com/ubuntu/ {{ ubuntu_release }} main
+ filename: kitware
+
+- name: Add ROS APT Key
+ become: True
+ apt_key:
+ url: https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc
+
+- name: Add ROS APT List
+ become: True
+ apt_repository:
+ repo: deb http://packages.ros.org/ros/ubuntu {{ ubuntu_release }} main
+ filename: ros
+
+- name: Upgrade APT Packages
+ become: True
+ apt:
+ cache_valid_time: 604800
+ state: latest
+ upgrade: yes
+
+- name: Install APT Packages
+ become: True
+ apt:
+ cache_valid_time: 604800
+ state: latest
+ name:
+ - zsh
+ - fzf
+ - neovim
+ - sudo
+ - cmake
+ # Caches intermediate build files, speeds up compilation over time
+ - ccache
+ # Faster than make
+ - ninja-build
+ - tmux
+ - zstd
+ - htop
+ - curl
+ - unzip
+ - rsync
+ - python3-pip
+ - python3-rosdep
+ - python3-catkin-tools
+ - python3-virtualenvwrapper
+ - clang-16
+ - clangd-16
+ - clang-tidy-16
+ - clang-format-16
+ - lld-16
+ - lldb-16
+ - gcc-13
+ - g++-13
+ - python3.10
+ - python3.10-dev
+ - python3.10-venv
+ - ros-{{ ros_distro }}-rosbash
+
+- name: Initialize rosdep
+ become: True
+ command: rosdep init
+ args:
+ # This command will be skipped if this file already exists
+ creates: /etc/ros/rosdep/sources.list.d/20-default.list
+
+- name: Update rosdep
+ command: rosdep update
+
+- name: Install ROS Packages
+ command: rosdep install --from-paths {{ catkin_workspace }}/src --ignore-src -y --rosdistro={{ ros_distro }}
+
+- name: Download Bun Installer
+ get_url:
+ url: https://bun.sh/install
+ dest: /tmp/bun_installer.sh
+ mode: 0755
+
+- name: Run Bun Installer
+ command: /tmp/bun_installer.sh
+ args:
+ creates: ~/.bun/bin/bun
+
+- name: Install Catkin Profiles
+ synchronize:
+ src: files/profiles
+ dest: "{{ catkin_workspace }}/.catkin_tools"
+ recursive: true
+
+- name: Set G++ 13 as Default
+ become: True
+ alternatives:
+ name: g++
+ path: /usr/bin/g++-13
+ link: /usr/bin/g++
+ priority: 130
+
+- name: Set GCC 13 as Default
+ become: True
+ alternatives:
+ name: gcc
+ path: /usr/bin/gcc-13
+ link: /usr/bin/gcc
+ priority: 130
+
+- name: Set Clang++ 16 as Default
+ become: True
+ alternatives:
+ name: clang++
+ path: /usr/bin/clang++-16
+ link: /usr/bin/clang++
+ priority: 160
+
+- name: Set Clang 16 as Default
+ become: True
+ alternatives:
+ name: clang
+ path: /usr/bin/clang-16
+ link: /usr/bin/clang
+ priority: 160
+
+- name: Setup Python Virtual Environment
+ pip:
+ name:
+ # Installs from pyproject.toml
+ - "{{ catkin_workspace }}/src/mrover[dev]"
+ virtualenv: "{{ catkin_workspace }}/src/mrover/venv"
+ virtualenv_command: python3.10 -m venv
diff --git a/ansible/roles/dev/files/home/.oh-my-zsh/custom/themes/mrover.zsh-theme b/ansible/roles/dev/files/home/.oh-my-zsh/custom/themes/mrover.zsh-theme
new file mode 100644
index 000000000..981bdecfb
--- /dev/null
+++ b/ansible/roles/dev/files/home/.oh-my-zsh/custom/themes/mrover.zsh-theme
@@ -0,0 +1,7 @@
+PROMPT="%(?:%{$fg_bold[green]%}➜ :%{$fg_bold[red]%}➜ ) %{$fg[yellow]%}%m %{$fg[gray]%}at %{$fg[yellow]%}%d%{$reset_color%}"
+PROMPT+=' $(git_prompt_info)'
+
+ZSH_THEME_GIT_PROMPT_PREFIX="%{$fg_bold[gray]%}on %{$fg[blue]%}git:(%{$fg[blue]%}"
+ZSH_THEME_GIT_PROMPT_SUFFIX="%{$reset_color%} "
+ZSH_THEME_GIT_PROMPT_DIRTY="%{$fg[blue]%}) %{$fg[yellow]%}*"
+ZSH_THEME_GIT_PROMPT_CLEAN="%{$fg[blue]%})"
diff --git a/ansible/roles/dev/files/home/.tmux.conf b/ansible/roles/dev/files/home/.tmux.conf
new file mode 100644
index 000000000..207d7e8f0
--- /dev/null
+++ b/ansible/roles/dev/files/home/.tmux.conf
@@ -0,0 +1 @@
+set -g mouse on
diff --git a/ansible/roles/dev/files/home/.zshrc b/ansible/roles/dev/files/home/.zshrc
new file mode 100644
index 000000000..3f1f5f1a4
--- /dev/null
+++ b/ansible/roles/dev/files/home/.zshrc
@@ -0,0 +1,110 @@
+# If you come from bash you might have to change your $PATH.
+# export PATH=$HOME/bin:/usr/local/bin:$PATH
+
+# Path to your oh-my-zsh installation.
+export ZSH="$HOME/.oh-my-zsh"
+
+# Set name of the theme to load --- if set to "random", it will
+# load a random theme each time oh-my-zsh is loaded, in which case,
+# to know which specific one was loaded, run: echo $RANDOM_THEME
+# See https://github.com/ohmyzsh/ohmyzsh/wiki/Themes
+ZSH_THEME="mrover"
+
+# Set list of themes to pick from when loading at random
+# Setting this variable when ZSH_THEME=random will cause zsh to load
+# a theme from this variable instead of looking in $ZSH/themes/
+# If set to an empty array, this variable will have no effect.
+# ZSH_THEME_RANDOM_CANDIDATES=( "robbyrussell" "agnoster" )
+
+# Uncomment the following line to use case-sensitive completion.
+# CASE_SENSITIVE="true"
+
+# Uncomment the following line to use hyphen-insensitive completion.
+# Case-sensitive completion must be off. _ and - will be interchangeable.
+# HYPHEN_INSENSITIVE="true"
+
+# Uncomment one of the following lines to change the auto-update behavior
+# zstyle ':omz:update' mode disabled # disable automatic updates
+# zstyle ':omz:update' mode auto # update automatically without asking
+# zstyle ':omz:update' mode reminder # just remind me to update when it's time
+
+# Uncomment the following line to change how often to auto-update (in days).
+# zstyle ':omz:update' frequency 13
+
+# Uncomment the following line if pasting URLs and other text is messed up.
+# DISABLE_MAGIC_FUNCTIONS="true"
+
+# Uncomment the following line to disable colors in ls.
+# DISABLE_LS_COLORS="true"
+
+# Uncomment the following line to disable auto-setting terminal title.
+# DISABLE_AUTO_TITLE="true"
+
+# Uncomment the following line to enable command auto-correction.
+# ENABLE_CORRECTION="true"
+
+# Uncomment the following line to display red dots whilst waiting for completion.
+# You can also set it to another string to have that shown instead of the default red dots.
+# e.g. COMPLETION_WAITING_DOTS="%F{yellow}waiting...%f"
+# Caution: this setting can cause issues with multiline prompts in zsh < 5.7.1 (see #5765)
+# COMPLETION_WAITING_DOTS="true"
+
+# Uncomment the following line if you want to disable marking untracked files
+# under VCS as dirty. This makes repository status check for large repositories
+# much, much faster.
+# DISABLE_UNTRACKED_FILES_DIRTY="true"
+
+# Uncomment the following line if you want to change the command execution time
+# stamp shown in the history command output.
+# You can set one of the optional three formats:
+# "mm/dd/yyyy"|"dd.mm.yyyy"|"yyyy-mm-dd"
+# or set a custom format using the strftime function format specifications,
+# see 'man strftime' for details.
+# HIST_STAMPS="mm/dd/yyyy"
+
+# Would you like to use another custom folder than $ZSH/custom?
+# ZSH_CUSTOM=/path/to/new-custom-folder
+
+# Which plugins would you like to load?
+# Standard plugins can be found in $ZSH/plugins/
+# Custom plugins may be added to $ZSH_CUSTOM/plugins/
+# Example format: plugins=(rails git textmate ruby lighthouse)
+# Add wisely, as too many plugins slow down shell startup.
+plugins=(git virtualenvwrapper fzf)
+
+source $ZSH/oh-my-zsh.sh
+
+# User configuration
+
+# export MANPATH="/usr/local/man:$MANPATH"
+
+# You may need to manually set your language environment
+# export LANG=en_US.UTF-8
+
+# Preferred editor for local and remote sessions
+# if [[ -n $SSH_CONNECTION ]]; then
+# export EDITOR='vim'
+# else
+# export EDITOR='mvim'
+# fi
+
+# Compilation flags
+# export ARCHFLAGS="-arch x86_64"
+
+# Set personal aliases, overriding those provided by oh-my-zsh libs,
+# plugins, and themes. Aliases can be placed here, though oh-my-zsh
+# users are encouraged to define aliases within the ZSH_CUSTOM folder.
+# For a full list of active aliases, run `alias`.
+#
+# Example aliases
+# alias zshconfig="mate ~/.zshrc"
+# alias ohmyzsh="mate ~/.oh-my-zsh"
+
+alias mrover="cd ~/catkin_ws/src/mrover"
+
+readonly CATKIN_WORKSPACE_PATH=~/catkin_ws
+source /opt/ros/noetic/setup.zsh
+readonly CATKIN_SETUP_PATH=${CATKIN_WORKSPACE_PATH}/devel/setup.zsh
+if [ -f ${CATKIN_SETUP_PATH} ]; then
+ source ${CATKIN_SETUP_PATH}
+fi
diff --git a/ansible/roles/dev/tasks/main.yml b/ansible/roles/dev/tasks/main.yml
index 7944b288e..7bb562d6b 100644
--- a/ansible/roles/dev/tasks/main.yml
+++ b/ansible/roles/dev/tasks/main.yml
@@ -1,76 +1,38 @@
-- name: Add CMake Key
- become: True
- apt_key:
- url: https://apt.kitware.com/keys/kitware-archive-latest.asc
- keyring: /usr/share/keyrings/kitware-archive-keyring.gpg
-
-- name: Add CMake APT
- become: True
- apt_repository:
- repo: deb [signed-by=/usr/share/keyrings/kitware-archive-keyring.gpg] https://apt.kitware.com/ubuntu/ {{ ubuntu_release }} main
- filename: kitware
+- name: Oh-My-Zsh
+ git:
+ repo: https://github.com/ohmyzsh/ohmyzsh.git
+ dest: /home/{{ ansible_user_id }}/.oh-my-zsh
+ version: master
-- name: Add LLVM Key
- become: True
- apt_key:
- url: https://apt.llvm.org/llvm-snapshot.gpg.key
+- name: Dotfiles
+ synchronize:
+ # The trailing slash is very important here
+ # It tells rsync to copy only the CONTENTS of the folder and not the folder itself
+ # This is vital to make sure this works will any usernames
+ src: files/home/
+ dest: /home/{{ ansible_user_id }}
+ recursive: true
-- name: Add LLVM APT
- become: True
- apt_repository:
- repo: deb http://apt.llvm.org/{{ ubuntu_release }}/ llvm-toolchain-{{ ubuntu_release }}-16 main
- filename: llvm
+- name: Use ZSH as a default shell
+ become: true
+ command: chsh --shell /usr/bin/zsh {{ ansible_user_id }}
-- name: ROS Key
- become: True
+- name: Add VSCode APT Key
+ become: true
apt_key:
- url: https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc
+ url: https://packages.microsoft.com/keys/microsoft.asc
+ keyring: /usr/share/keyrings/packages.microsoft.gpg
-- name: ROS APT
- become: True
+- name: Add VSCode APT List
+ become: true
apt_repository:
- repo: deb http://packages.ros.org/ros/ubuntu {{ ubuntu_release }} main
- filename: ros
+ filename: vscode
+ repo: "deb [arch=amd64,arm64,armhf signed-by=/usr/share/keyrings/packages.microsoft.gpg] https://packages.microsoft.com/repos/code stable main"
-- name: Install APT Packages
- become: True
+- name: Install VSCode APT Package
+ become: true
apt:
cache_valid_time: 604800
state: latest
name:
- - zsh
- - fzf
- - neovim
- - sudo
- - cmake
- - ccache
- - ninja-build
- - tmux
- - zstd
- - htop
- - python3-pip
- - python3-rosdep
- - python3-catkin-tools
- - clang-16
- - clangd-16
- - lld-16
- - lldb-16
- - ros-{{ ros_distro }}-rosbash
-
-- name: Initialize rosdep
- become: True
- command: rosdep init
- args:
- creates: /etc/ros/rosdep/sources.list.d/20-default.list
-
-- name: Update rosdep
- command: rosdep update
-
-- name: Install ROS Packages
- command: rosdep install --from-paths {{ catkin_workspace }}/src --ignore-src -y --rosdistro={{ ros_distro }}
-
-- name: Install Catkin Profiles
- synchronize:
- src: files/profiles
- dest: "{{ catkin_workspace }}/.catkin_tools"
- recursive: true
+ - code
diff --git a/ansible/roles/jetson_dev/tasks/main.yml b/ansible/roles/jetson_dev/tasks/main.yml
index 52d28088c..497da4e44 100644
--- a/ansible/roles/jetson_dev/tasks/main.yml
+++ b/ansible/roles/jetson_dev/tasks/main.yml
@@ -1,18 +1,3 @@
-- name: Oh-My-Zsh
- git:
- repo: https://github.com/ohmyzsh/ohmyzsh.git
- dest: /home/{{ ansible_user_id }}/.oh-my-zsh
- version: master
-
-- name: Dotfiles
- synchronize:
- src: files/home
- dest: /home/{{ ansible_user_id }}
- recursive: true
-
-- name: Use ZSH as a default shell
- command: chsh -s /usr/bin/zsh
-
- name: Jetson Stats
become: True
pip:
diff --git a/ansible/roles/jetson_networks/tasks/main.yml b/ansible/roles/jetson_networks/tasks/main.yml
index f7a2fc7d4..4a6f5a769 100644
--- a/ansible/roles/jetson_networks/tasks/main.yml
+++ b/ansible/roles/jetson_networks/tasks/main.yml
@@ -2,8 +2,10 @@
become: True
nmcli:
conn_name: MRover
+ state: present
type: ethernet
ifname: eth0
- state: present
autoconnect: yes
ip4: 10.0.0.10/24
+ gw4: 10.0.0.2
+ method6: disabled
diff --git a/barvis b/barvis
deleted file mode 100644
index f9b8b7167..000000000
Binary files a/barvis and /dev/null differ
diff --git a/bootstrap.sh b/bootstrap.sh
index e35df943b..ce0b241fe 100755
--- a/bootstrap.sh
+++ b/bootstrap.sh
@@ -7,12 +7,15 @@
# See: https://vaneyckt.io/posts/safer_bash_scripts_with_set_euxo_pipefail/
set -Eeuo pipefail
-BLUE='\033[0;34m'
-NC='\033[0m'
+readonly RED_BOLD='\033[1;31m'
+readonly BLUE_BOLD='\033[1;34m'
+readonly GREY_BOLD='\033[1;30m'
+readonly YELLOW_BOLD='\033[1;33m'
+readonly NC='\033[0m'
-echo "Ensuring SSH keys are set up ..."
+echo -e "${GREY_BOLD}Ensuring SSH keys are set up ...${NC}"
if [ ! -f ~/.ssh/id_ed25519 ] && [ ! -f ~/.ssh/id_rsa ]; then
- echo "Please see: https://docs.github.com/en/authentication/connecting-to-github-with-ssh/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent"
+ echo -e "${RED_BOLD}Please see: https://docs.github.com/en/authentication/connecting-to-github-with-ssh/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent${NC}"
exit 1
fi
@@ -24,7 +27,7 @@ NEED_APT_UPDATE=false
for PPA in "${PPAS[@]}"; do
if ! grep -q "^deb .*${PPA}" /etc/apt/sources.list /etc/apt/sources.list.d/*;
then
- echo "Adding PPA: ${PPA}"
+ echo -e "${GREY_BOLD}Adding PPA: ${PPA}${NC}"
sudo apt-add-repository ppa:"${PPA}" -y
NEED_APT_UPDATE=true
fi
@@ -37,20 +40,27 @@ sudo apt install -y ansible git git-lfs
readonly DEFAULT_CATKIN_PATH=~/catkin_ws
-echo "${BLUE}Enter path to ROS workspace... [leave blank for ${DEFAULT_CATKIN_PATH}]:${NC}"
+echo -e "${BLUE_BOLD}[ADVANCED] Enter path to ROS workspace... [leave blank for ${DEFAULT_CATKIN_PATH}]:${NC}"
read -r CATKIN_PATH
if [ -z "${CATKIN_PATH}" ]; then
CATKIN_PATH=${DEFAULT_CATKIN_PATH}
fi
-echo "Using ${CATKIN_PATH} as ROS workspace"
+echo -e "${GREY_BOLD}Using ${CATKIN_PATH} as ROS workspace${NC}"
readonly MROVER_PATH=${CATKIN_PATH}/src/mrover
+FIRST_TIME_SETUP=false
-if [ ! -d "${MROVER_PATH}" ]; then
- echo "Creating ROS workspace ..."
+if [ ! -d ${MROVER_PATH} ]; then
+ echo -e "${GREY_BOLD}Creating ROS workspace ...${NC}"
mkdir -p ${CATKIN_PATH}/src
git clone git@github.com:umrover/mrover-ros ${CATKIN_PATH}/src/mrover
+ FIRST_TIME_SETUP=true
fi
-echo "Using Ansible to finish up ..."
+echo -e "${GREY_BOLD}Using Ansible to finish up ...${NC}"
${MROVER_PATH}/ansible.sh dev.yml
+
+if [ "${FIRST_TIME_SETUP}" ]; then
+ echo -e "${GREY_BOLD}All done! Welcome to MRover!${NC}"
+ echo -e "${YELLOW_BOLD}Please log out and back in!${NC}"
+fi
diff --git a/package.xml b/package.xml
index c7dba2fad..e87ee3c2c 100644
--- a/package.xml
+++ b/package.xml
@@ -1,119 +1,103 @@
- mrover
- 1.0.0
- The MRover Package
-
- MRover
-
-
-
-
- MIT
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- message_generation
-
-
-
-
-
- message_runtime
-
-
-
-
-
-
- catkin
- roscpp
- rospy
- std_msgs
- nodelet
- roscpp
- rospy
- std_msgs
- roscpp
- rospy
- std_msgs
- nodelet
- xacro
- joint_state_publisher
- robot_state_publisher
- ros_numpy
- rosbridge_server
- tf2_web_republisher
-
-
- rviz
- hector_trajectory_server
-
-
- compressed_depth_image_transport
- compressed_image_transport
- theora_image_transport
- tf2_geometry_msgs
- tf2_ros
- tf2
- image_transport
- sensor_msgs
- dynamic_reconfigure
- python3-cairosvg
- python3-joblib
- rtabmap_ros
-
-
- rviz_imu_plugin
- robot_localization
- nmea_navsat_driver
-
-
- smach_ros
-
-
- teleop_twist_joy
-
-
- gazebo_ros
- hector_gazebo
- teleop_twist_keyboard
-
-
- moveit
-
-
- mavlink
- mavros
- mavros_extras
- mavros_msgs
-
-
- rosunit
- rostest
-
-
-
-
-
-
+ mrover
+ 2024.0.0
+ The MRover Package
+
+ MRover
+
+
+
+
+ GPLv3
+
+ https://github.com/umrover/mrover-ros
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ message_generation
+
+
+
+
+
+ message_runtime
+
+
+
+
+
+
+ catkin
+ roscpp
+ rospy
+ std_msgs
+ nodelet
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+ nodelet
+ xacro
+ joint_state_publisher
+ robot_state_publisher
+ rosbridge_server
+ tf2_web_republisher
+
+
+ rviz
+ hector_trajectory_server
+
+
+ tf2
+ tf2_ros
+ tf2_geometry_msgs
+ image_transport
+ theora_image_transport
+ compressed_image_transport
+ compressed_depth_image_transport
+ sensor_msgs
+ dynamic_reconfigure
+
+
+ rviz_imu_plugin
+ robot_localization
+ nmea_navsat_driver
+
+
+ smach_ros
+ control_msgs
+
+
+ teleop_twist_joy
+
+
+ gazebo_ros
+ hector_gazebo
+ teleop_twist_keyboard
+
+
+ rosunit
+ rostest
+
+
+
+
+
+
diff --git a/pyproject.toml b/pyproject.toml
index 01feaf88c..17c26b5cc 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -1,6 +1,6 @@
[project]
name = "mrover"
-version = "1.0.0"
+version = "2024.0.0"
description = "MRover Python Code"
readme = "README.md"
requires-python = ">=3.10.0"
@@ -9,23 +9,26 @@ maintainers = [
{ name = "Michigan Mars Rover Team" }
]
dependencies = [
- "django",
- "empy",
- "numpy",
- "pandas",
- "shapely",
- "pyserial",
- "moteus",
- "pymap3d",
- "aenum",
- "pyyaml",
- "rospkg",
+ # ROS dependencies
+ "rospkg==1.5.0",
+ "netifaces==0.11.0",
+ "defusedxml==0.7.1",
+ # MRover dependencies
+ "Django==4.2.5",
+ "empy==3.3.4",
+ "numpy==1.26.0",
+ "pandas==2.1.0",
+ "shapely==2.0.1",
+ "pyserial==3.5",
+ "moteus==0.3.59",
+ "pymap3d==3.0.1",
+ "aenum==3.1.15",
]
[project.optional-dependencies]
dev = [
- "black",
- "mypy",
+ "black==23.9.1",
+ "mypy==1.5.1",
]
[project.urls]
diff --git a/src/esw/__init__.py b/src/esw/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/src/esw/brushless.py b/src/esw/brushless.py
index 59b9b73ff..0cd9f40bc 100755
--- a/src/esw/brushless.py
+++ b/src/esw/brushless.py
@@ -125,12 +125,10 @@ def is_mode_indicating_error(mode: int) -> bool:
class MoteusBridge:
-
MOTEUS_RESPONSE_TIME_INDICATING_DISCONNECTED_S = 0.01
ROVER_NODE_TO_MOTEUS_WATCHDOG_TIMEOUT_S = 0.1
def __init__(self, can_id: int, transport, gear_ratio: int):
-
self._can_id = can_id
self.controller = moteus.Controller(id=can_id, transport=transport)
self.command_lock = threading.Lock()
@@ -369,7 +367,6 @@ async def send_command(self) -> None:
for bridge in self._motor_bridges.values():
if lost_communication_now:
-
# If we only just lost communications this iteration.
if not self._lost_communication:
rospy.loginfo(
diff --git a/src/esw/cameras.py b/src/esw/cameras.py
index 3d7c20438..9ff860c5a 100755
--- a/src/esw/cameras.py
+++ b/src/esw/cameras.py
@@ -169,7 +169,6 @@ def __del__(self) -> None:
class StreamManager:
-
MAX_STREAMS: int = rospy.get_param("cameras/max_streams")
"""
The maximum number of streams that can be simultaneously sustained.
@@ -316,7 +315,6 @@ def handle_req(self, req: ChangeCamerasRequest) -> ChangeCamerasResponse:
# If a stream is being requested...
# (resolution == -1 means a request to cancel stream)
if req.camera_cmd.resolution >= 0:
-
# If we cannot handle any more streams, return False.
available_port_arr = [True, True, True, True]
for i, stream in enumerate(self._stream_by_device):
@@ -414,7 +412,6 @@ def send(
# Transmit loop
while True:
-
ret, frame = cap_send.read()
if not ret:
rospy.logerr("Empty frame")
diff --git a/src/esw/imu_driver.py b/src/esw/imu_driver.py
index 798f8c92a..22ea8b084 100755
--- a/src/esw/imu_driver.py
+++ b/src/esw/imu_driver.py
@@ -91,7 +91,6 @@ def main():
attempts = 0
while not rospy.is_shutdown():
-
# try to read a line from the serial connection,
# if this fails 100 times in a row then end the program
try:
diff --git a/src/esw/science.py b/src/esw/science.py
index 71ed6cdbe..a77cdf69b 100755
--- a/src/esw/science.py
+++ b/src/esw/science.py
@@ -90,7 +90,6 @@ class ScienceBridge:
_last_mcu_active: Bool
def __init__(self) -> None:
-
self._active_publisher = rospy.Publisher("science_mcu_active", Bool, queue_size=1)
self._mcu_active_timeout_s = rospy.get_param("science/info/mcu_active_timeout_s")
diff --git a/src/navigation/approach_post.py b/src/navigation/approach_post.py
index 424a93754..bd4c505b0 100644
--- a/src/navigation/approach_post.py
+++ b/src/navigation/approach_post.py
@@ -1,9 +1,9 @@
import tf2_ros
-import rospy
-from context import Context
from aenum import Enum, NoAlias
from geometry_msgs.msg import Twist
from util.ros_utils import get_rosparam
+
+from context import Context
from state import BaseState
diff --git a/src/navigation/context.py b/src/navigation/context.py
index 23cdac886..aa55d29a0 100644
--- a/src/navigation/context.py
+++ b/src/navigation/context.py
@@ -1,25 +1,23 @@
from __future__ import annotations
-from pdb import post_mortem
-import rospy
-import tf2_ros
-from geometry_msgs.msg import Twist
+
+from dataclasses import dataclass
+from typing import ClassVar, Optional, List, Tuple
+
import mrover.msg
import mrover.srv
-from util.SE3 import SE3
-from visualization_msgs.msg import Marker
-from typing import ClassVar, Optional, List, Tuple
import numpy as np
-from dataclasses import dataclass
-from shapely.geometry import Point, LineString
-from mrover.msg import Waypoint, GPSWaypoint, EnableAuton, WaypointType, GPSPointList
import pymap3d
-from drive import DriveController
-from util.ros_utils import get_rosparam
-
+import rospy
+import tf2_ros
+from geometry_msgs.msg import Twist
+from mrover.msg import Waypoint, GPSWaypoint, EnableAuton, WaypointType, GPSPointList
+from shapely.geometry import Point
from std_msgs.msg import Time, Bool
-from drive import DriveController
+from util.SE3 import SE3
from util.ros_utils import get_rosparam
+from visualization_msgs.msg import Marker
+from drive import DriveController
TAG_EXPIRATION_TIME_SECONDS = 60
diff --git a/src/navigation/drive.py b/src/navigation/drive.py
index 31057f2a0..1264c345c 100644
--- a/src/navigation/drive.py
+++ b/src/navigation/drive.py
@@ -1,9 +1,9 @@
from __future__ import annotations
-from typing import Tuple, Optional
-import numpy as np
-import rospy
+
from enum import Enum
+from typing import Tuple, Optional
+import numpy as np
from geometry_msgs.msg import Twist
from util.SE3 import SE3
from util.np_utils import angle_to_rotate, normalized
diff --git a/src/navigation/failure_identification/failure_identification.py b/src/navigation/failure_identification/failure_identification.py
index 23cf994f4..3e9123825 100755
--- a/src/navigation/failure_identification/failure_identification.py
+++ b/src/navigation/failure_identification/failure_identification.py
@@ -1,20 +1,21 @@
#!/usr/bin/env python3
-import rospy
+
+from pathlib import Path
+from typing import Optional
+
import message_filters
-from mrover.msg import MotorsStatus
+import numpy as np
+import pandas as pd
+import rospy
from geometry_msgs.msg import Twist
+from mrover.msg import MotorsStatus
from nav_msgs.msg import Odometry
+from pandas import DataFrame
from smach_msgs.msg import SmachContainerStatus
from std_msgs.msg import Bool
-import pandas as pd
-from pandas import DataFrame
-from watchdog import WatchDog
-import numpy as np
-import os
-from pathlib import Path
from util.ros_utils import get_rosparam
-from util.SO3 import SO3
-from typing import Optional
+
+from watchdog import WatchDog
DATAFRAME_MAX_SIZE = get_rosparam("failure_identification/dataframe_max_size", 200)
POST_RECOVERY_GRACE_PERIOD = get_rosparam("failure_identification/post_recovery_grace_period", 5.0)
diff --git a/src/navigation/failure_identification/watchdog.py b/src/navigation/failure_identification/watchdog.py
index 5ad0cbd9b..8a63fe9e6 100644
--- a/src/navigation/failure_identification/watchdog.py
+++ b/src/navigation/failure_identification/watchdog.py
@@ -1,9 +1,9 @@
+from typing import Tuple
+
import numpy as np
-import rospy
from pandas import DataFrame
-from util.ros_utils import get_rosparam
from util.SO3 import SO3
-from typing import Tuple
+from util.ros_utils import get_rosparam
WINDOW_SIZE = get_rosparam("watchdog/window_size", 100)
ANGULAR_THRESHOLD = get_rosparam("watchdog/angular_threshold", 0.001)
diff --git a/src/navigation/gate.py b/src/navigation/gate.py
index 3635b7faa..280a386cd 100644
--- a/src/navigation/gate.py
+++ b/src/navigation/gate.py
@@ -1,21 +1,18 @@
from __future__ import annotations
-from typing import ClassVar, Optional
-from unicodedata import normalize
-from context import Gate, Context
-import numpy as np
-import rospy
+from dataclasses import dataclass
+from typing import Optional
-from context import Context, Environment, Rover, convert_cartesian_to_gps
+import numpy as np
from aenum import Enum, NoAlias
-from state import BaseState
-from trajectory import Trajectory
-from dataclasses import dataclass
-from drive import DriveController
+from mrover.msg import GPSPointList
+from shapely.geometry import LineString, Polygon, Point
from util.np_utils import normalized, perpendicular_2d
from util.ros_utils import get_rosparam
-from shapely.geometry import LineString, Polygon, Point
-from mrover.msg import GPSPointList
+
+from context import Context, Rover, convert_cartesian_to_gps
+from context import Gate
+from state import BaseState
STOP_THRESH = get_rosparam("gate/stop_thresh", 0.2)
DRIVE_FWD_THRESH = get_rosparam("gate/drive_fwd_thresh", 0.34) # 20 degrees
@@ -216,7 +213,6 @@ class GateTraverseStateTransitions(Enum):
class GateTraverseState(BaseState):
-
STOP_THRESH = get_rosparam("gate/stop_thresh", 0.2)
DRIVE_FWD_THRESH = get_rosparam("gate/drive_fwd_thresh", 0.34) # 20 degrees
APPROACH_DISTANCE = get_rosparam("gate/approach_distance", 2.0)
diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py
index aba60a399..4a3c82279 100755
--- a/src/navigation/navigation.py
+++ b/src/navigation/navigation.py
@@ -7,18 +7,19 @@
import rospy
import smach
import smach_ros
+from navigation.state import DoneState, DoneStateTransitions, OffState, OffStateTransitions
+from smach.log import loginfo
+from smach.log import set_loggers
+from std_msgs.msg import String
+
+from approach_post import ApproachPostState, ApproachPostStateTransitions
from context import Context
from gate import GateTraverseState, GateTraverseStateTransitions
-from approach_post import ApproachPostState, ApproachPostStateTransitions
-from navigation.state import DoneState, DoneStateTransitions, OffState, OffStateTransitions
-from waypoint import WaypointState, WaypointStateTransitions
-from search import SearchState, SearchStateTransitions
-from recovery import RecoveryState, RecoveryStateTransitions
from partial_gate import PartialGateState, PartialGateStateTransitions
from post_backup import PostBackupState, PostBackupTransitions
-from smach.log import set_loggers
-from smach.log import loginfo, logwarn, logerr
-from std_msgs.msg import String
+from recovery import RecoveryState, RecoveryStateTransitions
+from search import SearchState, SearchStateTransitions
+from waypoint import WaypointState, WaypointStateTransitions
class Navigation(threading.Thread):
diff --git a/src/navigation/partial_gate.py b/src/navigation/partial_gate.py
index e76f2e44d..722484a10 100644
--- a/src/navigation/partial_gate.py
+++ b/src/navigation/partial_gate.py
@@ -1,12 +1,13 @@
from dataclasses import dataclass
-import numpy as np
+from typing import Optional
+import numpy as np
+from aenum import Enum, NoAlias
from util import np_utils
-from typing import Optional
+
+from context import Context
from state import BaseState
from trajectory import Trajectory
-from aenum import Enum, NoAlias
-from context import Context
STOP_THRESH = 0.2
DRIVE_FWD_THRESH = 0.95
@@ -64,7 +65,6 @@ class PartialGateStateTransitions(Enum):
class PartialGateState(BaseState):
-
traj: Optional[PartialGateTrajectory] = None
def __init__(
diff --git a/src/navigation/post_backup.py b/src/navigation/post_backup.py
index f56edd01e..66f278b27 100644
--- a/src/navigation/post_backup.py
+++ b/src/navigation/post_backup.py
@@ -1,17 +1,20 @@
from __future__ import annotations
+
from dataclasses import dataclass
-import numpy as np
from typing import Optional
-from state import BaseState
-from trajectory import Trajectory
+
+import numpy as np
+import rospy
+import tf2_ros
from aenum import Enum, NoAlias
-from context import Context
-from util.ros_utils import get_rosparam
-from util.np_utils import perpendicular_2d
from shapely.geometry import Point, LineString
from util.SE3 import SE3
-import tf2_ros
-import rospy
+from util.np_utils import perpendicular_2d
+from util.ros_utils import get_rosparam
+
+from context import Context
+from state import BaseState
+from trajectory import Trajectory
POST_RADIUS = get_rosparam("gate/post_radius", 0.7) * get_rosparam("single_fiducial/post_avoidance_multiplier", 1.42)
BACKUP_DISTANCE = get_rosparam("recovery/recovery_distance", 2.0)
diff --git a/src/navigation/recovery.py b/src/navigation/recovery.py
index 3d4be3b8a..7f0e38d68 100644
--- a/src/navigation/recovery.py
+++ b/src/navigation/recovery.py
@@ -1,15 +1,14 @@
-import smach
-from typing import List
from context import Context
-from aenum import Enum, NoAlias
-from state import BaseState
-from geometry_msgs.msg import Twist
-import rospy
-import numpy as np
from typing import Optional
-from util.np_utils import perpendicular_2d, rotate_2d
+
+import numpy as np
+import rospy
+from aenum import Enum, NoAlias
+from util.np_utils import rotate_2d
from util.ros_utils import get_rosparam
+from context import Context
+from state import BaseState
STOP_THRESH = get_rosparam("recovery/stop_thresh", 0.2)
DRIVE_FWD_THRESH = get_rosparam("recovery/drive_fwd_thresh", 0.34) # 20 degrees
diff --git a/src/navigation/search.py b/src/navigation/search.py
index e6873223a..964380688 100644
--- a/src/navigation/search.py
+++ b/src/navigation/search.py
@@ -1,15 +1,16 @@
from __future__ import annotations
+
+from dataclasses import dataclass
from typing import Optional
import numpy as np
+from aenum import Enum, NoAlias
+from mrover.msg import GPSPointList
+from util.ros_utils import get_rosparam
from context import Context, convert_cartesian_to_gps
-from aenum import Enum, NoAlias
from state import BaseState
-from dataclasses import dataclass
from trajectory import Trajectory
-from mrover.msg import GPSPointList
-from util.ros_utils import get_rosparam
@dataclass
diff --git a/src/navigation/state.py b/src/navigation/state.py
index 23c1e8926..9084ab741 100644
--- a/src/navigation/state.py
+++ b/src/navigation/state.py
@@ -1,11 +1,12 @@
-from abc import ABC
-from typing import List
+from abc import ABC, abstractmethod
+from typing import List, Optional
import smach
-from context import Context
from aenum import Enum, NoAlias
from geometry_msgs.msg import Twist
+from context import Context
+
class BaseState(smach.State, ABC):
"""
@@ -19,9 +20,9 @@ def __init__(
self,
context: Context,
own_transitions: List[str],
- add_outcomes: List[str] = None,
- add_input_keys: List[str] = None,
- add_output_keys: List[str] = None,
+ add_outcomes: Optional[List[str]] = None,
+ add_input_keys: Optional[List[str]] = None,
+ add_output_keys: Optional[List[str]] = None,
):
add_outcomes = add_outcomes or []
add_input_keys = add_input_keys or []
@@ -70,9 +71,10 @@ def reset(self):
"""
pass
+ @abstractmethod
def evaluate(self, ud: smach.UserData) -> str:
"""Override me instead of execute!"""
- pass
+ ...
class DoneStateTransitions(Enum):
diff --git a/src/navigation/trajectory.py b/src/navigation/trajectory.py
index 674a0f582..a44dceb3a 100644
--- a/src/navigation/trajectory.py
+++ b/src/navigation/trajectory.py
@@ -1,4 +1,5 @@
from dataclasses import dataclass, field
+
import numpy as np
diff --git a/src/navigation/waypoint.py b/src/navigation/waypoint.py
index d86cc8029..9300660a7 100644
--- a/src/navigation/waypoint.py
+++ b/src/navigation/waypoint.py
@@ -1,14 +1,13 @@
-from typing import List
+from typing import List, Optional
import numpy as np
-import rospy
-
import tf2_ros
-from context import Context, Environment
from aenum import Enum, NoAlias
-from state import BaseState
from util.ros_utils import get_rosparam
+from context import Context
+from state import BaseState
+
class WaypointStateTransitions(Enum):
_settings_ = NoAlias
@@ -31,9 +30,9 @@ class WaypointState(BaseState):
def __init__(
self,
context: Context,
- add_outcomes: List[str] = None,
- add_input_keys: List[str] = None,
- add_output_keys: List[str] = None,
+ add_outcomes: Optional[List[str]] = None,
+ add_input_keys: Optional[List[str]] = None,
+ add_output_keys: Optional[List[str]] = None,
):
add_outcomes = add_outcomes or []
add_input_keys = add_input_keys or []
diff --git a/src/teleop/jetson/arm_trajectory_server.py b/src/teleop/jetson/arm_trajectory_server.py
index d91dcdce2..94a213ad8 100755
--- a/src/teleop/jetson/arm_trajectory_server.py
+++ b/src/teleop/jetson/arm_trajectory_server.py
@@ -74,17 +74,14 @@ def error_threshold_exceeded(feedback: FollowJointTrajectoryFeedback) -> str:
class MoveItAction(object):
-
# Rearranges the point path following the name convention joint_0, ... joint_6
def rearrange(self, joint_trajectory: JointTrajectory) -> None:
-
mapping = [joint_trajectory.joint_names.index(j) for j in conf_joint_names]
# Return early if already arranged properly
if mapping == sorted(mapping):
return
for point in joint_trajectory.points:
-
temp_positions: List[float] = []
temp_velocities: List[float] = []
temp_accelerations: List[float] = []
@@ -121,7 +118,6 @@ def __init__(self, name: str) -> None:
# Action callback
def execute_cb(self, goal: FollowJointTrajectoryGoal) -> None:
-
rospy.loginfo("Executing FollowJointTrajectory Action")
# It is required to rearrange the arrays because MoveIt doesn't guarantee order preservation
self.rearrange(goal.trajectory)
diff --git a/src/util/boost_cpp23_workaround.hpp b/src/util/boost_cpp23_workaround.hpp
index 3734738b6..3220f2516 100644
--- a/src/util/boost_cpp23_workaround.hpp
+++ b/src/util/boost_cpp23_workaround.hpp
@@ -4,62 +4,62 @@
#include
// Quintin: This is heinous, but it's the only way I can get the whole code to compile with anything greater than C++17
-
#ifdef __GNUC__
-#define GENERATE_ALLOCATOR_SPECIALIZATION(_Tp) \
- namespace std { \
- _GLIBCXX_BEGIN_NAMESPACE_VERSION \
- \
- template<> \
- class allocator<_Tp> : public __allocator_base<_Tp> { \
- public: \
- typedef _Tp value_type; \
- typedef size_t size_type; \
- typedef ptrdiff_t difference_type; \
- \
- typedef _Tp* pointer; \
- typedef const _Tp* const_pointer; \
- typedef _Tp& reference; \
- typedef const _Tp& const_reference; \
- \
- template \
- struct rebind { \
- typedef allocator<_Tp1> other; \
- }; \
- \
- using propagate_on_container_move_assignment = true_type; \
- \
- using is_always_equal \
- _GLIBCXX20_DEPRECATED_SUGGEST("std::allocator_traits::is_always_equal") = true_type; \
- \
- _GLIBCXX20_CONSTEXPR \
- allocator() _GLIBCXX_NOTHROW {} \
- \
- _GLIBCXX20_CONSTEXPR \
- allocator(const allocator& __a) _GLIBCXX_NOTHROW \
- : __allocator_base<_Tp>(__a) {} \
- \
- allocator& operator=(const allocator&) = default; \
- \
- template \
- _GLIBCXX20_CONSTEXPR \
- allocator(const allocator<_Tp1>&) _GLIBCXX_NOTHROW {} \
- \
- ~allocator() _GLIBCXX_NOTHROW { \
- } \
- \
- [[nodiscard, __gnu__::__always_inline__]] _Tp* \
- allocate(size_t __n) { \
- return __allocator_base<_Tp>::allocate(__n, 0); \
- } \
- \
- [[__gnu__::__always_inline__]] void \
- deallocate(_Tp* __p, size_t __n) { \
- __allocator_base<_Tp>::deallocate(__p, __n); \
- } \
- \
- _GLIBCXX_END_NAMESPACE_VERSION \
- }; \
+#define GENERATE_ALLOCATOR_SPECIALIZATION(_Tp) \
+ namespace std { \
+ \
+ template<> \
+ class allocator<_Tp> : public __allocator_base<_Tp> { \
+ public: \
+ typedef _Tp value_type; \
+ typedef size_t size_type; \
+ typedef ptrdiff_t difference_type; \
+ \
+ typedef _Tp* pointer; \
+ typedef const _Tp* const_pointer; \
+ typedef _Tp& reference; \
+ typedef const _Tp& const_reference; \
+ \
+ template \
+ struct rebind { \
+ typedef allocator<_Tp1> other; \
+ }; \
+ \
+ using propagate_on_container_move_assignment = true_type; \
+ \
+ using is_always_equal = true_type; \
+ \
+ __attribute__((__always_inline__)) \
+ _GLIBCXX20_CONSTEXPR \
+ allocator() _GLIBCXX_NOTHROW {} \
+ \
+ __attribute__((__always_inline__)) \
+ _GLIBCXX20_CONSTEXPR \
+ allocator(const allocator& __a) _GLIBCXX_NOTHROW \
+ : __allocator_base<_Tp>(__a) {} \
+ \
+ allocator& operator=(const allocator&) = default; \
+ \
+ template \
+ __attribute__((__always_inline__)) \
+ _GLIBCXX20_CONSTEXPR \
+ allocator(const allocator<_Tp1>&) _GLIBCXX_NOTHROW {} \
+ \
+ __attribute__((__always_inline__)) ~allocator() _GLIBCXX_NOTHROW {} \
+ \
+ [[nodiscard, __gnu__::__always_inline__]] _Tp* \
+ allocate(size_t __n) { \
+ return __allocator_base<_Tp>::allocate(__n, 0); \
+ } \
+ \
+ [[__gnu__::__always_inline__]] void \
+ deallocate(_Tp* __p, size_t __n) { \
+ __allocator_base<_Tp>::deallocate(__p, __n); \
+ } \
+ \
+ friend __attribute__((__always_inline__)) _GLIBCXX20_CONSTEXPR bool \
+ operator==(const allocator&, const allocator&) _GLIBCXX_NOTHROW { return true; } \
+ }; \
}
#include
diff --git a/starter_project/autonomy/AutonomyStarterProject.cmake b/starter_project/autonomy/AutonomyStarterProject.cmake
new file mode 100644
index 000000000..4b84c75af
--- /dev/null
+++ b/starter_project/autonomy/AutonomyStarterProject.cmake
@@ -0,0 +1,31 @@
+## Starter Project
+# CMake is a build scripting system
+# It is a program whose sole purpose is to generate Makefiles (or Ninja build files)
+# It is used extensively in industry
+
+# TODO: add your new message file here under FILES
+add_message_files(
+ DIRECTORY
+ ${CMAKE_CURRENT_LIST_DIR}/msg
+ FILES
+ #add new messages here
+)
+
+# Collect all cpp files in the src subdirectory to be used for perception
+file(GLOB_RECURSE STARTER_PROJECT_PERCEPTION_SOURCES "${CMAKE_CURRENT_LIST_DIR}/src/*.cpp")
+# Define a new CMake target, specifically a built C++ executable, that uses the found source files
+add_executable(starter_project_perception ${STARTER_PROJECT_PERCEPTION_SOURCES})
+# Ensure that our project builds after message generation
+add_dependencies(starter_project_perception ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+# Link needed libraries
+target_link_libraries(starter_project_perception ${catkin_LIBRARIES} ${OpenCV_LIBS})
+# Include needed directories
+target_include_directories(starter_project_perception PUBLIC ${catkin_INCLUDE_DIRS})
+
+# Install our executable so that ROS knows about it
+# This allows us to launch it with rosrun
+install(TARGETS starter_project_perception
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/starter_project/autonomy/config/soil_base.png b/starter_project/autonomy/config/soil_base.png
new file mode 120000
index 000000000..02756e2cc
--- /dev/null
+++ b/starter_project/autonomy/config/soil_base.png
@@ -0,0 +1 @@
+../../../config/gazebo/env_description/soil_base.png
\ No newline at end of file
diff --git a/starter_project/autonomy/config/soil_normal.png b/starter_project/autonomy/config/soil_normal.png
new file mode 120000
index 000000000..3399db4f9
--- /dev/null
+++ b/starter_project/autonomy/config/soil_normal.png
@@ -0,0 +1 @@
+../../../config/gazebo/env_description/soil_normal.png
\ No newline at end of file
diff --git a/starter_project/autonomy/config/starter_project.rviz b/starter_project/autonomy/config/starter_project.rviz
new file mode 100644
index 000000000..31d1b36ae
--- /dev/null
+++ b/starter_project/autonomy/config/starter_project.rviz
@@ -0,0 +1,316 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /RobotModel1
+ - /Image1
+ Splitter Ratio: 0.5
+ Tree Height: 796
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Name: Time
+ SyncMode: 0
+ SyncSource: Image
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ back_left_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ back_right_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ center_left_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ center_right_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_left_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_right_wheel_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ imu_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ back_left_wheel_link:
+ Value: true
+ back_right_wheel_link:
+ Value: true
+ base_link:
+ Value: true
+ camera_link:
+ Value: true
+ center_left_wheel_link:
+ Value: true
+ center_right_wheel_link:
+ Value: true
+ front_left_wheel_link:
+ Value: true
+ front_right_wheel_link:
+ Value: true
+ imu_link:
+ Value: true
+ Marker Alpha: 1
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ base_link:
+ back_left_wheel_link:
+ {}
+ back_right_wheel_link:
+ {}
+ camera_link:
+ {}
+ center_left_wheel_link:
+ {}
+ center_right_wheel_link:
+ {}
+ front_left_wheel_link:
+ {}
+ front_right_wheel_link:
+ {}
+ imu_link:
+ {}
+ Update Interval: 0
+ Value: true
+ - Alpha: 1
+ Class: rviz/Axes
+ Enabled: false
+ Length: 1
+ Name: Axes
+ Radius: 0.10000000149011612
+ Reference Frame:
+ Show Trail: false
+ Value: false
+ - Class: rviz/Image
+ Enabled: true
+ Image Topic: /camera/right/image
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Image
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: compressed
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: RGB8
+ Decay Time: 0
+ Enabled: false
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Flat Squares
+ Topic: /camera/depth/points
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: false
+ - Angle Tolerance: 0.10000000149011612
+ Class: rviz/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Keep: 100
+ Name: Odometry
+ Position Tolerance: 0.10000000149011612
+ Queue Size: 10
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 255; 25; 0
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic: /odometry/filtered
+ Unreliable: false
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: base_link
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 4.978219032287598
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7853981852531433
+ Focal Point:
+ X: 0.5774051547050476
+ Y: 0.20844532549381256
+ Z: 0.04878166690468788
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.40020281076431274
+ Target Frame:
+ Yaw: 5.461760997772217
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1380
+ Hide Left Dock: false
+ Hide Right Dock: true
+ Image:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd000000040000000000000156000004cafc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a5000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000003e60000011f0000001600ffffff00000001000001000000035dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035d000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000d2b0000003efc0100000002fb0000000800540069006d0065010000000000000d2b0000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000bcf000004ca00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 3371
+ X: 69
+ Y: 28
diff --git a/starter_project/autonomy/config/starter_project.world b/starter_project/autonomy/config/starter_project.world
new file mode 100644
index 000000000..a8235fb42
--- /dev/null
+++ b/starter_project/autonomy/config/starter_project.world
@@ -0,0 +1,131 @@
+
+
+
+ 1
+ 0 0 10 0 -0 0
+ 0.8 0.8 0.8 1
+ 0.2 0.2 0.2 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+ 0
+ 0
+ 0
+
+
+ 0 0 -9.8
+ 6e-06 2.3e-05 -4.2e-05
+
+
+ 0.001
+ 1
+ 1000
+
+
+ 0.7 0.7 0.7 1
+ 0.529 0.808 0.922 1
+ 1
+
+
+
+ EARTH_WGS84
+ 0
+ 0
+ 0
+ 0
+
+
+ 5.5 2.0 0.5 0 0 4.71239
+ 1
+
+
+
+
+ model://post/meshes/4x4_1000-0.dae
+
+
+ 10
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ model://post/meshes/4x4_1000-0.dae
+
+
+
+ 0
+ 0
+ 0
+
+
+
+ 0 0 -0.1 0 0 0
+ 1
+
+
+
+
+ model://starter_project_ground.dae
+
+
+ 10
+
+
+ 65535
+
+
+
+
+ 100
+ 50
+
+
+
+
+
+
+
+
+
+
+
+ model://starter_project_ground.dae
+
+
+
+
+ model://soil_normal.png
+
+
+
+ 0
+ 0
+ 0
+
+
+
+
+ 6.01711 -3.68905 2.02916 0 0.275643 2.35619
+ orbit
+ perspective
+
+
+
+
\ No newline at end of file
diff --git a/starter_project/autonomy/config/starter_project_ground.blend b/starter_project/autonomy/config/starter_project_ground.blend
new file mode 100644
index 000000000..dad3e614c
--- /dev/null
+++ b/starter_project/autonomy/config/starter_project_ground.blend
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:dc79a8b52a191b1ebdcf462e986cc787f7467db36aa0a381f4fd664814744fce
+size 906528
diff --git a/starter_project/autonomy/config/starter_project_ground.dae b/starter_project/autonomy/config/starter_project_ground.dae
new file mode 100644
index 000000000..53c9fe8b7
--- /dev/null
+++ b/starter_project/autonomy/config/starter_project_ground.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:079221b516b0918a668fa6e3ac68357b404e5dc28d1fb0b85576584a721e9ab9
+size 71515
diff --git a/starter_project/autonomy/format.sh b/starter_project/autonomy/format.sh
new file mode 100755
index 000000000..bc7e569ae
--- /dev/null
+++ b/starter_project/autonomy/format.sh
@@ -0,0 +1,44 @@
+#!/usr/bin/env bash
+
+# See: https://vaneyckt.io/posts/safer_bash_scripts_with_set_euxo_pipefail/
+set -Eeuo pipefail
+
+readonly RED='\033[0;31m'
+readonly NC='\033[0m'
+
+BLACK_ARGS=(
+ "--line-length=120"
+ "--color"
+)
+
+# Just do a dry run if the "fix" argument is not passed
+if [ $# -eq 0 ] || [ "$1" != "--fix" ]; then
+ BLACK_ARGS+=("--diff") # Show difference
+ BLACK_ARGS+=("--check") # Exit with non-zero code if changes are required (for CI)
+fi
+
+function print_update_error() {
+ echo -e "${RED}[Error] Please update with ./ansible.sh build.yml${NC}"
+ exit 1
+}
+
+function find_executable() {
+ local readonly executable="$1"
+ local readonly version="$2"
+ local readonly path=$(which "${executable}")
+ if [ ! -x "${path}" ]; then
+ echo -e "${RED}[Error] Could not find ${executable}${NC}"
+ print_update_error
+ fi
+ if ! "${path}" --version | grep -q "${version}"; then
+ echo -e "${RED}[Error] Wrong ${executable} version${NC}"
+ print_update_error
+ fi
+ echo "${path}"
+}
+
+readonly BLACK_PATH=$(find_executable black 23.9.1)
+
+echo
+echo "Style checking Python with black ..."
+"${BLACK_PATH}" "${BLACK_ARGS[@]}" ./src
diff --git a/starter_project/autonomy/launch/starter_project.launch b/starter_project/autonomy/launch/starter_project.launch
new file mode 100644
index 000000000..a0b8ce28c
--- /dev/null
+++ b/starter_project/autonomy/launch/starter_project.launch
@@ -0,0 +1,49 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+starter_project/
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/starter_project/autonomy/lint.sh b/starter_project/autonomy/lint.sh
new file mode 100755
index 000000000..1b45973b5
--- /dev/null
+++ b/starter_project/autonomy/lint.sh
@@ -0,0 +1,33 @@
+#!/usr/bin/env bash
+
+# See: https://vaneyckt.io/posts/safer_bash_scripts_with_set_euxo_pipefail/
+set -Eeuo pipefail
+
+readonly RED='\033[0;31m'
+readonly NC='\033[0m'
+
+function print_update_error() {
+ echo -e "${RED}[Error] Please update with ./ansible.sh build.yml${NC}"
+ exit 1
+}
+
+function find_executable() {
+ local readonly executable="$1"
+ local readonly version="$2"
+ local readonly path=$(which "${executable}")
+ if [ ! -x "${path}" ]; then
+ echo -e "${RED}[Error] Could not find ${executable}${NC}"
+ print_update_error
+ fi
+ if ! "${path}" --version | grep -q "${version}"; then
+ echo -e "${RED}[Error] Wrong ${executable} version${NC}"
+ print_update_error
+ fi
+ echo "${path}"
+}
+
+readonly MYPY_PATH=$(find_executable mypy 1.5.1)
+
+echo
+echo "Style checking Python with mypy ..."
+"${MYPY_PATH}" --config-file=mypy.ini --check ./src
diff --git a/starter_project/autonomy/msg/.keep b/starter_project/autonomy/msg/.keep
new file mode 100644
index 000000000..29321fad2
--- /dev/null
+++ b/starter_project/autonomy/msg/.keep
@@ -0,0 +1,2 @@
+# The point of this file is to have the msg folder in the repository
+# Empty folders are not supported by Git
\ No newline at end of file
diff --git a/starter_project/autonomy/mypy.ini b/starter_project/autonomy/mypy.ini
new file mode 120000
index 000000000..141a30f41
--- /dev/null
+++ b/starter_project/autonomy/mypy.ini
@@ -0,0 +1 @@
+../../mypy.ini
\ No newline at end of file
diff --git a/starter_project/autonomy/src/localization.py b/starter_project/autonomy/src/localization.py
new file mode 100755
index 000000000..b8ff5423a
--- /dev/null
+++ b/starter_project/autonomy/src/localization.py
@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+
+# python linear algebra library
+import numpy as np
+
+# library for interacting with ROS and TF tree
+import rospy
+import tf2_ros
+
+# ROS message types we need to use
+from sensor_msgs.msg import NavSatFix, Imu
+
+# SE3 library for handling poses and TF tree
+from util.SE3 import SE3
+
+
+class Localization:
+ pose: SE3
+
+ def __init__(self):
+ # create subscribers for GPS and IMU data, linking them to our callback functions
+ # TODO
+
+ # create a transform broadcaster for publishing to the TF tree
+ self.tf_broadcaster = tf2_ros.TransformBroadcaster()
+
+ # initialize pose to all zeros
+ self.pose = SE3()
+
+ def gps_callback(self, msg: NavSatFix):
+ """
+ This function will be called every time this node receives a NavSatFix message
+ on the /gps topic. It should read the GPS location from the given NavSatFix message,
+ convert it to cartesian coordinates, store that value in `self.pose`, then publish
+ that pose to the TF tree.
+ """
+ # TODO
+
+ def imu_callback(self, msg: Imu):
+ """
+ This function will be called every time this node receives an Imu message
+ on the /imu topic. It should read the orientation data from the given Imu message,
+ store that value in `self.pose`, then publish that pose to the TF tree.
+ """
+ # TODO
+
+ @staticmethod
+ def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndarray) -> np.ndarray:
+ """
+ This is a utility function that should convert spherical (latitude, longitude)
+ coordinates into cartesian (x, y, z) coordinates using the specified reference point
+ as the center of the tangent plane used for approximation.
+ :param spherical_coord: the spherical coordinate to convert,
+ given as a numpy array [latitude, longitude]
+ :param reference_coord: the reference coordinate to use for conversion,
+ given as a numpy array [latitude, longitude]
+ :returns: the approximated cartesian coordinates in meters, given as a numpy array [x, y, z]
+ """
+ # TODO
+
+
+def main():
+ # initialize the node
+ rospy.init_node("localization")
+
+ # create and start our localization system
+ localization = Localization()
+
+ # let the callback functions run asynchronously in the background
+ rospy.spin()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/starter_project/autonomy/src/navigation/context.py b/starter_project/autonomy/src/navigation/context.py
new file mode 100644
index 000000000..4bbd06f61
--- /dev/null
+++ b/starter_project/autonomy/src/navigation/context.py
@@ -0,0 +1,73 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Optional
+
+import numpy as np
+import rospy
+import tf2_ros
+from geometry_msgs.msg import Twist
+from mrover.msg import StarterProjectTag
+from util.SE3 import SE3
+from visualization_msgs.msg import Marker
+
+
+@dataclass
+class Rover:
+ ctx: Context
+
+ def get_pose(self) -> Optional[SE3]:
+ # TODO: return the pose of the rover (or None if we don't have one (catch exception))
+ pass
+
+ def send_drive_command(self, twist: Twist):
+ # TODO: send the Twist message to the rover
+ pass
+
+ def send_drive_stop(self):
+ # TODO: tell the rover to stop
+ pass
+
+
+@dataclass
+class Environment:
+ """
+ Context class to represent the rover's environment
+ Information such as locations of fiducials or obstacles
+ """
+
+ ctx: Context
+ fid_pos: Optional[StarterProjectTag]
+
+ def receive_fid_data(self, message: StarterProjectTag):
+ # TODO: handle incoming FID data messages here
+ pass
+
+ def get_fid_data(self) -> Optional[StarterProjectTag]:
+ """
+ Retrieves the last received message regarding fid pose
+ if it exists, otherwise returns None
+ """
+ # TODO: return either None or your position message
+ pass
+
+
+class Context:
+ tf_buffer: tf2_ros.Buffer
+ tf_listener: tf2_ros.TransformListener
+ vel_cmd_publisher: rospy.Publisher
+ vis_publisher: rospy.Publisher
+ fid_listener: rospy.Subscriber
+
+ # Use these as the primary interfaces in states
+ rover: Rover
+ env: Environment
+
+ def __init__(self):
+ self.tf_buffer = tf2_ros.Buffer()
+ self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
+ self.vel_cmd_publisher = rospy.Publisher("cmd_vel", Twist, queue_size=1)
+ self.vis_publisher = rospy.Publisher("nav_vis", Marker)
+ self.rover = Rover(self)
+ self.env = Environment(self, None)
+ self.fid_listener = rospy.Subscriber("/tag", StarterProjectTag, self.env.receive_fid_data)
diff --git a/starter_project/autonomy/src/navigation/drive.py b/starter_project/autonomy/src/navigation/drive.py
new file mode 100644
index 000000000..0bf724678
--- /dev/null
+++ b/starter_project/autonomy/src/navigation/drive.py
@@ -0,0 +1,56 @@
+from typing import Tuple
+
+import numpy as np
+from geometry_msgs.msg import Twist
+from util.SE3 import SE3
+
+MAX_DRIVING_EFFORT = 1
+MIN_DRIVING_EFFORT = -1
+TURNING_P = 100.0
+
+
+def get_drive_command(
+ target_pos: np.ndarray,
+ rover_pose: SE3,
+ completion_thresh: float,
+ turn_in_place_thresh: float,
+) -> Tuple[Twist, bool]:
+ """
+ :param target_pos: Target position to drive to.
+ :param rover_pose: Current rover pose.
+ :param completion_thresh: If the distance to the target is less than this stop.
+ :param turn_in_place_thresh Minimum cosine of the angle in between the target and current heading
+ in order to drive forward. When below, turn in place.
+ :return: Rover drive effort command.
+ """
+ if not (0.0 < turn_in_place_thresh < 1.0):
+ raise ValueError(f"Argument {turn_in_place_thresh} should be between 0 and 1")
+ rover_pos = rover_pose.position
+ rover_dir = rover_pose.rotation.direction_vector()
+ # Get vector from rover to target
+ target_dir = target_pos - rover_pos
+ target_dist = np.linalg.norm(target_dir)
+ if target_dist == 0:
+ target_dist = np.finfo(float).eps
+ # Normalize direction
+ target_dir /= target_dist
+ # Both vectors are unit vectors so the dot product magnitude is 0-1
+ # 0 alignment is perpendicular, 1 is parallel (fully aligned)
+ alignment = np.dot(target_dir, rover_dir)
+
+ if target_dist < completion_thresh:
+ return Twist(), True
+
+ cmd_vel = Twist()
+ if alignment > turn_in_place_thresh:
+ # We are pretty aligned so we can drive straight
+ error = target_dist
+ cmd_vel.linear.x = np.clip(error, 0.0, MAX_DRIVING_EFFORT)
+ # Determine the sign of our effort by seeing if we are to the left or to the right of the target
+ # This is done by dotting rover_dir and target_dir rotated 90 degrees ccw
+ perp_alignment = target_dir[0] * -rover_dir[1] + target_dir[1] * rover_dir[0]
+ sign = np.sign(perp_alignment)
+ # 1 is target alignment (dot product of two normalized vectors that are parallel is 1)
+ error = 1.0 - alignment
+ cmd_vel.angular.z = np.clip(error * TURNING_P * sign, MIN_DRIVING_EFFORT, MAX_DRIVING_EFFORT)
+ return cmd_vel, False
diff --git a/starter_project/autonomy/src/navigation/drive_state.py b/starter_project/autonomy/src/navigation/drive_state.py
new file mode 100644
index 000000000..bae1d07f1
--- /dev/null
+++ b/starter_project/autonomy/src/navigation/drive_state.py
@@ -0,0 +1,30 @@
+import numpy as np
+
+from context import Context
+from drive import get_drive_command
+from state import BaseState
+
+
+class DriveState(BaseState):
+ def __init__(self, context: Context):
+ super().__init__(
+ context,
+ # TODO:
+ add_outcomes=["TODO: add outcomes here"],
+ )
+
+ def evaluate(self, ud):
+ target = np.array([5.5, 2.0, 0.0])
+
+ # TODO: get the rover's pose, if it doesn't exist stay in DriveState (with outcome "driving_to_point")
+
+ # TODO: get the drive command and completion status based on target and pose
+ # (HINT: use get_drive_command(), with completion_thresh set to 0.7 and turn_in_place_thresh set to 0.2)
+
+ # TODO: if we are finished getting to the target, go to TagSeekState (with outcome "reached_point")
+
+ # TODO: send the drive command to the rover
+
+ # TODO: tell smach to stay in the DriveState by returning with outcome "driving_to_point"
+
+ pass
diff --git a/starter_project/autonomy/src/navigation/navigation_starter_project.py b/starter_project/autonomy/src/navigation/navigation_starter_project.py
new file mode 100755
index 000000000..a3f00f0dd
--- /dev/null
+++ b/starter_project/autonomy/src/navigation/navigation_starter_project.py
@@ -0,0 +1,75 @@
+#!/usr/bin/env python3
+
+import signal
+import sys
+import threading
+
+# ros and state machine imports
+import rospy
+import smach
+import smach_ros
+
+# navigation specific imports
+from context import Context
+from drive_state import DriveState
+from state import DoneState
+from tag_seek import TagSeekState
+
+
+class Navigation(threading.Thread):
+ state_machine: smach.StateMachine
+ context: Context
+ sis: smach_ros.IntrospectionServer
+
+ def __init__(self, context: Context):
+ super().__init__()
+ self.name = "NavigationThread"
+ self.state_machine = smach.StateMachine(outcomes=["terminated"])
+ self.context = context
+ self.sis = smach_ros.IntrospectionServer("", self.state_machine, "/SM_ROOT")
+ self.sis.start()
+ with self.state_machine:
+ # TODO: add DriveState and its transitions here
+
+ # DoneState and its transitions
+ self.state_machine.add(
+ "DoneState",
+ DoneState(self.context),
+ transitions={"done": "DoneState"},
+ )
+ # TODO: add TagSeekState and its transitions here
+
+ def run(self):
+ self.state_machine.execute()
+
+ def stop(self):
+ self.sis.stop()
+ # Requests current state to go into 'terminated' to cleanly exit state machine
+ self.state_machine.request_preempt()
+ # Wait for smach thread to terminate
+ self.join()
+ self.context.rover.send_drive_stop()
+
+
+def main():
+ # TODO: init a node called "navigation"
+
+ # context and navigation objects
+ context = Context()
+ navigation = Navigation(context)
+
+ # Define custom handler for Ctrl-C that shuts down smach properly
+ def sigint_handler(_sig, _frame):
+ navigation.stop()
+ rospy.signal_shutdown("keyboard interrupt")
+ try:
+ sys.exit(0)
+ except SystemExit:
+ pass
+
+ signal.signal(signal.SIGINT, sigint_handler)
+ navigation.start()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/starter_project/autonomy/src/navigation/state.py b/starter_project/autonomy/src/navigation/state.py
new file mode 100644
index 000000000..c1a8e623c
--- /dev/null
+++ b/starter_project/autonomy/src/navigation/state.py
@@ -0,0 +1,63 @@
+from abc import ABC, abstractmethod
+from typing import List, Optional
+
+import smach
+from geometry_msgs.msg import Twist
+
+from context import Context
+
+
+class BaseState(smach.State, ABC):
+ """
+ Custom base state which handles termination cleanly via smach preemption.
+ """
+
+ context: Context
+
+ def __init__(
+ self,
+ context: Context,
+ add_outcomes: Optional[List[str]] = None,
+ add_input_keys: Optional[List[str]] = None,
+ add_output_keys: Optional[List[str]] = None,
+ ):
+ add_outcomes = add_outcomes or []
+ add_input_keys = add_input_keys or []
+ add_output_keys = add_output_keys or []
+ super().__init__(
+ add_outcomes + ["terminated"],
+ add_input_keys,
+ add_output_keys,
+ )
+ self.context = context
+
+ def execute(self, ud):
+ """
+ Override execute method to add logic for early termination.
+ Base classes should override evaluate instead of this!
+ :param ud: State machine user data
+ :return: Next state, 'terminated' if we want to quit early
+ """
+ if self.preempt_requested():
+ self.service_preempt()
+ return "terminated"
+ return self.evaluate(ud)
+
+ @abstractmethod
+ def evaluate(self, ud: smach.UserData) -> str:
+ """Override me instead of execute!"""
+ ...
+
+
+class DoneState(BaseState):
+ def __init__(self, context: Context):
+ super().__init__(
+ context,
+ add_outcomes=["done"],
+ )
+
+ def evaluate(self, ud):
+ # Stop rover
+ cmd_vel = Twist()
+ self.context.rover.send_drive_command(cmd_vel)
+ return "done"
diff --git a/starter_project/autonomy/src/navigation/tag_seek.py b/starter_project/autonomy/src/navigation/tag_seek.py
new file mode 100644
index 000000000..577731516
--- /dev/null
+++ b/starter_project/autonomy/src/navigation/tag_seek.py
@@ -0,0 +1,30 @@
+from geometry_msgs.msg import Twist
+
+from context import Context
+from state import BaseState
+
+
+class TagSeekState(BaseState):
+ def __init__(self, context: Context):
+ super().__init__(
+ context,
+ # TODO: add outcomes
+ add_outcomes=["TODO: add outcomes here"],
+ )
+
+ def evaluate(self, ud):
+ DISTANCE_TOLERANCE = 0.99
+ ANUGLAR_TOLERANCE = 0.3
+ # TODO: get the tag's location and properties (HINT: use get_fid_data() from context.env)
+
+ # TODO: if we don't have a tag: go to the DoneState (with outcome "failure")
+
+ # TODO: if we are within angular and distance tolerances: go to DoneState (with outcome "success")
+
+ # TODO: figure out the Twist command to be applied to move the rover closer to the tag
+
+ # TODO: send Twist command to rover
+
+ # TODO: stay in the TagSeekState (with outcome "working")
+
+ pass
diff --git a/starter_project/autonomy/src/perception.cpp b/starter_project/autonomy/src/perception.cpp
new file mode 100644
index 000000000..2185a5fe2
--- /dev/null
+++ b/starter_project/autonomy/src/perception.cpp
@@ -0,0 +1,82 @@
+#include "perception.hpp"
+
+// ROS Headers, ros namespace
+#include
+
+int main(int argc, char** argv) {
+ ros::init(argc, argv, "starter_project_perception"); // Our node name (See: http://wiki.ros.org/Nodes)
+
+ [[maybe_unused]] mrover::Perception perception;
+
+ // "spin" blocks until our node dies
+ // It listens for new messages and calls our subscribed functions with them
+ ros::spin();
+
+ return EXIT_SUCCESS;
+}
+
+namespace mrover {
+
+ Perception::Perception() : mNodeHandle{}, mImageTransport{mNodeHandle} {
+ // Subscribe to camera image messages
+ // Every time another node publishes to this topic we will be notified
+ // Specifically the callback we passed will be invoked
+ mImageSubscriber = mImageTransport.subscribe("camera/right/image", 1, &Perception::imageCallback, this);
+
+ // Create a publisher for our tag topic
+ // See: http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29
+ // TODO: uncomment me!
+ // mTagPublisher = mNodeHandle.advertise("tag", 1);
+
+ mTagDetectorParams = cv::aruco::DetectorParameters::create();
+ mTagDictionary = cv::aruco::getPredefinedDictionary(0);
+ }
+
+ void Perception::imageCallback(sensor_msgs::ImageConstPtr const& image) {
+ // Create a cv::Mat from the ROS image message
+ // Note this does not copy the image data, it is basically a pointer
+ // Be careful if you extend its lifetime beyond this function
+ cv::Mat cvImage{static_cast(image->height), static_cast(image->width),
+ CV_8UC3, const_cast(image->data.data())};
+ // Detect tags in the image pixels
+ findTagsInImage(cvImage, mTags);
+ // Select the tag that is closest to the middle of the screen
+ StarterProjectTag tag = selectTag(mTags);
+ // Publish the message to our topic so navigation or others can receive it
+ publishTag(tag);
+ }
+
+ void Perception::findTagsInImage(cv::Mat const& image, std::vector& tags) { // NOLINT(*-convert-member-functions-to-static)
+ // hint: take a look at OpenCV's documentation for the detectMarkers function
+ // hint: you have mTagDictionary, mTagCorners, mTagIds, and mTagDetectorParams member variables already defined!
+ // hint: write and use the "getCenterFromTagCorners" and "getClosenessMetricFromTagCorners" functions
+
+ tags.clear(); // Clear old tags in output vector
+
+ // TODO: implement me!
+ }
+
+ StarterProjectTag Perception::selectTag(std::vector const& tags) { // NOLINT(*-convert-member-functions-to-static)
+ // TODO: implement me!
+ return {};
+ }
+
+ void Perception::publishTag(StarterProjectTag const& tag) {
+ // TODO: implement me!
+ }
+
+ float Perception::getClosenessMetricFromTagCorners(cv::Mat const& image, std::vector const& tagCorners) { // NOLINT(*-convert-member-functions-to-static)
+ // hint: think about how you can use the "image" parameter
+ // hint: this is an approximation that will be used later by navigation to stop "close enough" to a tag.
+ // hint: try not overthink, this metric does not have to be perfectly accurate, just correlated to distance away from a tag
+
+ // TODO: implement me!
+ return {};
+ }
+
+ std::pair Perception::getCenterFromTagCorners(std::vector const& tagCorners) { // NOLINT(*-convert-member-functions-to-static)
+ // TODO: implement me!
+ return {};
+ }
+
+} // namespace mrover
diff --git a/starter_project/autonomy/src/perception.hpp b/starter_project/autonomy/src/perception.hpp
new file mode 100644
index 000000000..d690e2486
--- /dev/null
+++ b/starter_project/autonomy/src/perception.hpp
@@ -0,0 +1,100 @@
+#pragma once
+
+// C++ Standard Library Headers, std namespace
+#include
+#include
+#include
+#include
+#include
+
+// OpenCV Headers, cv namespace
+#include
+#include
+
+// ROS Headers, ros namespace
+#include
+#include
+#include
+#include
+#include
+
+#if __has_include()
+#include
+#else
+struct StarterProjectTag {};
+#endif
+
+namespace mrover {
+
+ /**
+ * Starter project perception node
+ *
+ * Input: Image data, just RGB pixels.
+ * Output: ArUco tag pixel coordinates that is closest to the center of the camera.
+ * Also an approximation for how far away the tag is.
+ */
+ class Perception {
+ private:
+ ros::NodeHandle mNodeHandle;
+ image_transport::ImageTransport mImageTransport;
+ image_transport::Subscriber mImageSubscriber;
+ cv::Ptr mTagDetectorParams;
+ cv::Ptr mTagDictionary;
+ std::vector> mTagCorners;
+ std::vector mTagIds;
+ std::vector mTags;
+ ros::Publisher mTagPublisher;
+
+ public:
+ Perception();
+
+ /**
+ * Called when we receive a new image message from the camera.
+ * Specifically this is one frame.
+ *
+ * @param image
+ */
+ void imageCallback(sensor_msgs::ImageConstPtr const& image);
+
+ /**
+ * Given an image, detect ArUco tags, and fill a vector full of output messages.
+ *
+ * @param image Image
+ * @param tags Output vector of tags
+ */
+ void findTagsInImage(cv::Mat const& image, std::vector& tags);
+
+ /**
+ * Publish our processed tag
+ *
+ * @param tag Selected tag message
+ */
+ void publishTag(StarterProjectTag const& tag);
+
+ /**
+ * Given an ArUco tag in pixel space, find a metric for how close we are.
+ *
+ * @param image Access to the raw OpenCV image as a matrix
+ * @param tagCorners 4-tuple of the tag pixel coordinates representing the corners
+ * @return Closeness metric from rover to the tag
+ */
+ [[nodiscard]] float getClosenessMetricFromTagCorners(cv::Mat const& image, std::vector const& tagCorners);
+
+ /**
+ * Given an ArUco tag in pixel space, find the approximate center in pixel space
+ *
+ * @param tagCorners 4-tuple of tag pixel coordinates representing the corners
+ * @return 2-tuple (x,y) approximate center in pixel space
+ */
+ [[nodiscard]] std::pair getCenterFromTagCorners(std::vector const& tagCorners);
+
+ /**
+ * Select the tag closest to the center of the camera
+ *
+ * @param tags Vector of tags
+ * @return Center tag
+ */
+ [[nodiscard]] StarterProjectTag selectTag(std::vector const& tags);
+ };
+
+} // namespace mrover
diff --git a/starter_project/autonomy/src/util/SE3.py b/starter_project/autonomy/src/util/SE3.py
new file mode 100644
index 000000000..824a16bb0
--- /dev/null
+++ b/starter_project/autonomy/src/util/SE3.py
@@ -0,0 +1,109 @@
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+import numpy as np
+import rospy
+import tf2_ros
+from geometry_msgs.msg import TransformStamped, Vector3, Quaternion
+
+from .SO3 import SO3
+from .np_utils import numpify
+
+
+@dataclass(frozen=True)
+class SE3:
+ """
+ An SE3 object represents a pose in 3 dimensions,
+ AKA a member of the Special Euclidean group in 3 dimensions (SE3).
+ This consists of a 3D position and a 3D rotation (represented as an SO3).
+
+ NOTE: when passing an already existing numpy array to the constructor as the `position` argument,
+ make sure to call `.copy()` on it in order to avoid transferring ownership of the array.
+
+ For example:
+ >>> arr = np.array([1, 2, 3])
+ >>> p = SE3(position=arr.copy())
+
+ """
+
+ position: np.ndarray = field(default_factory=lambda: np.zeros(3))
+ rotation: SO3 = SO3()
+
+ @classmethod
+ def from_pos_quat(cls, position: np.ndarray, quaternion: np.ndarray):
+ """
+ Initialize an SE3 object using a position vector to specify position
+ and a quaternion vector to specify rotation.
+
+ :param position: position vector [x, y, z], defaults to zero vector
+ :param quaternion: quaternion vector [x, y, z, w], defaults to [0, 0, 0, 1]
+ """
+ return SE3(position, SO3(quaternion))
+
+ @classmethod
+ def from_tf_tree(cls, tf_buffer: tf2_ros.Buffer, parent_frame: str, child_frame: str) -> SE3:
+ """
+ Ask the TF tree for a transform from parent_frame to child_frame,
+ and return it as an SE3.
+
+ :param tf_buffer: the tf buffer used to query the TF tree
+ :param parent_frame: the parent frame of the desired transform
+ :param child_frame: the child frame of the desired transform
+
+ :raises tf2_ros.LookupException: if one or both of the requested frames don't exist in the TF tree
+ :raises tf2_ros.ConnectivityException: if no connection can be found between the two requested frames
+ :raises tf2_ros.ExtrapolationException: if the transform would've required extrapolation
+ (forward or backward in time) beyond current limits
+
+ :returns: an SE3 containing the transform from parent_frame to child_frame
+ """
+ tf_msg = tf_buffer.lookup_transform(parent_frame, child_frame, rospy.Time()).transform
+ result = SE3(position=numpify(tf_msg.translation), rotation=SO3(numpify(tf_msg.rotation)))
+ return result
+
+ def publish_to_tf_tree(
+ self,
+ tf_broadcaster: tf2_ros.TransformBroadcaster | tf2_ros.StaticTransformBroadcaster,
+ parent_frame: str,
+ child_frame: str,
+ ):
+ """
+ Publish the SE3 to the TF tree as a transform from parent_frame to child_frame.
+ Transform can be published as either a regular transform, which will expire after a short delay,
+ or a static transform, which will not expire. This will be decided by the type of
+ transform broadcaster passed to the function.
+
+ :param tf_broadcaster: the TF broadcaster used to publish to the TF tree
+ :param parent_frame: the parent frame of the transform to be published
+ :param child_frame: the child frame of the transform to be published
+ """
+ tf = TransformStamped()
+ tf.transform.translation = Vector3(*self.position)
+ tf.transform.rotation = Quaternion(*self.rotation.quaternion)
+ tf.header.stamp = rospy.Time.now()
+ tf.header.frame_id = parent_frame
+ tf.child_frame_id = child_frame
+ tf_broadcaster.sendTransform(tf)
+
+ def pos_distance_to(self, p: SE3) -> float:
+ """
+ Get the euclidean distance from the position of this SE3 to the position of another SE3
+
+ :param p: another SE3
+ :returns: euclidean distance between the two SE3s
+ """
+ return np.linalg.norm(p.position - self.position) # type: ignore
+
+ def is_approx(self, p: SE3, tolerance=1e-8) -> bool:
+ """
+ Check if two SE3s are approximately equal within a tolerance by checking that each
+ position vector is approximately equal and that each rotation is approximately equal.
+
+ :param p: another SE3
+ :param tolerance: the tolerance for comparing each number, if the difference
+ between each number is less than or equal to this tolerance,
+ they will be considered equal
+ :returns: True if the two SE3s are approximately equal, False otherwise
+ """
+ return np.allclose(self.position, p.position, atol=tolerance) and self.rotation.is_approx(p.rotation, tolerance)
diff --git a/starter_project/autonomy/src/util/SO3.py b/starter_project/autonomy/src/util/SO3.py
new file mode 100644
index 000000000..7eca45ade
--- /dev/null
+++ b/starter_project/autonomy/src/util/SO3.py
@@ -0,0 +1,86 @@
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+import numpy as np
+from tf.transformations import (
+ quaternion_inverse,
+ quaternion_matrix,
+ quaternion_from_matrix,
+ quaternion_multiply,
+)
+
+
+@dataclass(frozen=True)
+class SO3:
+ """
+ An SO3 object represents a rotation in 3 dimensions,
+ AKA a member of the Special Orthogonal group in 3 dimensions (SO3).
+
+ NOTE: when passing an already existing numpy array to the constructor as the `quaternion` argument,
+ make sure to call `.copy()` on it in order to avoid transferring ownership of the array.
+
+ For example:
+ >>> arr = np.array([1, 2, 3, 4])
+ >>> r = SO3(quaternion=arr.copy())
+
+ """
+
+ quaternion: np.ndarray = field(default_factory=lambda: np.array([0, 0, 0, 1]))
+
+ @classmethod
+ def from_matrix(cls, rotation_matrix: np.ndarray) -> SO3:
+ """
+ Create an SO3 object from a rotation matrix.
+
+ :param rotation_matrix: the 3x3 rotation matrix
+ :returns: the created SO3 object
+ """
+ homogenous = np.eye(4)
+ homogenous[:3, :3] = rotation_matrix
+ return SO3(quaternion_from_matrix(homogenous))
+
+ def rotation_matrix(self) -> np.ndarray:
+ """
+ Get the rotation matrix representation of the SO3.
+
+ :returns: a 3x3 rotation matrix
+ """
+ return quaternion_matrix(self.quaternion)[:3, :3]
+
+ def direction_vector(self) -> np.ndarray:
+ """
+ Get the unit forward direction vector of the SO3 (the x axis vector).
+
+ :returns: unit direction vector [x, y, z]
+ """
+ return self.rotation_matrix()[:, 0]
+
+ def rot_distance_to(self, r: SO3) -> float:
+ """
+ Get the rotational distance between this SO3 and another SO3,
+ defined as the angle of rotation from one SO3 to the other along the shortest arc.
+
+ :param r: the other SO3
+ :returns: the angle in radians between the orientations of the two SO3s
+ """
+ q1 = self.quaternion
+ q2 = r.quaternion
+ q_diff = quaternion_multiply(quaternion_inverse(q1), q2)
+ angle = 2 * np.arccos(q_diff[3])
+ if angle > np.pi:
+ angle -= np.pi
+ return angle
+
+ def is_approx(self, r: SO3, tolerance=1e-8) -> bool:
+ """
+ Check if two SO3s are approximately equal within a tolerance by checking that each
+ element of the quaternion vector is approximately equal.
+
+ :param r: another SO3
+ :param tolerance: the tolerance for comparing each number, if the difference
+ between each number is less than or equal to this tolerance,
+ they will be considered equal
+ :returns: True if the two SO3s are approximately equal, False otherwise
+ """
+ return np.allclose(self.quaternion, r.quaternion, atol=tolerance)
diff --git a/starter_project/autonomy/src/util/__init__.py b/starter_project/autonomy/src/util/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/starter_project/autonomy/src/util/np_utils.py b/starter_project/autonomy/src/util/np_utils.py
new file mode 100644
index 000000000..a1f361163
--- /dev/null
+++ b/starter_project/autonomy/src/util/np_utils.py
@@ -0,0 +1,32 @@
+from typing import Union
+
+import numpy as np
+from geometry_msgs.msg import Vector3, Quaternion, Point
+
+
+def _translation_to_numpy(translation: Vector3) -> np.ndarray:
+ return np.array([translation.x, translation.y, translation.z])
+
+
+def _point_to_numpy(point: Point) -> np.ndarray:
+ return np.array([point.x, point.y, point.z])
+
+
+def _rotation_to_numpy(rotation: Quaternion) -> np.ndarray:
+ return np.array([rotation.x, rotation.y, rotation.z, rotation.w])
+
+
+def normalized(v):
+ norm = np.linalg.norm(v)
+ return v / norm
+
+
+def numpify(msg: Union[Vector3, Quaternion, Point]) -> np.ndarray:
+ if msg.__class__ == Vector3:
+ return _translation_to_numpy(msg)
+ elif msg.__class__ == Quaternion:
+ return _rotation_to_numpy(msg)
+ elif msg.__class__ == Point:
+ return _point_to_numpy(msg)
+ else:
+ raise Exception("type of msg must be either Vector3 or Quaternion!!!")
diff --git a/starter_project/autonomy/src/util/ros_utils.py b/starter_project/autonomy/src/util/ros_utils.py
new file mode 100644
index 000000000..468e9b791
--- /dev/null
+++ b/starter_project/autonomy/src/util/ros_utils.py
@@ -0,0 +1,23 @@
+import rospy
+from geometry_msgs.msg import Point
+from visualization_msgs.msg import Marker
+
+
+def send_debug_arrow(self, rot):
+ # TODO: not working
+ marker = Marker()
+ marker.header.frame_id = "odom"
+ marker.header.stamp = rospy.Time.now()
+ marker.id = 0
+ marker.action = Marker.ADD
+ marker.type = Marker.ARROW
+ marker.pose.orientation.x = rot[0]
+ marker.pose.orientation.y = rot[1]
+ marker.pose.orientation.z = rot[2]
+ marker.pose.orientation.w = rot[3]
+ marker.scale = 2.0
+ marker.color.r = 0.0
+ marker.color.b = 1.0
+ marker.color.a = 1.0
+ marker.points = [Point(0, 0, 0), Point(2, 0, 0)]
+ self.context.vis_publisher.publish(marker)
diff --git a/starter_project/autonomy/src/util/tf_utils.py b/starter_project/autonomy/src/util/tf_utils.py
new file mode 100644
index 000000000..ee0037b68
--- /dev/null
+++ b/starter_project/autonomy/src/util/tf_utils.py
@@ -0,0 +1,35 @@
+import numpy as np
+
+import rospy
+from geometry_msgs.msg import Point, TransformStamped, Vector3
+from sensor_msgs.msg import NavSatFix, geometry_msgs
+
+EARTH_RADIUS = 6371000
+
+
+def gps_to_world(gps_coord: NavSatFix, ref_coord: NavSatFix, name: str, parent: str = "world") -> TransformStamped:
+ """
+ Returns the GPS to cartesian world transform.
+
+ :param gps_coord: The gps coordinate that we want in the cartesian world frame
+ :param name: The name of the returned transform frame
+ :param parent: The name of the reference world frame
+ """
+ t = geometry_msgs.msg.TransformStamped()
+ t.header.stamp = rospy.Time.now()
+ t.header.frame_id = parent
+ t.child_frame_id = name
+ longitude_delta = gps_coord.longitude - ref_coord.longitude
+ latitude_delta = gps_coord.latitude - ref_coord.latitude
+ t.transform.translation.x = np.radians(longitude_delta) * EARTH_RADIUS
+ t.transform.translation.y = np.radians(latitude_delta) * EARTH_RADIUS
+ t.transform.translation.z = gps_coord.altitude
+ return t
+
+
+def vector3_to_point(vec3: Vector3) -> Point:
+ return Point(x=vec3.x, y=vec3.y, z=vec3.z)
+
+
+def point_to_vector3(pt: Point) -> Vector3:
+ return Vector3(x=pt.x, y=pt.y, z=pt.z)
diff --git a/style.sh b/style.sh
index 9ef9d9f24..779ef6d8e 100755
--- a/style.sh
+++ b/style.sh
@@ -1,60 +1,69 @@
#!/usr/bin/env bash
-# Print each command, fail on unset variables
-set -xu
+# See: https://vaneyckt.io/posts/safer_bash_scripts_with_set_euxo_pipefail/
+set -Eeuo pipefail
-RED='\033[0;31m'
-NC='\033[0m'
+readonly RED='\033[0;31m'
+readonly NC='\033[0m'
-## Check that all tools are installed
-
-clang_format_executable=clang-format-12
-clang_format_executable_path=$(which "$clang_format_executable")
-if [ ! -x "$clang_format_executable_path" ]; then
- echo -e "${RED}[Error] Please install clang-format with: sudo apt install ${clang_format_executable}${NC}"
- exit 1
-fi
+BLACK_ARGS=(
+ "--line-length=120"
+ "--color"
+)
+CLANG_FORMAT_ARGS=(
+ "-style=file"
+)
-
-black_executable=black
-black_executable_path=$(which "$black_executable")
-if [ ! -x "$black_executable_path" ]; then
- echo -e "${RED}[Error] Please run pip3 install -r requirements.txt${NC}"
- exit 1
+# Just do a dry run if the "fix" argument is not passed
+if [ $# -eq 0 ] || [ "$1" != "--fix" ]; then
+ BLACK_ARGS+=("--diff") # Show difference
+ BLACK_ARGS+=("--check") # Exit with non-zero code if changes are required (for CI)
+ CLANG_FORMAT_ARGS+=("--dry-run")
fi
-# Style check Python with black
-if ! black --version | grep -q 'black, 22.8.0'; then
- echo -e "${RED}[Error] Wrong black version${NC}"
+function print_update_error() {
+ echo -e "${RED}[Error] Please update with ./ansible.sh build.yml${NC}"
exit 1
-fi
+}
+function find_executable() {
+ local readonly executable="$1"
+ local readonly version="$2"
+ local readonly path=$(which "${executable}")
+ if [ ! -x "${path}" ]; then
+ echo -e "${RED}[Error] Could not find ${executable}${NC}"
+ print_update_error
+ fi
+ if ! "${path}" --version | grep -q "${version}"; then
+ echo -e "${RED}[Error] Wrong ${executable} version${NC}"
+ print_update_error
+ fi
+ echo "${path}"
+}
-mypy_executable=mypy
-mypy_executable_path=$(which "$mypy_executable")
-if [ ! -x "$mypy_executable_path" ]; then
- echo -e "${RED}[Error] Please run pip3 install -r requirements.txt${NC}"
- exit 1
-fi
+## Check that all tools are installed
-if ! mypy --version | grep -q 'mypy 0.971'; then
- echo -e "${RED}[Error] Wrong mypy version${NC}"
- exit 1
-fi
+readonly CLANG_FORMAT_PATH=$(find_executable clang-format-16 16.0)
+readonly BLACK_PATH=$(find_executable black 23.9.1)
+readonly MYPY_PATH=$(find_executable mypy 1.5.1)
## Run checks
-# Fail immediately if any command below fails
-set -Eeo pipefail
-
echo "Style checking C++ ..."
-find ./src -regex '.*\.\(cpp\|hpp\|h\)' -exec "$clang_format_executable_path" --dry-run -style=file -i {} \;
+readonly FOLDERS=(
+ ./src/perception
+ ./src/gazebo
+ ./src/util
+)
+for FOLDER in "${FOLDERS[@]}"; do
+ find "${FOLDER}" -regex '.*\.\(cpp\|hpp\|h\)' -exec "${CLANG_FORMAT_PATH}" "${CLANG_FORMAT_ARGS[@]}" -i {} \;
+done
echo "Done"
+echo
echo "Style checking Python with black ..."
-"$black_executable_path" --check --diff --line-length=120 ./src ./scripts
-echo "Done"
+"$BLACK_PATH" "${BLACK_ARGS[@]}" ./src ./scripts
+echo
echo "Style checking Python with mypy ..."
-"$mypy_executable_path" --config-file mypy.ini --check ./src ./scripts
-echo "Done"
+"$MYPY_PATH" --config-file=mypy.ini --check ./src ./scripts
diff --git a/update.sh b/update.sh
deleted file mode 100755
index c5223cc51..000000000
--- a/update.sh
+++ /dev/null
@@ -1,4 +0,0 @@
-# script to update and install software dependencies
-rosdep update
-rosdep install --from-paths ~/catkin_ws/src/ --ignore-src -y --rosdistro=noetic
-pip install -r ~/catkin_ws/src/mrover/requirements.txt
\ No newline at end of file