diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 5a62dc471..874c68bc2 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -23,7 +23,7 @@ jobs:
uses: ros-tooling/setup-ros@v0.2
with:
use-ros2-testing: true
- required-ros-distributions: galactic
+ required-ros-distributions: humble
- name: Install Custom Dependencies
run: |
# Webots (temporary disabled)
@@ -34,4 +34,4 @@ jobs:
- name: Build packages and run and tests
uses: ros-tooling/action-ros-ci@v0.2
with:
- target-ros2-distro: galactic
+ target-ros2-distro: humble
diff --git a/.github/workflows/linter_cpp.yml b/.github/workflows/linter_cpp.yml
index 027200ec2..271ee819e 100644
--- a/.github/workflows/linter_cpp.yml
+++ b/.github/workflows/linter_cpp.yml
@@ -10,7 +10,7 @@ jobs:
fail-fast: false
matrix:
linters: [cpplint, xmllint, lint_cmake]
- packages: [mep3_driver, mep3_behavior_tree, mep3_navigation]
+ packages: [mep3_hardware, mep3_behavior, mep3_navigation]
steps:
- name: Checkout repository
uses: actions/checkout@v2
@@ -22,6 +22,6 @@ jobs:
- name: Run linter ${{ matrix.linters }} for package ${{ matrix.packages }}
uses: ros-tooling/action-ros-lint@v0.1
with:
- distribution: galactic
+ distribution: humble
linter: ${{ matrix.linters }}
package-name: ${{ matrix.packages }}
diff --git a/.github/workflows/linter_python.yml b/.github/workflows/linter_python.yml
index 948e0de15..75e568edd 100644
--- a/.github/workflows/linter_python.yml
+++ b/.github/workflows/linter_python.yml
@@ -22,6 +22,6 @@ jobs:
- name: Run linter ${{ matrix.linters }} for package ${{ matrix.packages }}
uses: ros-tooling/action-ros-lint@v0.1
with:
- distribution: galactic
+ distribution: humble
linter: ${{ matrix.linters }}
package-name: ${{ matrix.packages }}
diff --git a/.gitignore b/.gitignore
index 98fbcd30d..fba065966 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,4 @@
-.vscode/
+.vscode/settings.json
.idea/
build/
install/
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 000000000..20867a4a2
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,64 @@
+{
+ "configurations": [
+ {
+ "name": "ROS2 Humble",
+ "browse": {
+ "databaseFilename": "",
+ "limitSymbolsToIncludedHeaders": true
+ },
+ "includePath": [
+ "${env:HOME}/webots/include/controller/cpp",
+ "/usr/include/python3.10",
+ "/usr/include",
+ "${workspaceFolder}/mep3_behavior/include",
+ "${workspaceFolder}/../../install/mep3_msgs/include/mep3_msgs",
+ "${workspaceFolder}/mep3_hardware/include",
+ "${workspaceFolder}/mep3_navigation/include",
+ "${workspaceFolder}/mep3_behavior/include",
+ "${workspaceFolder}/../dynamixel-workbench/dynamixel_workbench_toolbox/include",
+ "/opt/ros/humble/include",
+ "/opt/ros/humble/include/rclcpp",
+ "/opt/ros/humble/include/rclcpp_action",
+ "/opt/ros/humble/include/action_msgs",
+ "/opt/ros/humble/include/rcl_action",
+ "/opt/ros/humble/include/unique_identifier_msgs",
+ "/opt/ros/humble/include/can_msgs",
+ "/opt/ros/humble/include/pluginlib",
+ "/opt/ros/humble/include/geometry_msgs",
+ "/opt/ros/humble/include/nav_msgs",
+ "/opt/ros/humble/include/std_msgs",
+ "/opt/ros/humble/include/diagnostic_msgs",
+ "/opt/ros/humble/include/tf2_geometry_msgs",
+ "/opt/ros/humble/include/class_loader",
+ "/opt/ros/humble/include/rosidl_runtime_cpp",
+ "/opt/ros/humble/include/rosidl_runtime_c",
+ "/opt/ros/humble/include/rosidl_typesupport_interface",
+ "/opt/ros/humble/include/builtin_interfaces",
+ "/opt/ros/humble/include/rclcpp_lifecycle",
+ "/opt/ros/humble/include/rcutils",
+ "/opt/ros/humble/include/rcl",
+ "/opt/ros/humble/include/rmw",
+ "/opt/ros/humble/include/rcl_yaml_param_parser",
+ "/opt/ros/humble/include/rcl_interfaces",
+ "/opt/ros/humble/include/rcpputils",
+ "/opt/ros/humble/include/tracetools",
+ "/opt/ros/humble/include/libstatistics_collector",
+ "/opt/ros/humble/include/statistics_msgs",
+ "/opt/ros/humble/include/lifecycle_msgs",
+ "/opt/ros/humble/include/rcl_lifecycle",
+ "/opt/ros/humble/include/map_msgs",
+ "/opt/ros/humble/include/nav2_msgs",
+ "/opt/ros/humble/include/tf2",
+ "/opt/ros/humble/include/bond",
+ "/opt/ros/humble/include/tf2_ros",
+ "/opt/ros/humble/include/tf2_msgs",
+ "/opt/ros/humble/include/ament_index_cpp",
+ ],
+ "intelliSenseMode": "gcc-x64",
+ "compilerPath": "/usr/bin/g++",
+ "cStandard": "c99",
+ "cppStandard": "c++17"
+ }
+ ],
+ "version": 4
+}
diff --git a/README.md b/README.md
index a435db830..a7d42d68e 100644
--- a/README.md
+++ b/README.md
@@ -18,15 +18,15 @@ Memristor Eurobot Platform based on ROS 2
## Getting started
```sh
-# Make sure you have ROS 2 Galactic installed.
-# https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html
+# Make sure you have ROS 2 Humble installed.
+# https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html
# Source ROS 2
-source /opt/ros/galactic/local_setup.bash
+source /opt/ros/humble/local_setup.bash
# Create a workspace
-mkdir -p ~/galactic_ws/src
-cd ~/galactic_ws
+mkdir -p ~/ros2_ws/src
+cd ~/ros2_ws
git clone git@github.com:memristor/mep3.git src/mep3
# On embedded device: touch src/mep3/mep3_simulation/COLCON_IGNORE
@@ -80,7 +80,7 @@ ros2 launch mep3_bringup simulation_launch.py bt:=false
```
- Control the robot from another terminal window
```sh
-source /opt/ros/galactic/local_setup.bash
+source /opt/ros/humble/local_setup.bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=big/cmd_vel
```
- Run the simulation with the behavior tree:
@@ -105,17 +105,17 @@ ros2 launch mep3_bringup rviz_launch.py
- Change working directory to `~/ros2_ws`
- Run the following command:
```sh
- source /opt/ros/galactic/local_setup.bash
+ source /opt/ros/humble/local_setup.bash
colcon test --event-handlers console_cohesion+ --return-code-on-test-failure
```
### BehaviorTree strategies
-Robot strategies are located inside [mep3_behavior_tree/assets](./mep3_behavior_tree/assets)
+Robot strategies are located inside [mep3_behavior/assets](./mep3_behavior/assets)
directory with the following hierarchy:
```ini
-mep3_behavior_tree/assets
+mep3_behavior/assets
- skills/
- big_retract_hands.xml # skill for Big robot
- small_replace_statuette.xml # skill for Small robot
diff --git a/docker/Dockerfile b/docker/Dockerfile
deleted file mode 100644
index 6ad12e64a..000000000
--- a/docker/Dockerfile
+++ /dev/null
@@ -1,92 +0,0 @@
-FROM ros:humble
-
-ARG DEBIAN_FRONTEND=noninteractive
-ARG UID=1000
-
-# User
-RUN useradd -d /memristor -m \
- -u $UID -U \
- -s /usr/bin/bash \
- -c "Memristor Robotics" memristor && \
- echo "memristor ALL=(ALL) NOPASSWD: ALL" >> /etc/sudoers
-
-# System utilities
-RUN apt-get update
-RUN apt-get install -y \
- git \
- git-lfs \
- curl \
- wget \
- vim \
- rsync \
- dialog
-
-# ROS packages
-RUN apt-get install -y \
- ros-humble-navigation2 \
- ros-humble-nav2-bringup \
- ros-humble-rviz2 \
- ros-humble-teleop-twist-keyboard \
- ros-humble-webots-ros2
-RUN echo 'source /opt/ros/humble/local_setup.bash' >> /memristor/.bashrc
-
-# VS Code
-RUN curl -L -o /tmp/vscode.deb \
- 'https://code.visualstudio.com/sha/download?build=stable&os=linux-deb-x64' && \
- apt-get install -y /tmp/vscode.deb && \
- rm -f /tmp/vscode.deb
-RUN apt-get install -y alsa libxshmfence1 libgtk-3-dev
-RUN su memristor -c 'code --install-extension eamodio.gitlens' && \
- su memristor -c 'code --install-extension ms-python.python' && \
- su memristor -c 'code --install-extension ms-vscode.cpptools-extension-pack' && \
- su memristor -c 'code --install-extension usernamehw.errorlens' && \
- su memristor -c 'code --install-extension ms-iot.vscode-ros'
-
-# Webots
-RUN curl -L -o /tmp/webots.deb \
- '/~https://github.com/cyberbotics/webots/releases/download/R2022b/webots_2022b_amd64.deb' && \
- apt-get install -y /tmp/webots.deb && \
- rm -f /tmp/webots.deb && \
- mkdir -p /memristor/.config/Cyberbotics
-
-# TurboVNC
-RUN curl -L -o /tmp/turbovnc.deb \
- 'https://downloads.sourceforge.net/project/turbovnc/3.0.1/turbovnc_3.0.1_amd64.deb?use_mirror=autoselect' && \
- apt-get install -y /tmp/turbovnc.deb && \
- rm -f /tmp/turbovnc.deb && \
- echo 'PATH="$PATH:/opt/TurboVNC/bin"' >> /memristor/.bashrc
-
-# VirtualGL
-RUN curl -L -o /tmp/virtualgl.deb \
- 'https://downloads.sourceforge.net/project/virtualgl/3.0.2/virtualgl_3.0.2_amd64.deb?use_mirror=autoselect' && \
- apt-get install -y /tmp/virtualgl.deb && \
- rm -f /tmp/virtualgl.deb && \
- echo 'PATH="$PATH:/opt/VirtualGL/bin"' >> /memristor/.bashrc && \
- printf "1\n\n\n\nX" | /opt/VirtualGL/bin/vglserver_config && \
- sudo usermod -a -G vglusers memristor
-
-# NoVNC
-RUN apt-get install -y novnc && \
- ln -sf /usr/share/novnc/vnc.html /usr/share/novnc/index.html && \
- mkdir -p /memristor/.host
-
-# XFCE
-RUN apt-get install -y --no-install-recommends \
- xorg \
- dbus-x11 \
- xfce4 \
- xfce4-terminal && \
- echo '/usr/sbin/lightdm' > /etc/X11/default-display-manager
-
-# User config
-RUN echo 'export PS1="$(tput setaf 4)\w$(tput sgr0) [$(tput setaf 5)\$?$(tput sgr0)] $(tput setaf 3)\$(git branch --show-current 2>/dev/null | xargs -I{} echo \"({})\")$(tput sgr0)\n$(tput setaf 2)\\$ \\[$(tput sgr0)\\]"' >> /memristor/.bashrc
-RUN echo 'cp -p /memristor/.host/.Xauthority /memristor/.Xauthority' >> /memristor/.bashrc
-COPY ./config/vscode/. /memristor/ros2_ws/.vscode/
-COPY ./config/Cyberobotics/. /memristor/.config/Cyberbotics/
-COPY ./config/setup.sh /usr/bin/
-COPY ./config/vnc.sh /usr/bin/
-
-RUN chown -R memristor:memristor /memristor
-
-USER memristor
-WORKDIR /memristor/ros2_ws
diff --git a/docker/Dockerfile.base b/docker/Dockerfile.base
new file mode 100644
index 000000000..98b4091d7
--- /dev/null
+++ b/docker/Dockerfile.base
@@ -0,0 +1,68 @@
+FROM ros:humble
+
+ARG DEBIAN_FRONTEND=noninteractive
+ARG UID=1000
+
+# User
+RUN useradd -d /memristor -m \
+ -u $UID -U \
+ -s /usr/bin/bash \
+ -c "Memristor Robotics" memristor && \
+ echo "memristor ALL=(ALL) NOPASSWD: ALL" >> /etc/sudoers
+
+# Essentials
+RUN apt-get update && apt-get install -y \
+ ros-humble-navigation2 \
+ ros-humble-nav2-bringup \
+ ros-humble-rviz2 \
+ ros-humble-teleop-twist-keyboard \
+ ros-humble-webots-ros2 \
+ ros-humble-dynamixel-sdk \
+ ros-humble-can-msgs \
+ ros-humble-ruckig \
+ ros-humble-laser-filters \
+ ros-humble-domain-bridge \
+ ros-humble-rmw-cyclonedds-cpp \
+ python3-pil \
+ alsa \
+ libxshmfence1 \
+ libgtk-3-dev \
+ git \
+ git-lfs \
+ curl \
+ wget \
+ vim \
+ rsync \
+ dialog
+
+# VS Code
+RUN curl -L -o /tmp/vscode.deb \
+ 'https://code.visualstudio.com/sha/download?build=stable&os=linux-deb-x64' && \
+ apt-get install -y /tmp/vscode.deb && \
+ rm -f /tmp/vscode.deb
+
+RUN su memristor -c 'code --install-extension eamodio.gitlens' && \
+ su memristor -c 'code --install-extension ms-python.python' && \
+ su memristor -c 'code --install-extension ms-vscode.cpptools-extension-pack' && \
+ su memristor -c 'code --install-extension usernamehw.errorlens' && \
+ su memristor -c 'code --install-extension ms-iot.vscode-ros'
+
+# Webots
+RUN curl -L -o /tmp/webots.deb \
+ '/~https://github.com/cyberbotics/webots/releases/download/R2022b/webots_2022b_amd64.deb' && \
+ apt-get install -y /tmp/webots.deb && \
+ rm -f /tmp/webots.deb && \
+ mkdir -p /memristor/.config/Cyberbotics
+
+# User config
+COPY config/bashrc /tmp/bashrc
+COPY ./config/vscode/. /memristor/ros2_ws/.vscode/
+COPY ./config/Cyberobotics/. /memristor/.config/Cyberbotics/
+COPY ./config/setup.sh /usr/bin/
+
+RUN cat /tmp/bashrc >> /memristor/.bashrc && \
+ rm -f /tmp/bashrc && \
+ chown -R memristor:memristor /memristor
+
+USER memristor
+WORKDIR /memristor/ros2_ws
diff --git a/docker/Dockerfile.vnc b/docker/Dockerfile.vnc
new file mode 100644
index 000000000..4767f90be
--- /dev/null
+++ b/docker/Dockerfile.vnc
@@ -0,0 +1,38 @@
+FROM mep3
+
+USER root
+
+# TurboVNC
+RUN curl -L -o /tmp/turbovnc.deb \
+ 'https://downloads.sourceforge.net/project/turbovnc/3.0.1/turbovnc_3.0.1_amd64.deb?use_mirror=autoselect' && \
+ apt-get install -y /tmp/turbovnc.deb && \
+ rm -f /tmp/turbovnc.deb && \
+ echo 'PATH="$PATH:/opt/TurboVNC/bin"' >> /memristor/.bashrc
+
+# VirtualGL
+RUN curl -L -o /tmp/virtualgl.deb \
+ 'https://downloads.sourceforge.net/project/virtualgl/3.0.2/virtualgl_3.0.2_amd64.deb?use_mirror=autoselect' && \
+ apt-get install -y /tmp/virtualgl.deb && \
+ rm -f /tmp/virtualgl.deb && \
+ echo 'PATH="$PATH:/opt/VirtualGL/bin"' >> /memristor/.bashrc && \
+ printf "1\n\n\n\nX" | /opt/VirtualGL/bin/vglserver_config && \
+ sudo usermod -a -G vglusers memristor
+
+# NoVNC
+RUN apt-get install -y novnc && \
+ ln -sf /usr/share/novnc/vnc.html /usr/share/novnc/index.html && \
+ mkdir -p /memristor/.host
+
+# XFCE
+RUN apt-get install -y --no-install-recommends \
+ xorg \
+ dbus-x11 \
+ xfce4 \
+ xfce4-terminal && \
+ echo '/usr/sbin/lightdm' > /etc/X11/default-display-manager
+
+# User config
+COPY ./config/vnc.sh /usr/bin/
+
+USER memristor
+WORKDIR /memristor/ros2_ws
diff --git a/docker/Makefile b/docker/Makefile
index 367dc6444..8c14b1775 100644
--- a/docker/Makefile
+++ b/docker/Makefile
@@ -5,7 +5,11 @@ NVIDIA_GPU:=$(shell docker info | grep Runtimes | grep nvidia 1> /dev/null && ec
.PHONY: destroy build run setup
build:
- @docker build ${ROOT_DIR} -t mep3 --build-arg UID=${UID}
+ @docker build ${ROOT_DIR} -f ${ROOT_DIR}/Dockerfile.base -t mep3 --build-arg UID=${UID}
+
+build-vnc: build
+# TODO (parag): Handle the `mep3-vnc` container in other commands
+ @docker build ${ROOT_DIR} -f ${ROOT_DIR}/Dockerfile.vnc -t mep3-vnc
run:
@docker run \
diff --git a/docker/config/Cyberobotics/Webots-R2022b.conf b/docker/config/Cyberobotics/Webots-R2022b.conf
index ab4849b4b..b3eba5e5f 100644
--- a/docker/config/Cyberobotics/Webots-R2022b.conf
+++ b/docker/config/Cyberobotics/Webots-R2022b.conf
@@ -6,5 +6,12 @@ startupMode=Real-time
telemetry=false
theme=webots_night.qss
+[OpenGL]
+GTAO=0
+disableAntiAliasing=true
+disableShadows=true
+textureFiltering=1
+textureQuality=0
+
[Internal]
firstLaunch=false
diff --git a/docker/config/bashrc b/docker/config/bashrc
new file mode 100644
index 000000000..4f510426f
--- /dev/null
+++ b/docker/config/bashrc
@@ -0,0 +1,5 @@
+source /opt/ros/humble/local_setup.bash
+source /memristor/ros2_ws/install/local_setup.bash
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+export PS1="$(tput setaf 4)\w$(tput sgr0) [$(tput setaf 5)\$?$(tput sgr0)] $(tput setaf 3)\$(git branch --show-current 2>/dev/null | xargs -I{} echo \"({})\")$(tput sgr0)\n$(tput setaf 2)\\$ \\[$(tput sgr0)\\]"
+cp -p /memristor/.host/.Xauthority /memristor/.Xauthority
\ No newline at end of file
diff --git a/docker/config/setup.sh b/docker/config/setup.sh
index 48c3b360d..1529b98df 100755
--- a/docker/config/setup.sh
+++ b/docker/config/setup.sh
@@ -2,7 +2,6 @@
if dialog --title 'mep3 config' --yesno 'Run first time ROS setup' 5 30; then
clear
- touch /memristor/ros2_ws/src/mep3/mep3_simulation/COLCON_IGNORE
sudo -E rosdep init
sudo -E apt-get install -y python3-vcstool
cd /memristor/ros2_ws && vcs import src < /memristor/ros2_ws/src/mep3/mep3.repos
diff --git a/docker/config/shortcuts.sh b/docker/config/shortcuts.sh
index 98f963df9..313b884fc 100755
--- a/docker/config/shortcuts.sh
+++ b/docker/config/shortcuts.sh
@@ -87,9 +87,6 @@ shortcut_scratchpad_print() {
$1 == "navigate_to_pose" {
printf "\n", $2, $3, $4;
}
- $1 == "precise_navigate_to_pose" {
- printf "\n", $2, $3, $4;
- }
$1 == "dynamixel" {
printf "\n", $2, $3, $4, $5, $6;
}
@@ -387,20 +384,3 @@ shortcut_scoreboard_task() {
eval "ros2 topic pub --once /scoreboard mep3_msgs/msg/Scoreboard '${message}'"
}
alias sc="shortcut_scoreboard_task"
-
-## Launch ResistanceMeterAction action
-# Arguments:
-# - namespace [optional]
-# - side [right/left] [optional]
-shortcut_resistance_meter() {
- if echo "$2" | grep -qv '^[0-9\.-]*$'; then
- namespace="${1:-big}"
- shift
- else
- namespace='big'
- fi
- side="${1:-right}"
- last_action="resistance_meter ${task} ${points}"
- eval "ros2 action send_goal /${namespace}/resistance_meter/${side} mep3_msgs/action/ResistanceMeter {}"
-}
-alias re="shortcut_resistance_meter"
diff --git a/install.bash b/install.bash
deleted file mode 100755
index b1fb6af8e..000000000
--- a/install.bash
+++ /dev/null
@@ -1,61 +0,0 @@
-#!/usr/bin/env bash
-
-# Usage: ./script [rpi|pc]
-# Example: ./script pc
-
-# Configure the script
-set -e
-
-# If there is a ROS environment already sourced it might cause installation issues.
-if [ "$(env | grep ROS)" ] && [ ! "$(env | grep 'ROS_DISTRO=galactic')" ]; then
- echo "ERROR: You already have a ROS environment sourced (please check your ~/.bashrc)."
- exit 2
-fi
-
-# Export arguments
-TARGET_HARDWARE="$1"
-if [ "${TARGET_HARDWARE}" != "rpi" ] && [ "${TARGET_HARDWARE}" != "pc" ]; then
- echo "Usage: ./script [rpi|pc]"
- exit 1
-fi
-
-# Install ROS 2
-# Referece: https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html
-sudo apt update
-sudo apt install -y locales
-sudo locale-gen en_US en_US.UTF-8
-sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
-export LANG=en_US.UTF-8
-
-sudo apt install -y curl gnupg2 lsb-release
-sudo curl -sSL 'https://raw.githubusercontent.com/ros/rosdistro/master/ros.key' -o /usr/share/keyrings/ros-archive-keyring.gpg
-echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2-testing/ubuntu $(source /etc/os-release && echo "$UBUNTU_CODENAME") main" | sudo tee /etc/apt/sources.list.d/ros2.list >/dev/null
-sudo apt update
-
-case "${TARGET_HARDWARE}" in
- 'pc') sudo apt install -y ros-galactic-desktop python3-rosdep;;
- 'rpi') sudo apt install -y ros-galactic-ros-base python3-rosdep;;
-esac
-
-# Install apt dependencies
-sudo apt install -y git build-essential python3-argcomplete python3-colcon-common-extensions
-
-# Clone mep3
-mkdir -p ~/galactic_ws/src
-git clone /~https://github.com/memristor/mep3.git ~/galactic_ws/src/mep3
-if [ "${TARGET_HARDWARE}" = "rpi" ]; then
- touch ~/galactic_ws/src/mep3/mep3_simulation/COLCON_IGNORE
-fi
-
-# Build mep3
-pushd ~/galactic_ws
-source /opt/ros/galactic/local_setup.bash
-if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then
- # rosdep can be initialized only once so we have to check whether it has already been initialized.
- sudo rosdep init
-fi
-rosdep update
-yes | rosdep install --from-paths src --ignore-src
-colcon build
-source ./install/local_setup.bash
-popd
diff --git a/mep3_behavior_tree/CMakeLists.txt b/mep3_behavior/CMakeLists.txt
similarity index 90%
rename from mep3_behavior_tree/CMakeLists.txt
rename to mep3_behavior/CMakeLists.txt
index 99ca9c292..ffa552b23 100644
--- a/mep3_behavior_tree/CMakeLists.txt
+++ b/mep3_behavior/CMakeLists.txt
@@ -1,16 +1,14 @@
cmake_minimum_required(VERSION 3.5)
-project(mep3_behavior_tree)
+project(mep3_behavior)
-# Default to C17
+# Default to C99
if(NOT CMAKE_C_STANDARD)
- set(CMAKE_C_STANDARD 17)
- set(CMAKE_C_STANDARD_REQUIRED ON)
+ set(CMAKE_C_STANDARD 99)
endif()
-# Default to C++20
+# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 20)
- set(CMAKE_CXX_STANDARD_REQUIRED ON)
+ set(CMAKE_CXX_STANDARD 17)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
diff --git a/mep3_behavior_tree/CPPLINT.cfg b/mep3_behavior/CPPLINT.cfg
similarity index 100%
rename from mep3_behavior_tree/CPPLINT.cfg
rename to mep3_behavior/CPPLINT.cfg
diff --git a/mep3_behavior_tree/assets/skills/big_retract_hands.xml b/mep3_behavior/assets/skills/big_retract_hands.xml
similarity index 100%
rename from mep3_behavior_tree/assets/skills/big_retract_hands.xml
rename to mep3_behavior/assets/skills/big_retract_hands.xml
diff --git a/mep3_behavior_tree/assets/skills/common_swap.xml b/mep3_behavior/assets/skills/common_swap.xml
similarity index 100%
rename from mep3_behavior_tree/assets/skills/common_swap.xml
rename to mep3_behavior/assets/skills/common_swap.xml
diff --git a/mep3_behavior_tree/assets/skills/example.xml b/mep3_behavior/assets/skills/example.xml
similarity index 100%
rename from mep3_behavior_tree/assets/skills/example.xml
rename to mep3_behavior/assets/skills/example.xml
diff --git a/mep3_behavior_tree/assets/strategies/big/purple_strategy.xml b/mep3_behavior/assets/strategies/big/purple_strategy.xml
similarity index 74%
rename from mep3_behavior_tree/assets/strategies/big/purple_strategy.xml
rename to mep3_behavior/assets/strategies/big/purple_strategy.xml
index 65a1dd805..26a895f9a 100644
--- a/mep3_behavior_tree/assets/strategies/big/purple_strategy.xml
+++ b/mep3_behavior/assets/strategies/big/purple_strategy.xml
@@ -2,10 +2,12 @@
+
+
-
-
+
+
diff --git a/mep3_behavior_tree/assets/strategies/homologation_forward_backward.xml b/mep3_behavior/assets/strategies/homologation_forward_backward.xml
similarity index 100%
rename from mep3_behavior_tree/assets/strategies/homologation_forward_backward.xml
rename to mep3_behavior/assets/strategies/homologation_forward_backward.xml
diff --git a/mep3_behavior_tree/assets/strategies/homologation_resistance.xml b/mep3_behavior/assets/strategies/homologation_resistance.xml
similarity index 100%
rename from mep3_behavior_tree/assets/strategies/homologation_resistance.xml
rename to mep3_behavior/assets/strategies/homologation_resistance.xml
diff --git a/mep3_behavior_tree/assets/strategies/mep3_tree_nodes.xml b/mep3_behavior/assets/strategies/mep3_tree_nodes.xml
similarity index 100%
rename from mep3_behavior_tree/assets/strategies/mep3_tree_nodes.xml
rename to mep3_behavior/assets/strategies/mep3_tree_nodes.xml
diff --git a/mep3_behavior_tree/assets/strategies/small/purple_strategy.xml b/mep3_behavior/assets/strategies/small/purple_strategy.xml
similarity index 100%
rename from mep3_behavior_tree/assets/strategies/small/purple_strategy.xml
rename to mep3_behavior/assets/strategies/small/purple_strategy.xml
diff --git a/mep3_behavior_tree/assets/strategies/statuette.xml b/mep3_behavior/assets/strategies/statuette.xml
similarity index 100%
rename from mep3_behavior_tree/assets/strategies/statuette.xml
rename to mep3_behavior/assets/strategies/statuette.xml
diff --git a/mep3_behavior_tree/assets/strategies/strategy.xml b/mep3_behavior/assets/strategies/strategy.xml
similarity index 100%
rename from mep3_behavior_tree/assets/strategies/strategy.xml
rename to mep3_behavior/assets/strategies/strategy.xml
diff --git a/mep3_behavior_tree/assets/strategies/testing.xml b/mep3_behavior/assets/strategies/testing.xml
similarity index 100%
rename from mep3_behavior_tree/assets/strategies/testing.xml
rename to mep3_behavior/assets/strategies/testing.xml
diff --git a/mep3_behavior_tree/assets/skills/.gitkeep b/mep3_behavior/assets/tasks/.gitkeep
similarity index 100%
rename from mep3_behavior_tree/assets/skills/.gitkeep
rename to mep3_behavior/assets/tasks/.gitkeep
diff --git a/mep3_behavior_tree/assets/tasks/big_replica_and_statuette.xml b/mep3_behavior/assets/tasks/big_replica_and_statuette.xml
similarity index 100%
rename from mep3_behavior_tree/assets/tasks/big_replica_and_statuette.xml
rename to mep3_behavior/assets/tasks/big_replica_and_statuette.xml
diff --git a/mep3_behavior_tree/assets/tasks/example_moving.xml b/mep3_behavior/assets/tasks/example_moving.xml
similarity index 100%
rename from mep3_behavior_tree/assets/tasks/example_moving.xml
rename to mep3_behavior/assets/tasks/example_moving.xml
diff --git a/mep3_behavior_tree/assets/tasks/example_scoreboard.xml b/mep3_behavior/assets/tasks/example_scoreboard.xml
similarity index 100%
rename from mep3_behavior_tree/assets/tasks/example_scoreboard.xml
rename to mep3_behavior/assets/tasks/example_scoreboard.xml
diff --git a/mep3_behavior_tree/assets/tasks/small_excavation_squares.xml b/mep3_behavior/assets/tasks/small_excavation_squares.xml
similarity index 100%
rename from mep3_behavior_tree/assets/tasks/small_excavation_squares.xml
rename to mep3_behavior/assets/tasks/small_excavation_squares.xml
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/add_obstacle_action.hpp b/mep3_behavior/include/mep3_behavior/add_obstacle_action.hpp
similarity index 95%
rename from mep3_behavior_tree/include/mep3_behavior_tree/add_obstacle_action.hpp
rename to mep3_behavior/include/mep3_behavior/add_obstacle_action.hpp
index c500259a9..5b426e5e0 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/add_obstacle_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/add_obstacle_action.hpp
@@ -18,7 +18,7 @@
#include
#include
-#include "mep3_behavior_tree/pose_2d.hpp"
+#include "mep3_behavior/pose_2d.hpp"
#include "mep3_msgs/msg/temporal_obstacle.hpp"
#include "behaviortree_cpp_v3/behavior_tree.h"
@@ -27,7 +27,7 @@
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/point.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class AddObstacleAction : public BT::AsyncActionNode
{
@@ -73,6 +73,6 @@ namespace mep3_behavior_tree
return BT::NodeStatus::SUCCESS;
}
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__ADD_OBSTACLE_ACTION_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/blackboard_control_flow.hpp b/mep3_behavior/include/mep3_behavior/blackboard_control_flow.hpp
similarity index 99%
rename from mep3_behavior_tree/include/mep3_behavior_tree/blackboard_control_flow.hpp
rename to mep3_behavior/include/mep3_behavior/blackboard_control_flow.hpp
index c25f7a62f..04060edfe 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/blackboard_control_flow.hpp
+++ b/mep3_behavior/include/mep3_behavior/blackboard_control_flow.hpp
@@ -23,7 +23,7 @@
#include "behaviortree_cpp_v3/control_node.h"
#include "rclcpp/rclcpp.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class CompareBlackboardControl : public BT::ControlNode
@@ -368,6 +368,6 @@ class PassAction : public BT::SyncActionNode
}
};
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__BLACKBOARD_CONTROL_FLOW_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/bt_action_node.hpp b/mep3_behavior/include/mep3_behavior/bt_action_node.hpp
similarity index 61%
rename from mep3_behavior_tree/include/mep3_behavior_tree/bt_action_node.hpp
rename to mep3_behavior/include/mep3_behavior/bt_action_node.hpp
index e5ad9b062..7223e723a 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/bt_action_node.hpp
+++ b/mep3_behavior/include/mep3_behavior/bt_action_node.hpp
@@ -12,77 +12,64 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef MEP3_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
-#define MEP3_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
+#ifndef NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
#include
#include
-#include
+#include
#include "behaviortree_cpp_v3/action_node.h"
-#include "rclcpp/executors/single_threaded_executor.hpp"
-#include "mep3_behavior_tree/team_color_strategy_mirror.hpp"
+#include "nav2_util/node_utils.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
+#include "nav2_behavior_tree/bt_conversions.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
+
+using namespace std::chrono_literals; // NOLINT
+
/**
- * @brief Abstract class representing an action based BT node
- * @tparam ActionT Type of action
- */
+ * @brief Abstract class representing an action based BT node
+ * @tparam ActionT Type of action
+ */
template
class BtActionNode : public BT::ActionNodeBase
{
public:
/**
- * @brief A nav2_behavior_tree::BtActionNode constructor
- * @param xml_tag_name Name for the XML tag for this node
- * @param action_name Action name this node creates a client for
- * @param conf BT node configuration
- */
+ * @brief A nav2_behavior_tree::BtActionNode constructor
+ * @param xml_tag_name Name for the XML tag for this node
+ * @param action_name Action name this node creates a client for
+ * @param conf BT node configuration
+ */
BtActionNode(
- const std::string & xml_tag_name, const std::string & action_name,
+ const std::string & xml_tag_name,
+ const std::string & action_name,
const BT::NodeConfiguration & conf)
: BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name)
{
node_ = config().blackboard->template get("node");
- callback_group_ =
- node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+ callback_group_ = node_->create_callback_group(
+ rclcpp::CallbackGroupType::MutuallyExclusive,
+ false);
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
- bt_loop_duration_ = std::chrono::milliseconds(10);
- server_timeout_ = std::chrono::milliseconds(1000);
+ // Get the required items from the blackboard
+ bt_loop_duration_ =
+ config().blackboard->template get("bt_loop_duration");
+ server_timeout_ =
+ config().blackboard->template get("server_timeout");
+ getInput("server_timeout", server_timeout_);
// Initialize the input and output messages
goal_ = typename ActionT::Goal();
result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult();
- std::string raw_action_name;
- std::string raw_name;
- if (getInput("server_name", raw_action_name)) {
- action_name_ = raw_action_name;
- }
- if (raw_action_name == "" && getInput("label", raw_name)) {
- // Unfortunately, `name` is a reserved property, so we have to use `label`.
- action_name_ = action_name + "/" + raw_name;
+ std::string remapped_action_name;
+ if (getInput("server_name", remapped_action_name)) {
+ action_name_ = remapped_action_name;
}
-
- std::string raw_mirror;
- if (!getInput("mirror", raw_mirror))
- raw_mirror = "default";
- mirror_ = StrategyMirror::string_to_mirror_enum(raw_mirror);
-
- original_action_name_ = action_name_;
- if (g_StrategyMirror.server_name_requires_mirroring(action_name_, mirror_)) {
- g_StrategyMirror.remap_server_name(action_name_);
- RCLCPP_INFO(
- node_->get_logger(),
- "Remapping \"%s\" action server to \"%s\"",
- original_action_name_.c_str(),
- action_name_.c_str()
- );
- }
-
createActionClient(action_name_);
// Give the derive class a chance to do any initialization
@@ -91,44 +78,39 @@ class BtActionNode : public BT::ActionNodeBase
BtActionNode() = delete;
- virtual ~BtActionNode() {}
+ virtual ~BtActionNode()
+ {
+ }
/**
- * @brief Create instance of an action client
- * @param action_name Action name to create client for
- */
+ * @brief Create instance of an action client
+ * @param action_name Action name to create client for
+ */
void createActionClient(const std::string & action_name)
{
- if (BtActionNode::action_client_list_.find(action_name) != action_client_list_.end()) {
- action_client_ = BtActionNode::action_client_list_[action_name];
- RCLCPP_INFO(node_->get_logger(), "Reusing an existing \"%s\" action server", action_name.c_str());
- return;
- }
-
// Now that we have the ROS node to use, create the action client for this BT action
action_client_ = rclcpp_action::create_client(node_, action_name, callback_group_);
// Make sure the server is actually there before continuing
- RCLCPP_INFO(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
- if (!action_client_->wait_for_action_server(std::chrono::seconds(35))) {
- RCLCPP_FATAL(node_->get_logger(), "Action server \"%s\" is not found", action_name.c_str());
- exit(1);
+ RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
+ if (!action_client_->wait_for_action_server(1s)) {
+ RCLCPP_ERROR(
+ node_->get_logger(), "\"%s\" action server not available after waiting for 1 s",
+ action_name.c_str());
+ throw std::runtime_error(std::string("Action server %s not available", action_name.c_str()));
}
- BtActionNode::action_client_list_[action_name] = action_client_;
}
/**
- * @brief Any subclass of BtActionNode that accepts parameters must provide a
- * providedPorts method and call providedBasicPorts in it.
- * @param addition Additional ports to add to BT port list
- * @return BT::PortsList Containing basic ports along with node-specific ports
- */
+ * @brief Any subclass of BtActionNode that accepts parameters must provide a
+ * providedPorts method and call providedBasicPorts in it.
+ * @param addition Additional ports to add to BT port list
+ * @return BT::PortsList Containing basic ports along with node-specific ports
+ */
static BT::PortsList providedBasicPorts(BT::PortsList addition)
{
BT::PortsList basic = {
BT::InputPort("server_name", "Action server name"),
- BT::InputPort("label", "Action name suffix"),
- BT::InputPort("mirror", "Action mirroring flag"),
BT::InputPort("server_timeout")
};
basic.insert(addition.begin(), addition.end());
@@ -137,49 +119,68 @@ class BtActionNode : public BT::ActionNodeBase
}
/**
- * @brief Creates list of BT ports
- * @return BT::PortsList Containing basic ports along with node-specific ports
- */
- static BT::PortsList providedPorts() {return providedBasicPorts({});}
+ * @brief Creates list of BT ports
+ * @return BT::PortsList Containing basic ports along with node-specific ports
+ */
+ static BT::PortsList providedPorts()
+ {
+ return providedBasicPorts({});
+ }
// Derived classes can override any of the following methods to hook into the
// processing for the action: on_tick, on_wait_for_result, and on_success
/**
- * @brief Function to perform some user-defined operation on tick
- * Could do dynamic checks, such as getting updates to values on the blackboard
- */
- virtual void on_tick() {}
+ * @brief Function to perform some user-defined operation on tick
+ * Could do dynamic checks, such as getting updates to values on the blackboard
+ */
+ virtual void on_tick()
+ {
+ }
/**
- * @brief Function to perform some user-defined operation after a timeout
- * waiting for a result that hasn't been received yet
- */
- virtual void on_wait_for_result() {}
+ * @brief Function to perform some user-defined operation after a timeout
+ * waiting for a result that hasn't been received yet. Also provides access to
+ * the latest feedback message from the action server. Feedback will be nullptr
+ * in subsequent calls to this function if no new feedback is received while waiting for a result.
+ * @param feedback shared_ptr to latest feedback message, nullptr if no new feedback was received
+ */
+ virtual void on_wait_for_result(std::shared_ptr/*feedback*/)
+ {
+ }
/**
- * @brief Function to perform some user-defined operation upon successful
- * completion of the action. Could put a value on the blackboard.
- * @return BT::NodeStatus Returns SUCCESS by default, user may override return another value
- */
- virtual BT::NodeStatus on_success() {return BT::NodeStatus::SUCCESS;}
+ * @brief Function to perform some user-defined operation upon successful
+ * completion of the action. Could put a value on the blackboard.
+ * @return BT::NodeStatus Returns SUCCESS by default, user may override return another value
+ */
+ virtual BT::NodeStatus on_success()
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
/**
- * @brief Function to perform some user-defined operation whe the action is aborted.
- * @return BT::NodeStatus Returns FAILURE by default, user may override return another value
- */
- virtual BT::NodeStatus on_aborted() {return BT::NodeStatus::FAILURE;}
+ * @brief Function to perform some user-defined operation whe the action is aborted.
+ * @return BT::NodeStatus Returns FAILURE by default, user may override return another value
+ */
+ virtual BT::NodeStatus on_aborted()
+ {
+ return BT::NodeStatus::FAILURE;
+ }
/**
- * @brief Function to perform some user-defined operation when the action is cancelled.
- * @return BT::NodeStatus Returns SUCCESS by default, user may override return another value
- */
- virtual BT::NodeStatus on_cancelled() {return BT::NodeStatus::SUCCESS;}
+ * @brief Function to perform some user-defined operation when the action is cancelled.
+ * @return BT::NodeStatus Returns SUCCESS by default, user may override return another value
+ */
+ virtual BT::NodeStatus on_cancelled()
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
/**
- * @brief The main override required by a BT action
- * @return BT::NodeStatus Status of tick execution
- */
+ * @brief The main override required by a BT action
+ * @return BT::NodeStatus Status of tick execution
+ */
BT::NodeStatus tick() override
{
// first step to be done only at the beginning of the Action
@@ -216,11 +217,13 @@ class BtActionNode : public BT::ActionNodeBase
// The following code corresponds to the "RUNNING" loop
if (rclcpp::ok() && !goal_result_available_) {
// user defined callback. May modify the value of "goal_updated_"
- on_wait_for_result();
+ on_wait_for_result(feedback_);
+
+ // reset feedback to avoid stale information
+ feedback_.reset();
auto goal_status = goal_handle_->get_status();
- if (
- goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
+ if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
{
goal_updated_ = false;
@@ -248,8 +251,7 @@ class BtActionNode : public BT::ActionNodeBase
}
}
} catch (const std::runtime_error & e) {
- if (
- e.what() == std::string("send_goal failed") ||
+ if (e.what() == std::string("send_goal failed") ||
e.what() == std::string("Goal was rejected by the action server"))
{
// Action related failure that should not fail the tree, but the node
@@ -283,19 +285,19 @@ class BtActionNode : public BT::ActionNodeBase
}
/**
- * @brief The other (optional) override required by a BT action. In this case, we
- * make sure to cancel the ROS2 action if it is still running.
- */
+ * @brief The other (optional) override required by a BT action. In this case, we
+ * make sure to cancel the ROS2 action if it is still running.
+ */
void halt() override
{
if (should_cancel_goal()) {
auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
- if (
- callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
+ if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(
- node_->get_logger(), "Failed to cancel action server for %s", action_name_.c_str());
+ node_->get_logger(),
+ "Failed to cancel action server for %s", action_name_.c_str());
}
}
@@ -304,9 +306,9 @@ class BtActionNode : public BT::ActionNodeBase
protected:
/**
- * @brief Function to check if current goal should be cancelled
- * @return bool True if current goal should be cancelled, false otherwise
- */
+ * @brief Function to check if current goal should be cancelled
+ * @return bool True if current goal should be cancelled, false otherwise
+ */
bool should_cancel_goal()
{
// Shut the node down if it is currently running
@@ -328,8 +330,8 @@ class BtActionNode : public BT::ActionNodeBase
}
/**
- * @brief Function to send new goal to action server
- */
+ * @brief Function to send new goal to action server
+ */
void send_new_goal()
{
goal_result_available_ = false;
@@ -340,8 +342,7 @@ class BtActionNode : public BT::ActionNodeBase
RCLCPP_DEBUG(
node_->get_logger(),
"Goal result for %s available, but it hasn't received the goal response yet. "
- "It's probably a goal result for the last goal request",
- action_name_.c_str());
+ "It's probably a goal result for the last goal request", action_name_.c_str());
return;
}
@@ -353,6 +354,11 @@ class BtActionNode : public BT::ActionNodeBase
result_ = result;
}
};
+ send_goal_options.feedback_callback =
+ [this](typename rclcpp_action::ClientGoalHandle::SharedPtr,
+ const std::shared_ptr feedback) {
+ feedback_ = feedback;
+ };
future_goal_handle_ = std::make_shared<
std::shared_future::SharedPtr>>(
@@ -361,11 +367,11 @@ class BtActionNode : public BT::ActionNodeBase
}
/**
- * @brief Function to check if the action server acknowledged a new goal
- * @param elapsed Duration since the last goal was sent and future goal handle has not completed.
- * After waiting for the future to complete, this value is incremented with the timeout value.
- * @return boolean True if future_goal_handle_ returns SUCCESS, False otherwise
- */
+ * @brief Function to check if the action server acknowledged a new goal
+ * @param elapsed Duration since the last goal was sent and future goal handle has not completed.
+ * After waiting for the future to complete, this value is incremented with the timeout value.
+ * @return boolean True if future_goal_handle_ returns SUCCESS, False otherwise
+ */
bool is_future_goal_handle_complete(std::chrono::milliseconds & elapsed)
{
auto remaining = server_timeout_ - elapsed;
@@ -399,8 +405,8 @@ class BtActionNode : public BT::ActionNodeBase
}
/**
- * @brief Function to increment recovery count on blackboard if this node wraps a recovery
- */
+ * @brief Function to increment recovery count on blackboard if this node wraps a recovery
+ */
void increment_recovery_count()
{
int recovery_count = 0;
@@ -409,10 +415,8 @@ class BtActionNode : public BT::ActionNodeBase
config().blackboard->template set("number_recoveries", recovery_count); // NOLINT
}
- std::string action_name_, original_action_name_;
- MirrorParam mirror_;
+ std::string action_name_;
typename std::shared_ptr> action_client_;
- static std::unordered_map>> action_client_list_;
// All ROS2 actions have a goal and a result
typename ActionT::Goal goal_;
@@ -421,6 +425,9 @@ class BtActionNode : public BT::ActionNodeBase
typename rclcpp_action::ClientGoalHandle::SharedPtr goal_handle_;
typename rclcpp_action::ClientGoalHandle::WrappedResult result_;
+ // To handle feedback from action server
+ std::shared_ptr feedback_;
+
// The node that will be used for any ROS operations
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
@@ -439,9 +446,6 @@ class BtActionNode : public BT::ActionNodeBase
rclcpp::Time time_goal_sent_;
};
-template
-std::unordered_map>> BtActionNode::action_client_list_;
-
-} // namespace mep3_behavior_tree
+} // namespace nav2_behavior_tree
-#endif // MEP3_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
+#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/canbus_send_action.hpp b/mep3_behavior/include/mep3_behavior/canbus_send_action.hpp
similarity index 97%
rename from mep3_behavior_tree/include/mep3_behavior_tree/canbus_send_action.hpp
rename to mep3_behavior/include/mep3_behavior/canbus_send_action.hpp
index 14913fc53..dfc396d82 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/canbus_send_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/canbus_send_action.hpp
@@ -25,7 +25,7 @@
#include "behaviortree_cpp_v3/action_node.h"
#include "rclcpp/rclcpp.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class CanbusSendAction : public BT::AsyncActionNode
{
@@ -81,6 +81,6 @@ BT::NodeStatus CanbusSendAction::tick()
return BT::NodeStatus::SUCCESS;
}
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__CANBUS_SEND_ACTION_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/delay_action.hpp b/mep3_behavior/include/mep3_behavior/delay_action.hpp
similarity index 97%
rename from mep3_behavior_tree/include/mep3_behavior_tree/delay_action.hpp
rename to mep3_behavior/include/mep3_behavior/delay_action.hpp
index facfb3560..67761afbb 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/delay_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/delay_action.hpp
@@ -23,7 +23,7 @@
#include "behaviortree_cpp_v3/action_node.h"
#include "rclcpp/rclcpp.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class DelayAction : public BT::ActionNodeBase
{
@@ -91,6 +91,6 @@ namespace mep3_behavior_tree
return BT::NodeStatus::RUNNING;
}
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__DELAY_ACTION_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/dynamixel_command_action.hpp b/mep3_behavior/include/mep3_behavior/dynamixel_command_action.hpp
similarity index 85%
rename from mep3_behavior_tree/include/mep3_behavior_tree/dynamixel_command_action.hpp
rename to mep3_behavior/include/mep3_behavior/dynamixel_command_action.hpp
index 8535950d4..58fa92c8b 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/dynamixel_command_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/dynamixel_command_action.hpp
@@ -20,20 +20,20 @@
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
-#include "mep3_behavior_tree/bt_action_node.hpp"
-#include "mep3_behavior_tree/table_specific_ports.hpp"
-#include "mep3_behavior_tree/team_color_strategy_mirror.hpp"
+#include "mep3_behavior/bt_action_node.hpp"
+#include "mep3_behavior/table_specific_ports.hpp"
+#include "mep3_behavior/team_color_strategy_mirror.hpp"
#include "mep3_msgs/action/dynamixel_command.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class DynamixelCommandAction
- : public mep3_behavior_tree::BtActionNode
+ : public mep3_behavior::BtActionNode
{
public:
explicit DynamixelCommandAction(
const std::string & xml_tag_name, const BT::NodeConfiguration & config)
- : mep3_behavior_tree::BtActionNode(
+ : mep3_behavior::BtActionNode(
xml_tag_name, "dynamixel_command", config)
{
if (!getInput("position", this->position))
@@ -52,11 +52,6 @@ class DynamixelCommandAction
if (table.length() > 0 && getInput("position_" + table, position_offset)) {
position += position_offset;
}
-
- if (g_StrategyMirror.angle_requires_mirroring(original_action_name_, mirror_)) {
- g_StrategyMirror.invert_angle(position);
- }
-
}
void on_tick() override;
@@ -116,6 +111,6 @@ BT::NodeStatus DynamixelCommandAction::on_cancelled()
return BT::NodeStatus::FAILURE;
}
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__DYNAMIXEL_COMMAND_ACTION_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/motion_command_action.hpp b/mep3_behavior/include/mep3_behavior/motion_command_action.hpp
similarity index 88%
rename from mep3_behavior_tree/include/mep3_behavior_tree/motion_command_action.hpp
rename to mep3_behavior/include/mep3_behavior/motion_command_action.hpp
index 421e4f592..e59c2547b 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/motion_command_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/motion_command_action.hpp
@@ -20,20 +20,20 @@
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
-#include "mep3_behavior_tree/bt_action_node.hpp"
-#include "mep3_behavior_tree/table_specific_ports.hpp"
-#include "mep3_behavior_tree/team_color_strategy_mirror.hpp"
+#include "mep3_behavior/bt_action_node.hpp"
+#include "mep3_behavior/table_specific_ports.hpp"
+#include "mep3_behavior/team_color_strategy_mirror.hpp"
#include "mep3_msgs/action/motion_command.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class MotionCommandAction
- : public mep3_behavior_tree::BtActionNode
+ : public mep3_behavior::BtActionNode
{
public:
explicit MotionCommandAction(
const std::string & xml_tag_name, const BT::NodeConfiguration & config)
- : mep3_behavior_tree::BtActionNode(
+ : mep3_behavior::BtActionNode(
xml_tag_name, "motion_command", config)
{
if (!getInput("command", this->command))
@@ -60,12 +60,12 @@ class MotionCommandAction
}
if (
- g_StrategyMirror.requires_mirroring(mirror_) && \
+ g_StrategyMirror.requires_mirroring() && \
command == "rotate_relative"
) {
g_StrategyMirror.invert_angle(value);
} else if (
- g_StrategyMirror.requires_mirroring(mirror_) && \
+ g_StrategyMirror.requires_mirroring() && \
command == "rotate_absolute"
) {
g_StrategyMirror.mirror_angle(value);
@@ -125,6 +125,6 @@ BT::NodeStatus MotionCommandAction::on_success()
return BT::NodeStatus::SUCCESS;
}
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__MOTION_COMMAND_ACTION_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/navigate_through_action.hpp b/mep3_behavior/include/mep3_behavior/navigate_through_action.hpp
similarity index 87%
rename from mep3_behavior_tree/include/mep3_behavior_tree/navigate_through_action.hpp
rename to mep3_behavior/include/mep3_behavior/navigate_through_action.hpp
index 24e34dad8..f84a1ebd6 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/navigate_through_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/navigate_through_action.hpp
@@ -22,20 +22,20 @@
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
-#include "mep3_behavior_tree/bt_action_node.hpp"
-#include "mep3_behavior_tree/pose_2d.hpp"
-#include "mep3_behavior_tree/table_specific_ports.hpp"
-#include "mep3_behavior_tree/team_color_strategy_mirror.hpp"
+#include "mep3_behavior/bt_action_node.hpp"
+#include "mep3_behavior/pose_2d.hpp"
+#include "mep3_behavior/table_specific_ports.hpp"
+#include "mep3_behavior/team_color_strategy_mirror.hpp"
#include "nav2_msgs/action/navigate_through_poses.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class NavigateThroughAction
- : public mep3_behavior_tree::BtActionNode
+ : public mep3_behavior::BtActionNode
{
public:
explicit NavigateThroughAction(const std::string &xml_tag_name, const BT::NodeConfiguration &config)
- : mep3_behavior_tree::BtActionNode(
+ : mep3_behavior::BtActionNode(
xml_tag_name, "navigate_through_poses", config)
{
}
@@ -77,7 +77,7 @@ namespace mep3_behavior_tree
<< table << "' detected" << std::endl;
}
- bool requires_mirroring = g_StrategyMirror.requires_mirroring(mirror_);
+ bool requires_mirroring = g_StrategyMirror.requires_mirroring();
goal_.behavior_tree = behavior_tree;
for (auto &pose : poses)
@@ -112,6 +112,6 @@ namespace mep3_behavior_tree
return BT::NodeStatus::SUCCESS;
}
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__NAVIGATE_THROUGH_ACTION_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/navigate_to_action.hpp b/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp
similarity index 86%
rename from mep3_behavior_tree/include/mep3_behavior_tree/navigate_to_action.hpp
rename to mep3_behavior/include/mep3_behavior/navigate_to_action.hpp
index 41c36970b..496e1e270 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/navigate_to_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/navigate_to_action.hpp
@@ -21,20 +21,20 @@
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
-#include "mep3_behavior_tree/bt_action_node.hpp"
-#include "mep3_behavior_tree/pose_2d.hpp"
-#include "mep3_behavior_tree/table_specific_ports.hpp"
-#include "mep3_behavior_tree/team_color_strategy_mirror.hpp"
+#include "mep3_behavior/bt_action_node.hpp"
+#include "mep3_behavior/pose_2d.hpp"
+#include "mep3_behavior/table_specific_ports.hpp"
+#include "mep3_behavior/team_color_strategy_mirror.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class NavigateToAction
- : public mep3_behavior_tree::BtActionNode
+ : public mep3_behavior::BtActionNode
{
public:
explicit NavigateToAction(const std::string &xml_tag_name, const BT::NodeConfiguration &config)
- : mep3_behavior_tree::BtActionNode(
+ : mep3_behavior::BtActionNode(
xml_tag_name, "navigate_to_pose", config)
{
if (!getInput("goal", this->goal))
@@ -49,7 +49,7 @@ namespace mep3_behavior_tree
goal += goal_offset;
}
- if (g_StrategyMirror.requires_mirroring(mirror_)) {
+ if (g_StrategyMirror.requires_mirroring()) {
g_StrategyMirror.mirror_pose(goal);
}
}
@@ -109,6 +109,6 @@ namespace mep3_behavior_tree
return BT::NodeStatus::SUCCESS;
}
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__NAVIGATE_TO_ACTION_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/pose_2d.hpp b/mep3_behavior/include/mep3_behavior/pose_2d.hpp
similarity index 100%
rename from mep3_behavior_tree/include/mep3_behavior_tree/pose_2d.hpp
rename to mep3_behavior/include/mep3_behavior/pose_2d.hpp
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/remove_obstacle_action.hpp b/mep3_behavior/include/mep3_behavior/remove_obstacle_action.hpp
similarity index 96%
rename from mep3_behavior_tree/include/mep3_behavior_tree/remove_obstacle_action.hpp
rename to mep3_behavior/include/mep3_behavior/remove_obstacle_action.hpp
index 64da344fe..37b7b6b9d 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/remove_obstacle_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/remove_obstacle_action.hpp
@@ -24,7 +24,7 @@
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class RemoveObstacleAction : public BT::AsyncActionNode
{
@@ -66,6 +66,6 @@ namespace mep3_behavior_tree
return BT::NodeStatus::SUCCESS;
}
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__REMOVE_OBSTACLE_ACTION_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/scoreboard_task_action.hpp b/mep3_behavior/include/mep3_behavior/scoreboard_task_action.hpp
similarity index 96%
rename from mep3_behavior_tree/include/mep3_behavior_tree/scoreboard_task_action.hpp
rename to mep3_behavior/include/mep3_behavior/scoreboard_task_action.hpp
index d1648352b..810dd3888 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/scoreboard_task_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/scoreboard_task_action.hpp
@@ -23,7 +23,7 @@
#include "behaviortree_cpp_v3/action_node.h"
#include "rclcpp/rclcpp.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class ScoreboardTaskAction : public BT::AsyncActionNode
{
@@ -70,6 +70,6 @@ BT::NodeStatus ScoreboardTaskAction::tick()
return BT::NodeStatus::SUCCESS;
}
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__SCOREBOARD_TASK_ACTION_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/set_shared_blackboard_action.hpp b/mep3_behavior/include/mep3_behavior/set_shared_blackboard_action.hpp
similarity index 97%
rename from mep3_behavior_tree/include/mep3_behavior_tree/set_shared_blackboard_action.hpp
rename to mep3_behavior/include/mep3_behavior/set_shared_blackboard_action.hpp
index 334af2648..08dd9cecd 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/set_shared_blackboard_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/set_shared_blackboard_action.hpp
@@ -25,7 +25,7 @@
#include "behaviortree_cpp_v3/action_node.h"
#include "rclcpp/rclcpp.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
using KeyValueT = diagnostic_msgs::msg::KeyValue;
@@ -73,6 +73,6 @@ namespace mep3_behavior_tree
return BT::NodeStatus::SUCCESS;
}
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__SET_SHARED_BLACKBOARD_ACTION_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/table_specific_ports.hpp b/mep3_behavior/include/mep3_behavior/table_specific_ports.hpp
similarity index 95%
rename from mep3_behavior_tree/include/mep3_behavior_tree/table_specific_ports.hpp
rename to mep3_behavior/include/mep3_behavior/table_specific_ports.hpp
index 668a9ce9f..019d9c802 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/table_specific_ports.hpp
+++ b/mep3_behavior/include/mep3_behavior/table_specific_ports.hpp
@@ -18,7 +18,7 @@
#include
#include
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class InputPortNameFactory {
@@ -42,6 +42,6 @@ class InputPortNameFactory {
// Globally shared singleton
InputPortNameFactory g_InputPortNameFactory;
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__TABLE_SPECIFIC_PORTS_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/task_sequence_control.hpp b/mep3_behavior/include/mep3_behavior/task_sequence_control.hpp
similarity index 98%
rename from mep3_behavior_tree/include/mep3_behavior_tree/task_sequence_control.hpp
rename to mep3_behavior/include/mep3_behavior/task_sequence_control.hpp
index b322f8d77..ac886b7c5 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/task_sequence_control.hpp
+++ b/mep3_behavior/include/mep3_behavior/task_sequence_control.hpp
@@ -20,7 +20,7 @@
#include "behaviortree_cpp_v3/control_node.h"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class TaskSequenceControl : public BT::ControlNode
{
@@ -106,5 +106,5 @@ namespace mep3_behavior_tree
// DO NOT reset current_child_idx_ on halt
BT::ControlNode::halt();
}
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__TASK_SEQUENCE_CONTROL_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/team_color_strategy_mirror.hpp b/mep3_behavior/include/mep3_behavior/team_color_strategy_mirror.hpp
similarity index 53%
rename from mep3_behavior_tree/include/mep3_behavior_tree/team_color_strategy_mirror.hpp
rename to mep3_behavior/include/mep3_behavior/team_color_strategy_mirror.hpp
index 3110e2471..03d794994 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/team_color_strategy_mirror.hpp
+++ b/mep3_behavior/include/mep3_behavior/team_color_strategy_mirror.hpp
@@ -24,21 +24,15 @@
#include
#include "behaviortree_cpp_v3/action_node.h"
-#include "mep3_behavior_tree/pose_2d.hpp"
+#include "mep3_behavior/pose_2d.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
enum TeamColor {
Purple,
Yellow
};
-enum MirrorParam {
- True,
- False,
- Default
-};
-
class StrategyMirror {
public:
StrategyMirror() {
@@ -51,14 +45,6 @@ class StrategyMirror {
this->color = StrategyMirror::string_to_color_enum(color);
}
- void set_angle_blacklist(const std::vector& list) {
- this->mirror_angle_blacklist = list;
- }
-
- void set_name_blacklist(const std::vector& list) {
- this->mirror_name_blacklist = list;
- }
-
void set_default_color(const std::string& color) {
this->default_color = StrategyMirror::string_to_color_enum(color);
}
@@ -78,73 +64,11 @@ class StrategyMirror {
}
}
- void remap_server_name(std::string& server_name) {
- if (this->color == this->default_color)
- return;
- const auto re_left = std::regex("left");
- const auto re_right = std::regex("right");
- const auto re_placeholder = std::regex("PLACEHOLDER");
- server_name = std::regex_replace(server_name, re_right, "PLACEHOLDER");
- server_name = std::regex_replace(server_name, re_left, "right");
- server_name = std::regex_replace(server_name, re_placeholder, "left");
- }
-
- bool requires_mirroring(
- const MirrorParam mirror
- ) {
- switch (mirror) {
- case MirrorParam::True:
- return true;
- case MirrorParam::False:
- return false;
- default:
- if (this->color == this->default_color)
- return false;
- else
- return true;
- }
- }
-
- bool server_name_requires_mirroring(
- const std::string& server_name,
- const MirrorParam mirror
- ) {
- switch (mirror) {
- case MirrorParam::True:
- return true;
- case MirrorParam::False:
+ bool requires_mirroring() {
+ if (this->color == this->default_color)
return false;
- default:
- if (this->color == this->default_color)
- return false;
- // Return true if not found in blacklist array
- return std::find(
- this->mirror_name_blacklist.begin(),
- this->mirror_name_blacklist.end(),
- StrategyMirror::strip_server_name(server_name)
- ) == this->mirror_name_blacklist.end();
- }
- }
-
- bool angle_requires_mirroring(
- const std::string& server_name,
- const MirrorParam mirror
- ) {
- switch (mirror) {
- case MirrorParam::True:
+ else
return true;
- case MirrorParam::False:
- return false;
- default:
- if (this->color == this->default_color)
- return false;
- // Return true if not found in blacklist array
- return std::find(
- this->mirror_angle_blacklist.begin(),
- this->mirror_angle_blacklist.end(),
- StrategyMirror::strip_server_name(server_name)
- ) == this->mirror_angle_blacklist.end();
- }
}
template
@@ -162,28 +86,6 @@ class StrategyMirror {
}
}
- template
- void mirror_resistance(Number& resistance) {
- switch (resistance) {
- case RESISTANCE_VALUE_YELLOW:
- resistance = RESISTANCE_VALUE_PURPLE;
- return;
- case RESISTANCE_VALUE_PURPLE:
- resistance = RESISTANCE_VALUE_YELLOW;
- return;
- }
- }
-
- static MirrorParam string_to_mirror_enum(const std::string& mirror) {
- if (mirror == "false") {
- return MirrorParam::False;
- } else if (mirror == "true") {
- return MirrorParam::True;
- } else {
- return MirrorParam::Default;
- }
- }
-
private:
static TeamColor string_to_color_enum(const std::string& color) {
if (color == "purple") {
@@ -206,8 +108,6 @@ class StrategyMirror {
}
TeamColor color, default_color;
- std::vector mirror_angle_blacklist, mirror_name_blacklist;
-
};
// Globally shared singleton
@@ -241,6 +141,6 @@ class DefaultTeamColorCondition : public BT::ConditionNode
}
};
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__TEAM_COLOR_STRATEGY_MIRROR_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/vacuum_pump_command_action.hpp b/mep3_behavior/include/mep3_behavior/vacuum_pump_command_action.hpp
similarity index 87%
rename from mep3_behavior_tree/include/mep3_behavior_tree/vacuum_pump_command_action.hpp
rename to mep3_behavior/include/mep3_behavior/vacuum_pump_command_action.hpp
index 19bf0be28..7fc29253d 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/vacuum_pump_command_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/vacuum_pump_command_action.hpp
@@ -19,19 +19,19 @@
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
-#include "mep3_behavior_tree/bt_action_node.hpp"
-#include "mep3_behavior_tree/team_color_strategy_mirror.hpp"
+#include "mep3_behavior/bt_action_node.hpp"
+#include "mep3_behavior/team_color_strategy_mirror.hpp"
#include "mep3_msgs/action/vacuum_pump_command.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class VacuumPumpCommandAction
- : public mep3_behavior_tree::BtActionNode
+ : public mep3_behavior::BtActionNode
{
public:
explicit VacuumPumpCommandAction(
const std::string & xml_tag_name, const BT::NodeConfiguration & config)
- : mep3_behavior_tree::BtActionNode(
+ : mep3_behavior::BtActionNode(
xml_tag_name, "vacuum_pump_command", config)
{
if (!getInput("connect", this->connect))
@@ -82,6 +82,6 @@ BT::NodeStatus VacuumPumpCommandAction::on_cancelled()
return BT::NodeStatus::FAILURE;
}
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__VACUUM_PUMP_COMMAND_ACTION_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/wait_match_start_action.hpp b/mep3_behavior/include/mep3_behavior/wait_match_start_action.hpp
similarity index 97%
rename from mep3_behavior_tree/include/mep3_behavior_tree/wait_match_start_action.hpp
rename to mep3_behavior/include/mep3_behavior/wait_match_start_action.hpp
index 5fc5fee33..6b92e8afe 100644
--- a/mep3_behavior_tree/include/mep3_behavior_tree/wait_match_start_action.hpp
+++ b/mep3_behavior/include/mep3_behavior/wait_match_start_action.hpp
@@ -22,7 +22,7 @@
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int8.hpp"
-namespace mep3_behavior_tree
+namespace mep3_behavior
{
class WaitMatchStartAction : public BT::SyncActionNode
{
@@ -77,6 +77,6 @@ BT::NodeStatus WaitMatchStartAction::tick()
return BT::NodeStatus::SUCCESS;
}
-} // namespace mep3_behavior_tree
+} // namespace mep3_behavior
#endif // MEP3_BEHAVIOR_TREE__WAIT_MATCH_START_ACTION_HPP_
diff --git a/mep3_behavior_tree/package.xml b/mep3_behavior/package.xml
similarity index 97%
rename from mep3_behavior_tree/package.xml
rename to mep3_behavior/package.xml
index 68f34a650..58bc380d7 100644
--- a/mep3_behavior_tree/package.xml
+++ b/mep3_behavior/package.xml
@@ -1,7 +1,7 @@
- mep3_behavior_tree
+ mep3_behavior
0.1.0
Memristor Eurobot Platform Strategy Planner
memristor
diff --git a/mep3_behavior_tree/src/mep3_behavior_tree.cpp b/mep3_behavior/src/mep3_behavior_tree.cpp
similarity index 54%
rename from mep3_behavior_tree/src/mep3_behavior_tree.cpp
rename to mep3_behavior/src/mep3_behavior_tree.cpp
index 9cf01436a..63e5adc95 100644
--- a/mep3_behavior_tree/src/mep3_behavior_tree.cpp
+++ b/mep3_behavior/src/mep3_behavior_tree.cpp
@@ -13,7 +13,7 @@
// limitations under the License.
#ifndef ASSETS_DIRECTORY
-#define ASSETS_DIRECTORY "mep3_behavior_tree/assets"
+#define ASSETS_DIRECTORY "mep3_behavior/assets"
#endif
#include
@@ -26,25 +26,22 @@
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behaviortree_cpp_v3/utils/shared_library.h"
#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h"
-#include "mep3_behavior_tree/dynamixel_command_action.hpp"
-#include "mep3_behavior_tree/lift_command_action.hpp"
-#include "mep3_behavior_tree/motion_command_action.hpp"
-#include "mep3_behavior_tree/navigate_to_action.hpp"
-#include "mep3_behavior_tree/precise_navigate_to_action.hpp"
-#include "mep3_behavior_tree/resistance_meter_action.hpp"
-#include "mep3_behavior_tree/table_specific_ports.hpp"
-#include "mep3_behavior_tree/team_color_strategy_mirror.hpp"
-#include "mep3_behavior_tree/scoreboard_task_action.hpp"
-#include "mep3_behavior_tree/task_sequence_control.hpp"
-#include "mep3_behavior_tree/vacuum_pump_command_action.hpp"
-#include "mep3_behavior_tree/wait_match_start_action.hpp"
-#include "mep3_behavior_tree/delay_action.hpp"
-#include "mep3_behavior_tree/canbus_send_action.hpp"
-#include "mep3_behavior_tree/set_shared_blackboard_action.hpp"
-#include "mep3_behavior_tree/blackboard_control_flow.hpp"
-#include "mep3_behavior_tree/navigate_through_action.hpp"
-#include "mep3_behavior_tree/add_obstacle_action.hpp"
-#include "mep3_behavior_tree/remove_obstacle_action.hpp"
+#include "mep3_behavior/dynamixel_command_action.hpp"
+#include "mep3_behavior/motion_command_action.hpp"
+#include "mep3_behavior/navigate_to_action.hpp"
+#include "mep3_behavior/table_specific_ports.hpp"
+#include "mep3_behavior/team_color_strategy_mirror.hpp"
+#include "mep3_behavior/scoreboard_task_action.hpp"
+#include "mep3_behavior/task_sequence_control.hpp"
+#include "mep3_behavior/vacuum_pump_command_action.hpp"
+#include "mep3_behavior/wait_match_start_action.hpp"
+#include "mep3_behavior/delay_action.hpp"
+#include "mep3_behavior/canbus_send_action.hpp"
+#include "mep3_behavior/set_shared_blackboard_action.hpp"
+#include "mep3_behavior/blackboard_control_flow.hpp"
+#include "mep3_behavior/navigate_through_action.hpp"
+#include "mep3_behavior/add_obstacle_action.hpp"
+#include "mep3_behavior/remove_obstacle_action.hpp"
#include "rclcpp/rclcpp.hpp"
using KeyValueT = diagnostic_msgs::msg::KeyValue;
@@ -59,7 +56,7 @@ int main(int argc, char **argv)
// Initialize ROS node
rclcpp::init(argc, argv);
- auto node = rclcpp::Node::make_shared("mep3_behavior_tree");
+ auto node = rclcpp::Node::make_shared("mep3_behavior");
// Initialize blackboard
auto blackboard = BT::Blackboard::create();
@@ -105,36 +102,16 @@ int main(int argc, char **argv)
std::vector({})
);
node->get_parameter("predefined_tables", predefined_tables);
- mep3_behavior_tree::g_InputPortNameFactory.set_names(
+ mep3_behavior::g_InputPortNameFactory.set_names(
predefined_tables.as_string_array()
);
// Set color
node->declare_parameter("color", "purple");
auto color = node->get_parameter("color");
- mep3_behavior_tree::g_StrategyMirror.set_color(color.as_string());
+ mep3_behavior::g_StrategyMirror.set_color(color.as_string());
blackboard->set("color", color.as_string());
- // Get mirroring blacklists
- node->declare_parameter>("mirror_angle_blacklist", std::vector({}));
- node->declare_parameter>("mirror_name_blacklist", std::vector({}));
- rclcpp::Parameter mirror_angle_blacklist(
- "mirror_angle_blacklist",
- std::vector({})
- );
- rclcpp::Parameter mirror_name_blacklist(
- "mirror_name_blacklist",
- std::vector({})
- );
- node->get_parameter("mirror_angle_blacklist", mirror_angle_blacklist);
- node->get_parameter("mirror_name_blacklist", mirror_name_blacklist);
- mep3_behavior_tree::g_StrategyMirror.set_angle_blacklist(
- mirror_angle_blacklist.as_string_array()
- );
- mep3_behavior_tree::g_StrategyMirror.set_name_blacklist(
- mirror_name_blacklist.as_string_array()
- );
-
blackboard->set(
"bt_loop_duration",
std::chrono::milliseconds(10));
@@ -152,64 +129,55 @@ int main(int argc, char **argv)
loader.getOSName("nav2_recovery_node_bt_node")
);
- factory.registerNodeType(
+ factory.registerNodeType(
"CanbusSend"
);
- factory.registerNodeType(
+ factory.registerNodeType(
"SetSharedBlackboard"
);
- factory.registerNodeType(
+ factory.registerNodeType(
"CompareBlackboard"
);
- factory.registerNodeType(
+ factory.registerNodeType(
"Blackboard"
);
- factory.registerNodeType(
+ factory.registerNodeType(
"Pass"
);
- factory.registerNodeType(
+ factory.registerNodeType(
"Wait"
);
- factory.registerNodeType(
+ factory.registerNodeType(
"Dynamixel"
);
- factory.registerNodeType(
+ factory.registerNodeType(
"Motion"
);
- factory.registerNodeType(
+ factory.registerNodeType(
"Navigate"
);
- factory.registerNodeType(
- "PreciseNavigate"
- );
- factory.registerNodeType(
+ factory.registerNodeType(
"VacuumPump"
);
- factory.registerNodeType(
- "ResistanceMeter"
- );
- factory.registerNodeType(
+ factory.registerNodeType(
"ScoreboardTask"
);
- factory.registerNodeType(
+ factory.registerNodeType(
"WaitMatchStart"
);
- factory.registerNodeType(
- "Lift"
- );
- factory.registerNodeType(
+ factory.registerNodeType(
"DefaultTeamColor"
);
- factory.registerNodeType(
+ factory.registerNodeType(
"TaskSequence"
);
- factory.registerNodeType(
+ factory.registerNodeType(
"NavigateThrough"
);
- factory.registerNodeType(
+ factory.registerNodeType(
"AddObstacle"
);
- factory.registerNodeType(
+ factory.registerNodeType(
"RemoveObstacle"
);
diff --git a/mep3_behavior_tree/assets/strategies/big/.gitkeep b/mep3_behavior_tree/assets/strategies/big/.gitkeep
deleted file mode 100644
index e69de29bb..000000000
diff --git a/mep3_behavior_tree/assets/strategies/big/florian.xml b/mep3_behavior_tree/assets/strategies/big/florian.xml
deleted file mode 100644
index 125bc92ba..000000000
--- a/mep3_behavior_tree/assets/strategies/big/florian.xml
+++ /dev/null
@@ -1,318 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- purple, yellow
-
-
- [deg]
- 0 - success, 1 - timeout, 2 - other error
- Action server name
- [s]
- [deg]
- [deg/s]
-
-
- purple, yellow
-
-
- [cm]
- 0 - success, 1 - timeout, 2 - other error
- Action server name
- [s]
- [cm]
- [cm/s]
-
-
- [rad/s^2]
- [m/s^2]
- [forward/rotate_relative/rotate_absolute]
- [success/drift]
- value to pass to motion function, in meters or radians
- [rad/s]
- [m/s]
-
-
-
-
-
-
- Behavior tree
- A target pose in format "x;y;theta".
-
-
- [Ohms]
- Action server name
- [%]
-
-
- points scored, can be negative
- unique task name
-
-
-
- 0 - disconnect, 1 - connected
- 0 - disconnected, 1 - connected, 2 - couldn't disconnect, 3 - couldn't connect, 4 - other
- Action server name
-
-
- 0 = unarmed; 1 = armed; 2 = started
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/mep3_behavior_tree/assets/strategies/big/florian_homologation.xml b/mep3_behavior_tree/assets/strategies/big/florian_homologation.xml
deleted file mode 100644
index 90bfd7d3a..000000000
--- a/mep3_behavior_tree/assets/strategies/big/florian_homologation.xml
+++ /dev/null
@@ -1,51 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/mep3_behavior_tree/assets/strategies/big/test_stuck.xml b/mep3_behavior_tree/assets/strategies/big/test_stuck.xml
deleted file mode 100644
index 68d68f001..000000000
--- a/mep3_behavior_tree/assets/strategies/big/test_stuck.xml
+++ /dev/null
@@ -1,42 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/mep3_behavior_tree/assets/strategies/resistance.xml b/mep3_behavior_tree/assets/strategies/resistance.xml
deleted file mode 100644
index 2c145dd98..000000000
--- a/mep3_behavior_tree/assets/strategies/resistance.xml
+++ /dev/null
@@ -1,317 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-<<<<<<< HEAD
-
-=======
-
->>>>>>> main
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-<<<<<<< HEAD
-<<<<<<< HEAD:mep3_behavior_tree/assets/strategies/resistance.xml
-=======
->>>>>>> main
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-<<<<<<< HEAD
-=======
->>>>>>> main:mep3_behavior_tree/assets/strategies/homologation_resistance.xml
-=======
->>>>>>> main
-
-
-
-
-<<<<<<< HEAD
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-=======
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
->>>>>>> main
-
-
-
-
-<<<<<<< HEAD
-=======
-
-
-
->>>>>>> main
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- purple, yellow
-
-
- [deg]
- 0 - success, 1 - timeout, 2 - other error
- Action server name
- [s]
- [deg]
- [deg/s]
-
-
- purple, yellow
-
-
- [cm]
- 0 - success, 1 - timeout, 2 - other error
- Action server name
- [s]
- [cm]
- [cm/s]
-
-
- [rad/s^2]
- [m/s^2]
- [forward/rotate_relative/rotate_absolute]
- [success/drift]
- value to pass to motion function, in meters or radians
- [rad/s]
- [m/s]
-
-
-
-
-
-
- Behavior tree
- A target pose in format "x;y;theta".
-
-
- [Ohms]
- Action server name
- [%]
-
-
- points scored, can be negative
- unique task name
-
-
-
- 0 - disconnect, 1 - connected
- 0 - disconnected, 1 - connected, 2 - couldn't disconnect, 3 - couldn't connect, 4 - other
- Action server name
-
-
- 0 = unarmed; 1 = armed; 2 = started
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/mep3_behavior_tree/assets/strategies/small/.gitkeep b/mep3_behavior_tree/assets/strategies/small/.gitkeep
deleted file mode 100644
index e69de29bb..000000000
diff --git a/mep3_behavior_tree/assets/strategies/small/kosta.xml b/mep3_behavior_tree/assets/strategies/small/kosta.xml
deleted file mode 100644
index 518559523..000000000
--- a/mep3_behavior_tree/assets/strategies/small/kosta.xml
+++ /dev/null
@@ -1,680 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- purple, yellow
-
-
- [deg]
- 0 - success, 1 - timeout, 2 - other error
- Action server name
- [s]
- [deg]
- [deg/s]
-
-
- purple, yellow
-
-
- [cm]
- 0 - success, 1 - timeout, 2 - other error
- Action server name
- [s]
- [cm]
- [cm/s]
-
-
- [rad/s^2]
- [m/s^2]
- [forward/rotate_relative/rotate_absolute]
- [success/drift]
- value to pass to motion function, in meters or radians
- [rad/s]
- [m/s]
-
-
-
-
-
-
- Behavior tree
- A target pose in format "x;y;theta".
-
-
- [Ohms]
- Action server name
- [%]
-
-
- points scored, can be negative
- unique task name
-
-
-
- 0 - disconnect, 1 - connected
- 0 - disconnected, 1 - connected, 2 - couldn't disconnect, 3 - couldn't connect, 4 - other
- Action server name
-
-
- 0 = unarmed; 1 = armed; 2 = started
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/mep3_behavior_tree/assets/strategies/small/kosta_homologation.xml b/mep3_behavior_tree/assets/strategies/small/kosta_homologation.xml
deleted file mode 100644
index 7286038aa..000000000
--- a/mep3_behavior_tree/assets/strategies/small/kosta_homologation.xml
+++ /dev/null
@@ -1,53 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/mep3_behavior_tree/assets/strategies/small/kosta_yellow.xml b/mep3_behavior_tree/assets/strategies/small/kosta_yellow.xml
deleted file mode 100644
index dc8fa0190..000000000
--- a/mep3_behavior_tree/assets/strategies/small/kosta_yellow.xml
+++ /dev/null
@@ -1,757 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- purple, yellow
-
-
- [deg]
- 0 - success, 1 - timeout, 2 - other error
- Action server name
- [s]
- [deg]
- [deg/s]
-
-
- purple, yellow
-
-
- [cm]
- 0 - success, 1 - timeout, 2 - other error
- Action server name
- [s]
- [cm]
- [cm/s]
-
-
- [rad/s^2]
- [m/s^2]
- [forward/rotate_relative/rotate_absolute]
- [success/drift]
- value to pass to motion function, in meters or radians
- [rad/s]
- [m/s]
-
-
-
-
-
-
- Behavior tree
- A target pose in format "x;y;theta".
-
-
- [Ohms]
- Action server name
- [%]
-
-
- points scored, can be negative
- unique task name
-
-
-
- 0 - disconnect, 1 - connected
- 0 - disconnected, 1 - connected, 2 - couldn't disconnect, 3 - couldn't connect, 4 - other
- Action server name
-
-
- 0 = unarmed; 1 = armed; 2 = started
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/mep3_behavior_tree/assets/tasks/.gitkeep b/mep3_behavior_tree/assets/tasks/.gitkeep
deleted file mode 100644
index e69de29bb..000000000
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/lift_command_action.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/lift_command_action.hpp
deleted file mode 100644
index 0ae9dfe89..000000000
--- a/mep3_behavior_tree/include/mep3_behavior_tree/lift_command_action.hpp
+++ /dev/null
@@ -1,94 +0,0 @@
-// Copyright 2021 Memristor Robotics
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MEP3_BEHAVIOR_TREE__LIFT_COMMAND_ACTION_HPP_
-#define MEP3_BEHAVIOR_TREE__LIFT_COMMAND_ACTION_HPP_
-
-#include
-
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-#include "mep3_behavior_tree/bt_action_node.hpp"
-#include "mep3_msgs/action/dynamixel_command.hpp"
-
-#define GEAR_RADIUS_CM 15.75
-#define RAD_TO_DEG 180 / 3.14159
-
-namespace mep3_behavior_tree
-{
-class LiftCommandAction
- : public mep3_behavior_tree::BtActionNode
-{
-public:
- explicit LiftCommandAction(
- const std::string & xml_tag_name, const BT::NodeConfiguration & config)
- : mep3_behavior_tree::BtActionNode(
- xml_tag_name, "lift_command", config)
- {
- }
-
- void on_tick() override;
- BT::NodeStatus on_success() override;
- BT::NodeStatus on_aborted() override;
- BT::NodeStatus on_cancelled() override;
-
- static BT::PortsList providedPorts()
- {
- return providedBasicPorts(
- {
- BT::InputPort<_Float64>("height"),
- BT::InputPort<_Float64>("velocity"),
- BT::InputPort<_Float64>("tolerance"),
- BT::InputPort<_Float64>("timeout"),
- BT::OutputPort("result")
- });
- }
-};
-
-void LiftCommandAction::on_tick()
-{
- _Float64 height, velocity, tolerance, timeout;
-
- getInput("height", height);
- getInput("velocity", velocity);
- getInput("tolerance", tolerance);
- getInput("timeout", timeout);
-
- goal_.position = height / GEAR_RADIUS_CM * RAD_TO_DEG;
- goal_.velocity = velocity / GEAR_RADIUS_CM * RAD_TO_DEG;
- goal_.tolerance = tolerance / GEAR_RADIUS_CM * RAD_TO_DEG;
- goal_.timeout = timeout;
-}
-
-BT::NodeStatus LiftCommandAction::on_success()
-{
- setOutput("result", 0);
- return BT::NodeStatus::SUCCESS;
-}
-
-BT::NodeStatus LiftCommandAction::on_aborted()
-{
- setOutput("result", 2);
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus LiftCommandAction::on_cancelled()
-{
- setOutput("result", 2);
- return BT::NodeStatus::FAILURE;
-}
-
-} // namespace mep3_behavior_tree
-
-#endif // MEP3_BEHAVIOR_TREE__LIFT_COMMAND_ACTION_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/precise_navigate_to_action.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/precise_navigate_to_action.hpp
deleted file mode 100644
index 96a5f47eb..000000000
--- a/mep3_behavior_tree/include/mep3_behavior_tree/precise_navigate_to_action.hpp
+++ /dev/null
@@ -1,114 +0,0 @@
-// Copyright 2021 Memristor Robotics
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MEP3_BEHAVIOR_TREE__PRECISE_NAVIGATE_TO_ACTION_HPP_
-#define MEP3_BEHAVIOR_TREE__PRECISE_NAVIGATE_TO_ACTION_HPP_
-
-#include
-#include
-#include
-
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-#include "mep3_behavior_tree/bt_action_node.hpp"
-#include "mep3_behavior_tree/pose_2d.hpp"
-#include "nav2_msgs/action/navigate_to_pose.hpp"
-
-namespace mep3_behavior_tree
-{
- class PreciseNavigateToAction
- : public mep3_behavior_tree::BtActionNode
- {
- public:
- explicit PreciseNavigateToAction(
- const std::string &xml_tag_name, const BT::NodeConfiguration &config)
- : mep3_behavior_tree::BtActionNode(
- xml_tag_name, "precise_navigate_to_pose", config)
- {
- if (!getInput("goal", this->goal))
- throw BT::RuntimeError(
- "PreciseNavigate action requires 'goal' argument"
- );
- getInput("behavior_tree", this->behavior_tree);
-
- // Warning: might break for a still undetermined reason
- std::string table = this->config().blackboard->get("table");
- BT::Pose2D goal_offset;
- if (table.length() > 0 && getInput("goal_" + table, goal_offset)) {
- goal += goal_offset;
- }
-
- if (g_StrategyMirror.requires_mirroring(mirror_)) {
- g_StrategyMirror.mirror_pose(goal);
- }
- }
-
- void on_tick() override;
- BT::NodeStatus on_success() override;
-
- static BT::PortsList providedPorts()
- {
- // Static parameters
- BT::PortsList port_list = providedBasicPorts({
- BT::InputPort("goal"),
- BT::InputPort("behavior_tree")
- });
-
- // Dynamic parameters
- for (std::string table : g_InputPortNameFactory.get_names()) {
- port_list.insert(
- BT::InputPort("goal_" + table)
- );
- }
-
- return port_list;
- }
-
- private:
- BT::Pose2D goal;
- std::string behavior_tree;
- };
-
- void PreciseNavigateToAction::on_tick()
- {
- std::cout << "Precise navigating to x=" << goal.x \
- << " y=" << goal.y \
- << " θ=" << goal.theta << "°" << std::endl;
-
- goal_.pose.header.frame_id = "map";
- goal_.pose.header.stamp = node_->get_clock()->now();
-
- // Position
- goal_.pose.pose.position.x = goal.x;
- goal_.pose.pose.position.y = goal.y;
- goal_.behavior_tree = behavior_tree;
-
- // Orientation (yaw)
- // Convert deg to rad
- double theta = goal.theta * M_PI / 180.0;
- // https://math.stackexchange.com/a/1972382
- goal_.pose.pose.orientation.w = std::cos(theta / 2.0);
- goal_.pose.pose.orientation.z = std::sin(theta / 2.0);
- }
-
- BT::NodeStatus PreciseNavigateToAction::on_success()
- {
- std::cout << "Navigation succesful " << std::endl;
-
- return BT::NodeStatus::SUCCESS;
- }
-
-} // namespace mep3_behavior_tree
-
-#endif // MEP3_BEHAVIOR_TREE__PRECISE_NAVIGATE_TO_ACTION_HPP_
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/resistance_meter_action.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/resistance_meter_action.hpp
deleted file mode 100644
index 22cd28244..000000000
--- a/mep3_behavior_tree/include/mep3_behavior_tree/resistance_meter_action.hpp
+++ /dev/null
@@ -1,127 +0,0 @@
-// Copyright 2021 Memristor Robotics
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MEP3_BEHAVIOR_TREE__RESISTANCE_METER_ACTION_HPP_
-#define MEP3_BEHAVIOR_TREE__RESISTANCE_METER_ACTION_HPP_
-
-#include
-#include
-
-#include "mep3_behavior_tree/bt_action_node.hpp"
-#include "mep3_behavior_tree/table_specific_ports.hpp"
-#include "mep3_behavior_tree/team_color_strategy_mirror.hpp"
-#include "mep3_msgs/action/resistance_meter.hpp"
-
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-
-namespace mep3_behavior_tree
-{
-
- class ResistanceMeterAction
- : public mep3_behavior_tree::BtActionNode
- {
- public:
- explicit ResistanceMeterAction(
- const std::string &name,
- const BT::NodeConfiguration &config) :
- mep3_behavior_tree::BtActionNode(
- name, "resistance_meter", config
- )
- {
- if (!getInput("resistance", this->expected))
- throw BT::RuntimeError(
- "ResistanceMeter action requires 'resistance' argument"
- );
- if (!getInput("tolerance", this->tolerance))
- throw BT::RuntimeError(
- "ResistanceMeter action requires 'tolerance' argument"
- );
-
- tolerance /= 100.0;
-
- std::string table = this->config().blackboard->get("table");
- int32_t resistance_offset;
- if (table.length() > 0 && getInput("resistance_" + table, resistance_offset)) {
- expected += resistance_offset;
- }
-
- if (g_StrategyMirror.requires_mirroring(mirror_)) {
- g_StrategyMirror.mirror_resistance(expected);
- }
- }
-
- void on_tick() override;
- BT::NodeStatus on_success() override;
- BT::NodeStatus on_aborted() override;
- BT::NodeStatus on_cancelled() override;
-
- static BT::PortsList providedPorts()
- {
- // Static parameters
- BT::PortsList port_list = providedBasicPorts({
- BT::InputPort("resistance"),
- BT::InputPort<_Float32>("tolerance"),
- });
-
- // Dynamic parameters
- for (std::string table : g_InputPortNameFactory.get_names()) {
- port_list.insert(
- BT::InputPort("resistance_" + table)
- );
- }
-
- return port_list;
- }
-
- private:
- int32_t measured, expected;
- _Float32 tolerance;
- };
-
- void ResistanceMeterAction::on_tick()
- {
- std::cout << "Measuring resistance on '" \
- << this->action_name_ << "'" << std::endl;
- }
-
- BT::NodeStatus ResistanceMeterAction::on_success()
- {
- int32_t measured = result_.result->resistance;
- std::cout << "Expected " << expected << "±" \
- << tolerance * expected << "Ω, " \
- << "measured " << 0 << "Ω" << std::endl;
- if (
- measured >= expected * (1.0 - tolerance) && \
- measured <= expected * (1.0 + tolerance)
- ) {
- return BT::NodeStatus::SUCCESS;
- } else {
- return BT::NodeStatus::FAILURE;
- }
- }
-
- BT::NodeStatus ResistanceMeterAction::on_aborted()
- {
- return BT::NodeStatus::FAILURE;
- }
-
- BT::NodeStatus ResistanceMeterAction::on_cancelled()
- {
- return BT::NodeStatus::FAILURE;
- }
-
-} // namespace mep3_behavior_tree
-
-#endif // MEP3_BEHAVIOR_TREE__RESISTANCE_METER_ACTION_HPP_
diff --git a/mep3_bringup/launch/robot_launch.py b/mep3_bringup/launch/robot_launch.py
index ea5e85843..78d219bcc 100644
--- a/mep3_bringup/launch/robot_launch.py
+++ b/mep3_bringup/launch/robot_launch.py
@@ -1,4 +1,9 @@
-"""Brings up a single robot."""
+# Brings up a single robot.
+#
+# Example usage:
+#
+# ros2 launch mep3_bringup robot_launch.py sim:=true color:=yellow namespace:=big strategy:=purple_strategy
+#
from math import pi
import os
@@ -27,30 +32,6 @@
'table2'
]
-ANGLE_MIRRORING_BLACKLIST = [
- # Florian (big)
- 'box',
- 'flipper_left',
- 'flipper_right',
- # Kosta (small)
- 'base',
- 'mid',
- 'gripper',
- 'rail'
-]
-
-SERVER_NAME_MIRRORING_BLACKLIST = [
- # Florian (big)
- 'box',
- # Kosta (small)
- 'base',
- 'mid',
- 'gripper',
- 'rail',
- 'fork_left',
- 'fork_right'
-]
-
def verify_color(context, *args, **kwargs):
if LaunchConfiguration('color').perform(context) \
@@ -91,9 +72,11 @@ def get_initial_pose_transform(namespace, color):
executable='static_transform_publisher',
output='screen',
arguments=[
- str(row_pose[0]),
- str(row_pose[1]), '0',
- str(row_pose[2]), '0', '0', 'map', 'odom'
+ '--x', str(row_pose[0]),
+ '--y', str(row_pose[1]),
+ '--yaw', str(row_pose[2]),
+ '--frame-id', 'map',
+ '--child-frame-id', 'odom'
],
namespace=namespace,
remappings=[('/tf_static', 'tf_static')],
@@ -103,11 +86,8 @@ def get_initial_pose_transform(namespace, color):
def generate_launch_description():
- package_dir = get_package_share_directory('mep3_bringup')
-
use_nav = LaunchConfiguration('nav', default=True)
use_behavior_tree = LaunchConfiguration('bt', default=True)
- use_regulator = LaunchConfiguration('regulator', default=True)
use_simulation = LaunchConfiguration('sim', default=False)
# Implementation wise, it would probably be easier to use
@@ -122,13 +102,11 @@ def generate_launch_description():
color = LaunchConfiguration('color')
table = LaunchConfiguration('table', default='')
- nav2_map = os.path.join(package_dir, 'resource', 'map.yml')
-
set_colorized_output = SetEnvironmentVariable('RCUTILS_COLORIZED_OUTPUT', '1')
diffdrive_controller_spawner = Node(
package='controller_manager',
- executable='spawner.py',
+ executable='spawner',
output='screen',
arguments=[
'diffdrive_controller',
@@ -142,8 +120,8 @@ def generate_launch_description():
}])
behavior_tree = Node(
- package='mep3_behavior_tree',
- executable='mep3_behavior_tree',
+ package='mep3_behavior',
+ executable='mep3_behavior',
name=['behavior', namespace],
output='screen',
parameters=[{
@@ -151,9 +129,7 @@ def generate_launch_description():
'color': color,
'table': table,
'strategy': strategy,
- 'predefined_tables': PREDEFINED_TABLE_NAMES,
- 'mirror_angle_blacklist': ANGLE_MIRRORING_BLACKLIST,
- 'mirror_name_blacklist': SERVER_NAME_MIRRORING_BLACKLIST
+ 'predefined_tables': PREDEFINED_TABLE_NAMES
}],
namespace=namespace,
condition=launch.conditions.IfCondition(use_behavior_tree))
@@ -163,7 +139,6 @@ def generate_launch_description():
os.path.join(get_package_share_directory('mep3_navigation'),
'launch', 'navigation_launch.py')),
launch_arguments=[
- ('map', nav2_map),
('use_sim_time', use_simulation),
('namespace', namespace),
('use_namespace', 'true'),
@@ -175,24 +150,9 @@ def generate_launch_description():
],
condition=launch.conditions.IfCondition(use_nav))
- regulator = Node(package='mep3_navigation',
- executable='distance_angle_regulator',
- output='screen',
- parameters=[{
- 'use_sim_time': use_simulation,
- },
- [
- get_package_share_directory('mep3_navigation'),
- '/params',
- '/config_regulator_', namespace, '.yaml'
- ]],
- namespace=namespace,
- remappings=[('/tf_static', 'tf_static'), ('/tf', 'tf')],
- condition=launch.conditions.IfCondition(use_regulator))
-
driver = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
- os.path.join(get_package_share_directory('mep3_driver'), 'launch',
+ os.path.join(get_package_share_directory('mep3_hardware'), 'launch',
'driver_launch.py')),
launch_arguments=[('namespace', namespace)],
condition=launch.conditions.UnlessCondition(use_simulation))
@@ -202,38 +162,28 @@ def generate_launch_description():
executable='static_transform_publisher',
output='screen',
arguments=[
- '0', '0', '0.3',
- str(pi), '0', '0', 'base_link', 'laser'
+ '--z', '0.3',
+ '--yaw', str(-pi/2),
+ '--frame-id', 'base_link',
+ '--child-frame-id', 'laser'
],
namespace=namespace,
remappings=[('/tf_static', 'tf_static')],
)
- laser_inflator = Node(package='mep3_navigation',
- executable='laser_inflator',
- parameters=[{
- 'inflation_radius': 0.05,
- 'inflation_angular_step': 0.09
- }],
- remappings=[('/tf_static', 'tf_static'),
- ('/tf', 'tf'),
- ('scan', 'scan_filtered')],
- output='screen',
- namespace=namespace)
-
laser_filters = Node(package='laser_filters',
- executable='scan_to_scan_filter_chain',
- parameters=[
- PathJoinSubstitution([
- get_package_share_directory('mep3_navigation'),
- 'params', 'laser_filters.yaml',
- ])
- ],
- remappings=[('/tf_static', 'tf_static'),
- ('/tf', 'tf')
- ],
- output='screen',
- namespace=namespace)
+ executable='scan_to_scan_filter_chain',
+ parameters=[
+ PathJoinSubstitution([
+ get_package_share_directory('mep3_navigation'),
+ 'params', 'laser_filters.yaml',
+ ])
+ ],
+ remappings=[('/tf_static', 'tf_static'),
+ ('/tf', 'tf')
+ ],
+ output='screen',
+ namespace=namespace)
domain_bridge_node = Node(
package='domain_bridge',
@@ -253,7 +203,6 @@ def generate_launch_description():
on_exit_events = []
critical_nodes = [
behavior_tree,
- regulator,
]
for node in critical_nodes:
on_exit_event = RegisterEventHandler(
@@ -274,15 +223,11 @@ def generate_launch_description():
# Wheel controller
diffdrive_controller_spawner,
- # Lidar inflation
- laser_inflator,
-
# Lidar filtering
laser_filters,
# Navigation 2
nav2,
- regulator,
tf_base_link_laser,
driver,
] + on_exit_events + get_initial_pose_transform(namespace, color))
diff --git a/mep3_bringup/package.xml b/mep3_bringup/package.xml
index f717d3660..4dcccb119 100644
--- a/mep3_bringup/package.xml
+++ b/mep3_bringup/package.xml
@@ -8,6 +8,7 @@
TODO: License declaration
domain_bridge
+ rmw_cyclonedds_cpp
ament_copyright
ament_flake8
diff --git a/mep3_bringup/setup.cfg b/mep3_bringup/setup.cfg
index 0c059d972..56629a1e4 100644
--- a/mep3_bringup/setup.cfg
+++ b/mep3_bringup/setup.cfg
@@ -1,4 +1,4 @@
[develop]
-script-dir=$base/lib/mep3_bringup
+script_dir=$base/lib/mep3_bringup
[install]
-install-scripts=$base/lib/mep3_bringup
+install_scripts=$base/lib/mep3_bringup
diff --git a/mep3_driver/mep3_driver/lynx_driver.py b/mep3_driver/mep3_driver/lynx_driver.py
deleted file mode 100755
index cb84c82d7..000000000
--- a/mep3_driver/mep3_driver/lynx_driver.py
+++ /dev/null
@@ -1,223 +0,0 @@
-#!/usr/bin/env python3
-
-from distutils import command
-from functools import partial
-from traceback import print_tb
-
-from mep3_msgs.action import DynamixelCommand
-from rclpy.executors import MultiThreadedExecutor
-
-import rclpy
-from rclpy.node import Node
-
-from rclpy.action import ActionServer, CancelResponse, GoalResponse
-from rclpy.callback_groups import ReentrantCallbackGroup
-
-
-DEFAULT_POSITION = 0 # deg
-MAX_POSITION = 155 # deg
-MIN_POSITION = 0 # deg
-DEFAULT_VELOCITY = 10 # deg/s
-DEFAULT_TOLERANCE = 5 # deg
-DEFAULT_TIMEOUT = 5 # s
-"""
-Test:
-ros2 action send_goal /big/dynamixel_command/lift_motor mep3_msgs/action/DynamixelCommand "position: 22"
-"""
-
-from threading import current_thread, Lock
-
-import can
-import struct
-from math import isclose
-
-SERVOS = [
- {'id': 0, 'name': 'lift_motor', 'model': 'lynx'},
-]
-
-SERVO_CAN_ID = 0x00006C20
-POLL_PERIOD = 0.2
-
-
-def scale(val, src, dst):
- return ((val - src[0]) / (src[1] - src[0])) * (dst[1] - dst[0]) + dst[0]
-
-
-class LynxServo:
- def __init__(self, servo_id, name, model):
- self.id = servo_id
- self.name = name
- self.model = model
-
- # Values we need to keep track of while Node is active
- self.present_position = 0
- self.present_velocity = 0
-
- def get_command_data(self, position):
-
- if position > MAX_POSITION: position = MAX_POSITION
- if position < MIN_POSITION: position = MIN_POSITION
- position = int(position*10) # Scaling to Lynx protocol
-
- fmt = '>BHH'
- speed = int(37*10) # Scaling to Lynx protocol
-
- data = [self.id, position, speed]
-
- binary_data = struct.pack(fmt, *data)
-
- return binary_data
-
-
-class LynxDriver(Node):
- def __init__(self, can_mutex, can_bus):
- super().__init__('lynx_driver')
- self.__actions = []
- self.servo_list = []
-
- self.can_mutex = can_mutex
- self.bus = can_bus
-
- self.rate = self.create_rate(1 / POLL_PERIOD)
-
- callback_group = ReentrantCallbackGroup()
- for servo_config in SERVOS:
- new_servo = LynxServo(
- servo_id=servo_config['id'], name=servo_config['name'], model=servo_config['model']
- )
- self.servo_list.append(new_servo)
- self.__actions.append(
- ActionServer(
- self,
- DynamixelCommand,
- f"dynamixel_command/{servo_config['name']}",
- execute_callback=partial(self.__handle_dynamixel_command, servo=new_servo),
- callback_group=callback_group,
- goal_callback=self.__goal_callback,
- cancel_callback=self.__cancel_callback
- )
- )
-
- def __goal_callback(self, _):
- return GoalResponse.ACCEPT
-
- def __cancel_callback(self, _):
- return CancelResponse.ACCEPT
-
- def process_single_command(self, bin_data):
-
- self.can_mutex.acquire()
-
- msg = can.Message(arbitration_id=SERVO_CAN_ID,
- data=bin_data,
- is_extended_id=True)
-
- try:
- self.bus.send(msg)
- except can.CanError:
- self.get_logger().info("CAN ERROR: Cannot send message over CAN bus. Check if can0 is active.")
-
- message = self.bus.recv(0.1) # Wait until a message is received or 0.1s
-
- self.can_mutex.release()
-
- if message:
- ret_val = message
- else:
- self.get_logger().info("Timeout error for servo response. Check servo connections.")
- ret_val = False
-
- return ret_val
-
- def __handle_dynamixel_command(self, goal_handle,
- servo: LynxServo = None):
- self.get_logger().info(str(goal_handle.request))
- self.get_logger().info("Servo ID: " + str(servo.id))
- self.get_logger().info("Thread: " + str(current_thread().name))
-
- position = goal_handle.request.position # deg
- position += 6 # For some reason Servo doesn't go to 5 or lower
-
- if not position:
- position = 0.1
-
- tolerance = goal_handle.request.tolerance # deg
- timeout = goal_handle.request.timeout
-
- if not tolerance:
- tolerance = DEFAULT_TOLERANCE
- if not timeout:
- timeout = DEFAULT_TIMEOUT
-
- result = DynamixelCommand.Result()
-
- result.result = 2 # Other error
-
- if not isclose(position, servo.present_position,
- abs_tol=tolerance):
- # Send GoalPosition and Poll
- if not self.go_to_position(servo, position,
- timeout, tolerance):
- result.result = 1 # Set to Timeout error
- self.get_logger().info("Timeout!!!")
-
- goal_handle.abort()
- return result
-
- result.result = 0 # There are no errors, return 0
- goal_handle.succeed()
- return result
-
-
- def go_to_position(self, servo, position, timeout, tolerance):
- status = self.process_single_command(
- bin_data=servo.get_command_data(position))
- if not status:
- return 0
-
- number_of_tries = 0
-
- while not isclose(position, servo.present_position,
- abs_tol=tolerance):
-
- self.rate.sleep()
- status = self.process_single_command(
- bin_data=servo.get_command_data(position))
-
- if status:
-
- fmt = '>BHH'
- if len(status.data) == 5:
- servo.present_position = float( struct.unpack(fmt, status.data)[1])/10
- servo.present_velocity = float( struct.unpack(fmt, status.data)[2])/10
- self.get_logger().info("Servo responded pos: {}".format(servo.present_position))
- else:
- self.get_logger().info("Servo response is not OK")
-
-
- if number_of_tries > (timeout / POLL_PERIOD):
- return 0
-
- number_of_tries += 1
-
- return 1
-
-
-def main(args=None):
- rclpy.init(args=args)
-
- can_mutex = Lock()
- bus = can.ThreadSafeBus(bustype='socketcan', channel='can0', bitrate=500000)
-
- # Set filters for receiving data
- bus.set_filters(filters=[{"can_id": SERVO_CAN_ID, "can_mask": 0x1FFFFFFE, "extended": True}])
-
- executor = MultiThreadedExecutor(num_threads=6)
- driver = LynxDriver(can_mutex, bus)
- executor.add_node(driver)
- executor.spin()
- rclpy.shutdown()
-
-
-if __name__ == '__main__':
- main()
diff --git a/mep3_driver/mep3_driver/resistance_meter_driver.py b/mep3_driver/mep3_driver/resistance_meter_driver.py
deleted file mode 100755
index 11145275b..000000000
--- a/mep3_driver/mep3_driver/resistance_meter_driver.py
+++ /dev/null
@@ -1,115 +0,0 @@
-#!/usr/bin/env python3
-
-import struct
-import can
-import math
-from functools import partial
-from rclpy.action import ActionServer, CancelResponse, GoalResponse
-from threading import Lock, current_thread
-from mep3_msgs.action import ResistanceMeter
-import rclpy
-from rclpy.node import Node
-
-
-RESISANCE_METER_CAN_ID = 0x00008D73
-RESISANCE_METER_CAN_MASK = 0x1FFFFFFF
-RESISTANCE_METERS = [
- {'name': 'left'},
- {'name': 'right'},
-]
-
-
-def readout_to_resistance(value):
- return int(1.2*value - 250)
-
-
-class ResistanceMeterDriver(Node):
- def __init__(self, can_mutex, can_bus):
- super().__init__('resistance_meter_driver')
- self.__actions = []
- self.meter_list = []
-
- self.can_mutex = can_mutex
- self.bus = can_bus
-
- for meter in RESISTANCE_METERS:
- self.__actions.append(
- ActionServer(
- self,
- ResistanceMeter,
- f'resistance_meter/{meter["name"]}',
- execute_callback=partial(self.__handle_resistance_meter),
- goal_callback=self.__goal_callback,
- cancel_callback=self.__cancel_callback
- )
- )
-
- def __goal_callback(self, _):
- return GoalResponse.ACCEPT
-
- def __cancel_callback(self, _):
- return CancelResponse.ACCEPT
-
- def __handle_resistance_meter(self, goal_handle):
- self.get_logger().info(str(goal_handle.request))
- self.get_logger().info("Resistance meter ID: " + str(RESISANCE_METER_CAN_ID))
- self.get_logger().info("Thread: " + str(current_thread().name))
-
- result = ResistanceMeter.Result()
- resistance = None
-
- msg = can.Message(
- arbitration_id=RESISANCE_METER_CAN_ID,
- data=[], # Send an empty message
- is_extended_id=True
- )
-
- self.can_mutex.acquire()
-
- try:
- self.bus.send(msg)
- except can.CanError:
- self.get_logger().info(
- "CAN ERROR: Cannot send message over CAN bus. Check if can0 is active.")
-
- recv = self.bus.recv(timeout=1)
- print(recv)
- if recv is not None:
- readout = struct.unpack('>>>>>>>VALUE FROM CAN: " + str(readout))
- resistance = readout_to_resistance(readout)
- self.get_logger().info("Resistance: " + str(resistance))
- self.get_logger().info("can data resist: " + str(recv))
-
- self.can_mutex.release()
-
- if resistance is None:
- result.resistance = 0
- goal_handle.abort()
- else:
- goal_handle.succeed()
- result.resistance = resistance
-
- return result
-
-
-def main(args=None):
- rclpy.init(args=args)
-
- can_mutex = Lock()
- bus = can.ThreadSafeBus(bustype='socketcan',
- channel='can0', bitrate=500000)
-
- # Set filters for receiving data
- bus.set_filters(filters=[{
- 'can_id': RESISANCE_METER_CAN_ID,
- 'can_mask': RESISANCE_METER_CAN_MASK,
- 'extended': True
- }])
- driver = ResistanceMeterDriver(can_mutex, bus)
- rclpy.spin(driver)
- rclpy.shutdown()
-
-
-if __name__ == '__main__':
- main()
diff --git a/mep3_driver/mep3_driver_ros2_control.xml b/mep3_driver/mep3_driver_ros2_control.xml
deleted file mode 100644
index 4d7aee361..000000000
--- a/mep3_driver/mep3_driver_ros2_control.xml
+++ /dev/null
@@ -1,3 +0,0 @@
-
-
-
diff --git a/mep3_driver/CMakeLists.txt b/mep3_hardware/CMakeLists.txt
similarity index 89%
rename from mep3_driver/CMakeLists.txt
rename to mep3_hardware/CMakeLists.txt
index efab91052..9ca22ef8a 100644
--- a/mep3_driver/CMakeLists.txt
+++ b/mep3_hardware/CMakeLists.txt
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.5)
-project(mep3_driver)
+project(mep3_hardware)
# Default to C99
if(NOT CMAKE_C_STANDARD)
@@ -8,7 +8,7 @@ endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 14)
+ set(CMAKE_CXX_STANDARD 17)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -46,7 +46,7 @@ ament_target_dependencies(
rclcpp_lifecycle
)
-pluginlib_export_plugin_description_file(hardware_interface mep3_driver_ros2_control.xml)
+pluginlib_export_plugin_description_file(hardware_interface mep3_hardware_ros2_control.xml)
add_executable(socketcan_bridge src/socketcan_bridge.cpp)
target_include_directories(
@@ -101,11 +101,9 @@ install(
# Python
install(PROGRAMS
- mep3_driver/lynx_driver.py
- mep3_driver/vacuum_pump_driver.py
- mep3_driver/cinch_driver.py
- mep3_driver/lcd_driver.py
- mep3_driver/resistance_meter_driver.py
+ mep3_hardware/vacuum_pump_driver.py
+ mep3_hardware/cinch_driver.py
+ mep3_hardware/lcd_driver.py
DESTINATION lib/${PROJECT_NAME}
)
ament_python_install_package(${PROJECT_NAME}
diff --git a/mep3_driver/README.md b/mep3_hardware/README.md
similarity index 76%
rename from mep3_driver/README.md
rename to mep3_hardware/README.md
index 21856dda4..b405f60d6 100644
--- a/mep3_driver/README.md
+++ b/mep3_hardware/README.md
@@ -4,5 +4,5 @@ Creates ROS 2 interface on top of the physical robot. It includes the `ros2_cont
Run the driver:
```bash
-ros2 launch mep3_driver driver_launch.py
+ros2 launch mep3_hardware driver_launch.py
```
diff --git a/mep3_driver/include/mep3_driver/dynamixel_driver.hpp b/mep3_hardware/include/mep3_hardware/dynamixel_driver.hpp
similarity index 89%
rename from mep3_driver/include/mep3_driver/dynamixel_driver.hpp
rename to mep3_hardware/include/mep3_hardware/dynamixel_driver.hpp
index 92e5d3521..796eddd62 100644
--- a/mep3_driver/include/mep3_driver/dynamixel_driver.hpp
+++ b/mep3_hardware/include/mep3_hardware/dynamixel_driver.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef MEP3_DRIVER__DYNAMIXEL_DRIVER_HPP_
-#define MEP3_DRIVER__DYNAMIXEL_DRIVER_HPP_
+#ifndef MEP3_HARDWARE__DYNAMIXEL_DRIVER_HPP_
+#define MEP3_HARDWARE__DYNAMIXEL_DRIVER_HPP_
#include
#include
@@ -27,11 +27,11 @@
#define DEG2RAD(x) (x * M_PI / 180.0)
-#include "mep3_driver/simple_action_server.hpp"
+#include "mep3_hardware/simple_action_server.hpp"
#include "mep3_msgs/action/dynamixel_command.hpp"
#include "rclcpp/rclcpp.hpp"
-namespace mep3_driver
+namespace mep3_hardware
{
class DynamixelDriver : public rclcpp::Node
{
@@ -67,6 +67,6 @@ class DynamixelDriver : public rclcpp::Node
int32_t velocity_to_value(uint8_t id, float velocity);
};
-} // namespace mep3_driver
+} // namespace mep3_hardware
-#endif // MEP3_DRIVER__DYNAMIXEL_DRIVER_HPP_
+#endif // MEP3_HARDWARE__DYNAMIXEL_DRIVER_HPP_
diff --git a/mep3_driver/include/mep3_driver/motion_board_driver.hpp b/mep3_hardware/include/mep3_hardware/motion_board_driver.hpp
similarity index 93%
rename from mep3_driver/include/mep3_driver/motion_board_driver.hpp
rename to mep3_hardware/include/mep3_hardware/motion_board_driver.hpp
index fbd48417a..07a7b7354 100644
--- a/mep3_driver/include/mep3_driver/motion_board_driver.hpp
+++ b/mep3_hardware/include/mep3_hardware/motion_board_driver.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef MEP3_DRIVER__MOTION_BOARD_DRIVER_HPP_
-#define MEP3_DRIVER__MOTION_BOARD_DRIVER_HPP_
+#ifndef MEP3_HARDWARE__MOTION_BOARD_DRIVER_HPP_
+#define MEP3_HARDWARE__MOTION_BOARD_DRIVER_HPP_
extern "C" {
#include // CAN network layer definitions
@@ -28,7 +28,7 @@ extern "C" {
#include
#include
-namespace mep3_driver
+namespace mep3_hardware
{
class MotionBoardDriver
{
@@ -90,6 +90,6 @@ class MotionBoardDriver
void set_ki_angular(float val);
void set_kd_angular(float val);
};
-} // namespace mep3_driver
+} // namespace mep3_hardware
-#endif // MEP3_DRIVER__MOTION_BOARD_DRIVER_HPP_
+#endif // MEP3_HARDWARE__MOTION_BOARD_DRIVER_HPP_
diff --git a/mep3_driver/include/mep3_driver/robot_hardware_interface.hpp b/mep3_hardware/include/mep3_hardware/robot_hardware_interface.hpp
similarity index 67%
rename from mep3_driver/include/mep3_driver/robot_hardware_interface.hpp
rename to mep3_hardware/include/mep3_hardware/robot_hardware_interface.hpp
index 1020a0aeb..84d49f7f8 100644
--- a/mep3_driver/include/mep3_driver/robot_hardware_interface.hpp
+++ b/mep3_hardware/include/mep3_hardware/robot_hardware_interface.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef MEP3_DRIVER__ROBOT_HARDWARE_INTERFACE_HPP_
-#define MEP3_DRIVER__ROBOT_HARDWARE_INTERFACE_HPP_
+#ifndef MEP3_HARDWARE__ROBOT_HARDWARE_INTERFACE_HPP_
+#define MEP3_HARDWARE__ROBOT_HARDWARE_INTERFACE_HPP_
#include
@@ -25,18 +25,18 @@
#define POW2(N) (1UL << (N))
-namespace mep3_driver
+namespace mep3_hardware
{
class RobotHardwareInterface : public hardware_interface::SystemInterface
{
public:
- CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
- CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
- CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;
+ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
+ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
+ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;
std::vector export_state_interfaces() override;
std::vector export_command_interfaces() override;
- hardware_interface::return_type read() override;
- hardware_interface::return_type write() override;
+ hardware_interface::return_type read(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) override;
+ hardware_interface::return_type write(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) override;
private:
double left_wheel_velocity_command_;
@@ -60,6 +60,6 @@ class RobotHardwareInterface : public hardware_interface::SystemInterface
MotionBoardDriver motion_board_;
};
-} // namespace mep3_driver
+} // namespace mep3_hardware
-#endif // MEP3_DRIVER__ROBOT_HARDWARE_INTERFACE_HPP_
+#endif // MEP3_HARDWARE__ROBOT_HARDWARE_INTERFACE_HPP_
diff --git a/mep3_driver/include/mep3_driver/simple_action_server.hpp b/mep3_hardware/include/mep3_hardware/simple_action_server.hpp
similarity index 100%
rename from mep3_driver/include/mep3_driver/simple_action_server.hpp
rename to mep3_hardware/include/mep3_hardware/simple_action_server.hpp
diff --git a/mep3_driver/include/mep3_driver/socketcan_bridge.hpp b/mep3_hardware/include/mep3_hardware/socketcan_bridge.hpp
similarity index 90%
rename from mep3_driver/include/mep3_driver/socketcan_bridge.hpp
rename to mep3_hardware/include/mep3_hardware/socketcan_bridge.hpp
index 1b3ac449a..aef990bfe 100644
--- a/mep3_driver/include/mep3_driver/socketcan_bridge.hpp
+++ b/mep3_hardware/include/mep3_hardware/socketcan_bridge.hpp
@@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#ifndef MEP3_DRIVER__SOCKETCAN_BRIDGE_HPP_
-#define MEP3_DRIVER__SOCKETCAN_BRIDGE_HPP_
+#ifndef MEP3_HARDWARE__SOCKETCAN_BRIDGE_HPP_
+#define MEP3_HARDWARE__SOCKETCAN_BRIDGE_HPP_
extern "C" {
#include // CAN network layer definitions
@@ -32,7 +32,7 @@ extern "C" {
#include "can_msgs/msg/frame.hpp"
#include "rclcpp/rclcpp.hpp"
-namespace mep3_driver
+namespace mep3_hardware
{
class Bridge : public rclcpp::Node
{
@@ -58,6 +58,6 @@ class Bridge : public rclcpp::Node
std::thread canbus_receive_thread_;
};
-} // namespace mep3_driver
+} // namespace mep3_hardware
-#endif // MEP3_DRIVER__SOCKETCAN_BRIDGE_HPP_
+#endif // MEP3_HARDWARE__SOCKETCAN_BRIDGE_HPP_
diff --git a/mep3_driver/launch/driver_launch.py b/mep3_hardware/launch/hardware_launch.py
similarity index 77%
rename from mep3_driver/launch/driver_launch.py
rename to mep3_hardware/launch/hardware_launch.py
index 6c0b3554d..8fe0c353a 100644
--- a/mep3_driver/launch/driver_launch.py
+++ b/mep3_hardware/launch/hardware_launch.py
@@ -4,13 +4,12 @@
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.actions import OpaqueFunction
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from launch.conditions import IfCondition
-
def enable_can_interface():
is_can_up = False
if os.path.isfile('/sys/class/net/can0/operstate'):
@@ -27,13 +26,13 @@ def enable_can_interface():
def launch_setup(context, *args, **kwargs):
- package_dir = get_package_share_directory('mep3_driver')
- bringup_dir = os.path.join(get_package_share_directory('mep3_bringup'))
+ package_dir = get_package_share_directory('mep3_hardware')
namespace = LaunchConfiguration('namespace', default='big')
performed_namespace = namespace.perform(context)
- controller_params_file = os.path.join(get_package_share_directory('mep3_bringup'), 'resource', f'ros2_control_{performed_namespace}.yaml')
+ controller_params_file = os.path.join(get_package_share_directory(
+ 'mep3_bringup'), 'resource', f'ros2_control_{performed_namespace}.yaml')
robot_description = pathlib.Path(os.path.join(package_dir, 'resource', f'config_{performed_namespace}.urdf')).read_text()
dynamixel_config = os.path.join(package_dir, 'resource', f'dynamixel_config_{performed_namespace}.yaml')
@@ -56,32 +55,25 @@ def launch_setup(context, *args, **kwargs):
)
socketcan_bridge = Node(
- package='mep3_driver',
+ package='mep3_hardware',
executable='socketcan_bridge',
output='screen',
namespace=namespace
)
cinch_driver = Node(
- package='mep3_driver',
+ package='mep3_hardware',
executable='cinch_driver.py',
output='screen'
)
pumps_driver = Node(
- package='mep3_driver',
+ package='mep3_hardware',
executable='vacuum_pump_driver.py',
output='screen',
namespace=namespace
)
- resistance_driver = Node(
- package='mep3_driver',
- executable='resistance_meter_driver.py',
- output='screen',
- namespace=namespace
- )
-
lidar_rplidar = Node(
package='rplidar_ros',
executable='rplidar_composition',
@@ -92,11 +84,10 @@ def launch_setup(context, *args, **kwargs):
}],
output='screen',
namespace=namespace,
- # condition=IfCondition(PythonExpression(["'", namespace, "' == 'small'"]))
)
dynamixel_driver = Node(
- package='mep3_driver',
+ package='mep3_hardware',
executable='dynamixel_driver',
parameters=[dynamixel_config],
output='screen',
@@ -104,29 +95,20 @@ def launch_setup(context, *args, **kwargs):
)
lcd_driver = Node(
- package='mep3_driver',
+ package='mep3_hardware',
executable='lcd_driver.py',
output='screen',
condition=IfCondition(PythonExpression(["'", namespace, "' == 'small'"]))
)
- lynxs_driver = Node(
- package='mep3_driver',
- executable='lynx_driver.py',
- output='screen',
- namespace=namespace
- )
-
return [
controller_manager_node,
socketcan_bridge,
cinch_driver,
lidar_rplidar,
pumps_driver,
- resistance_driver,
dynamixel_driver,
- lcd_driver,
- lynxs_driver,
+ lcd_driver
]
diff --git a/mep3_driver/mep3_driver/__init__.py b/mep3_hardware/mep3_hardware/__init__.py
similarity index 100%
rename from mep3_driver/mep3_driver/__init__.py
rename to mep3_hardware/mep3_hardware/__init__.py
diff --git a/mep3_driver/mep3_driver/cinch_driver.py b/mep3_hardware/mep3_hardware/cinch_driver.py
similarity index 99%
rename from mep3_driver/mep3_driver/cinch_driver.py
rename to mep3_hardware/mep3_hardware/cinch_driver.py
index 226eafc70..ee2174527 100755
--- a/mep3_driver/mep3_driver/cinch_driver.py
+++ b/mep3_hardware/mep3_hardware/cinch_driver.py
@@ -22,7 +22,6 @@ def process_cinch_state(self):
self.__start_cinch.wait_for_press()
self.__publisher.publish(Int8(data=1))
rclpy.spin_once(self, timeout_sec=0)
-
self.__start_cinch.wait_for_release()
self.__publisher.publish(Int8(data=2))
diff --git a/mep3_driver/mep3_driver/lcd_driver.py b/mep3_hardware/mep3_hardware/lcd_driver.py
similarity index 93%
rename from mep3_driver/mep3_driver/lcd_driver.py
rename to mep3_hardware/mep3_hardware/lcd_driver.py
index 89ffda29a..755ce2fc0 100755
--- a/mep3_driver/mep3_driver/lcd_driver.py
+++ b/mep3_hardware/mep3_hardware/lcd_driver.py
@@ -30,8 +30,8 @@ def display_pts(self, p):
def listener_callback(self, msg):
- #if msg.task not in self.__completed_tasks:
- if True: # add always
+ # if msg.task not in self.__completed_tasks:
+ if True: # add always
self.__score += msg.points
self.__completed_tasks.add(msg.task)
self.get_logger().info(
@@ -48,6 +48,8 @@ def listener_callback(self, msg):
self.display_pts(self.__score)
self.get_logger().info(
'Current score: %i points' % self.__score)
+
+
def main(args=None):
rclpy.init(args=args)
diff --git a/mep3_driver/mep3_driver/vacuum_pump_driver.py b/mep3_hardware/mep3_hardware/vacuum_pump_driver.py
similarity index 100%
rename from mep3_driver/mep3_driver/vacuum_pump_driver.py
rename to mep3_hardware/mep3_hardware/vacuum_pump_driver.py
diff --git a/mep3_hardware/mep3_hardware_ros2_control.xml b/mep3_hardware/mep3_hardware_ros2_control.xml
new file mode 100644
index 000000000..3f72d597a
--- /dev/null
+++ b/mep3_hardware/mep3_hardware_ros2_control.xml
@@ -0,0 +1,3 @@
+
+
+
diff --git a/mep3_driver/package.xml b/mep3_hardware/package.xml
similarity index 97%
rename from mep3_driver/package.xml
rename to mep3_hardware/package.xml
index 9fe57cbbe..52786bd49 100644
--- a/mep3_driver/package.xml
+++ b/mep3_hardware/package.xml
@@ -1,7 +1,7 @@
- mep3_driver
+ mep3_hardware
0.0.0
TODO: Package description
lukic
diff --git a/mep3_driver/resource/config_big.urdf b/mep3_hardware/resource/config_big.urdf
similarity index 92%
rename from mep3_driver/resource/config_big.urdf
rename to mep3_hardware/resource/config_big.urdf
index ad8f8f5b5..fa0b506e4 100644
--- a/mep3_driver/resource/config_big.urdf
+++ b/mep3_hardware/resource/config_big.urdf
@@ -2,7 +2,7 @@
- mep3_driver::RobotHardwareInterface
+ mep3_hardware::RobotHardwareInterface
90.0
1.0
2.0
diff --git a/mep3_driver/resource/config_small.urdf b/mep3_hardware/resource/config_small.urdf
similarity index 92%
rename from mep3_driver/resource/config_small.urdf
rename to mep3_hardware/resource/config_small.urdf
index 11ffb2a97..92f35ddc9 100644
--- a/mep3_driver/resource/config_small.urdf
+++ b/mep3_hardware/resource/config_small.urdf
@@ -2,7 +2,7 @@
- mep3_driver::RobotHardwareInterface
+ mep3_hardware::RobotHardwareInterface
170.0
2.0
80.0
diff --git a/mep3_driver/resource/dynamixel_config_big.yaml b/mep3_hardware/resource/dynamixel_config_big.yaml
similarity index 100%
rename from mep3_driver/resource/dynamixel_config_big.yaml
rename to mep3_hardware/resource/dynamixel_config_big.yaml
diff --git a/mep3_driver/resource/dynamixel_config_small.yaml b/mep3_hardware/resource/dynamixel_config_small.yaml
similarity index 100%
rename from mep3_driver/resource/dynamixel_config_small.yaml
rename to mep3_hardware/resource/dynamixel_config_small.yaml
diff --git a/mep3_driver/src/dynamixel_driver.cpp b/mep3_hardware/src/dynamixel_driver.cpp
similarity index 98%
rename from mep3_driver/src/dynamixel_driver.cpp
rename to mep3_hardware/src/dynamixel_driver.cpp
index 83b529991..5b7569280 100644
--- a/mep3_driver/src/dynamixel_driver.cpp
+++ b/mep3_hardware/src/dynamixel_driver.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "mep3_driver/dynamixel_driver.hpp"
+#include "mep3_hardware/dynamixel_driver.hpp"
#include
#include
@@ -24,7 +24,7 @@
using std::placeholders::_1;
using namespace std::chrono_literals;
-namespace mep3_driver
+namespace mep3_hardware
{
DynamixelDriver::DynamixelDriver(const rclcpp::NodeOptions & options)
: Node("dynamixel_driver", options)
@@ -302,7 +302,7 @@ void DynamixelDriver::control_loop()
prev_torque_enabled = torque_enabled;
}
-} // namespace mep3_driver
+} // namespace mep3_hardware
int main(int argc, char * argv[])
{
@@ -310,7 +310,7 @@ int main(int argc, char * argv[])
rclcpp::ExecutorOptions options;
rclcpp::executors::MultiThreadedExecutor executor(
options, (size_t)12, false, std::chrono::nanoseconds(-1));
- auto node = std::make_shared();
+ auto node = std::make_shared();
executor.add_node(node);
executor.spin();
diff --git a/mep3_driver/src/motion_board_driver.cpp b/mep3_hardware/src/motion_board_driver.cpp
similarity index 98%
rename from mep3_driver/src/motion_board_driver.cpp
rename to mep3_hardware/src/motion_board_driver.cpp
index c33ce3d0c..34139056f 100644
--- a/mep3_driver/src/motion_board_driver.cpp
+++ b/mep3_hardware/src/motion_board_driver.cpp
@@ -12,14 +12,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "mep3_driver/motion_board_driver.hpp"
+#include "mep3_hardware/motion_board_driver.hpp"
#include
#include
#include
#include
-namespace mep3_driver
+namespace mep3_hardware
{
MotionBoardDriver::MotionBoardDriver()
{
@@ -259,4 +259,4 @@ void MotionBoardDriver::set_kd_angular(float val)
write(canbus_socket_, &frame, sizeof(struct can_frame));
}
-} // namespace mep3_driver
+} // namespace mep3_hardware
diff --git a/mep3_driver/src/robot_hardware_interface.cpp b/mep3_hardware/src/robot_hardware_interface.cpp
similarity index 86%
rename from mep3_driver/src/robot_hardware_interface.cpp
rename to mep3_hardware/src/robot_hardware_interface.cpp
index 1b1076e75..706dee0ff 100644
--- a/mep3_driver/src/robot_hardware_interface.cpp
+++ b/mep3_hardware/src/robot_hardware_interface.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "mep3_driver/robot_hardware_interface.hpp"
+#include "mep3_hardware/robot_hardware_interface.hpp"
#include
#include
@@ -22,9 +22,9 @@
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
-namespace mep3_driver
+namespace mep3_hardware
{
-CallbackReturn RobotHardwareInterface::on_init(const hardware_interface::HardwareInfo & info)
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotHardwareInterface::on_init(const hardware_interface::HardwareInfo & info)
{
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
return CallbackReturn::ERROR;
@@ -48,7 +48,7 @@ CallbackReturn RobotHardwareInterface::on_init(const hardware_interface::Hardwar
return CallbackReturn::SUCCESS;
}
-CallbackReturn RobotHardwareInterface::on_activate(const rclcpp_lifecycle::State &)
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotHardwareInterface::on_activate(const rclcpp_lifecycle::State &)
{
// init variables
left_wheel_velocity_command_ = 0;
@@ -64,7 +64,7 @@ CallbackReturn RobotHardwareInterface::on_activate(const rclcpp_lifecycle::State
int board_init_status = motion_board_.init();
if (board_init_status != 0) {
RCLCPP_FATAL(
- rclcpp::get_logger("mep3_driver"),
+ rclcpp::get_logger("mep3_hardware"),
"Motion board low lever driver init failed! Is 'can0' up?\n");
return CallbackReturn::ERROR;
}
@@ -81,7 +81,7 @@ CallbackReturn RobotHardwareInterface::on_activate(const rclcpp_lifecycle::State
return CallbackReturn::SUCCESS;
}
-CallbackReturn RobotHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &)
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &)
{
motion_board_.halt();
@@ -113,7 +113,7 @@ RobotHardwareInterface::export_command_interfaces()
return interfaces;
}
-hardware_interface::return_type RobotHardwareInterface::read()
+hardware_interface::return_type RobotHardwareInterface::read(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/)
{
// Read encoder data from the robot
int32_t tmp_left, tmp_right;
@@ -159,7 +159,7 @@ hardware_interface::return_type RobotHardwareInterface::read()
return hardware_interface::return_type::OK;
}
-hardware_interface::return_type RobotHardwareInterface::write()
+hardware_interface::return_type RobotHardwareInterface::write(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/)
{
// Send left and right wheel velocity commands to the robot
@@ -172,9 +172,9 @@ hardware_interface::return_type RobotHardwareInterface::write()
// TODO(angstrem98): Publish setpoints on topic.
/*RCLCPP_INFO(
- rclcpp::get_logger("mep3_driver"), "Setpoint left rad/s: %lf", left_wheel_velocity_command_);
+ rclcpp::get_logger("mep3_hardware"), "Setpoint left rad/s: %lf", left_wheel_velocity_command_);
RCLCPP_INFO(
- rclcpp::get_logger("mep3_driver"), "Setpoint right rad/s: %lf\n",
+ rclcpp::get_logger("mep3_hardware"), "Setpoint right rad/s: %lf\n",
right_wheel_velocity_command_);*/
// const int16_t speed_increments_left = (int16_t)round(speed_double_left);
@@ -189,7 +189,7 @@ hardware_interface::return_type RobotHardwareInterface::write()
return hardware_interface::return_type::OK;
}
-} // namespace mep3_driver
+} // namespace mep3_hardware
#include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(mep3_driver::RobotHardwareInterface, hardware_interface::SystemInterface)
+PLUGINLIB_EXPORT_CLASS(mep3_hardware::RobotHardwareInterface, hardware_interface::SystemInterface)
diff --git a/mep3_driver/src/socketcan_bridge.cpp b/mep3_hardware/src/socketcan_bridge.cpp
similarity index 96%
rename from mep3_driver/src/socketcan_bridge.cpp
rename to mep3_hardware/src/socketcan_bridge.cpp
index ec3f1b9dd..8a12968c6 100644
--- a/mep3_driver/src/socketcan_bridge.cpp
+++ b/mep3_hardware/src/socketcan_bridge.cpp
@@ -12,14 +12,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "mep3_driver/socketcan_bridge.hpp"
+#include "mep3_hardware/socketcan_bridge.hpp"
#include
#include
using std::placeholders::_1;
-namespace mep3_driver
+namespace mep3_hardware
{
Bridge::Bridge(const rclcpp::NodeOptions & options) : Node("socketcan_bridge", options)
{
@@ -140,12 +140,12 @@ int Bridge::init(std::string interface_name)
return 0;
}
-} // namespace mep3_driver
+} // namespace mep3_hardware
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
- rclcpp::spin(std::make_shared());
+ rclcpp::spin(std::make_shared());
rclcpp::shutdown();
return 0;
diff --git a/mep3_msgs/CMakeLists.txt b/mep3_msgs/CMakeLists.txt
index 8f664dd7a..b27cf79c0 100644
--- a/mep3_msgs/CMakeLists.txt
+++ b/mep3_msgs/CMakeLists.txt
@@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
-# Default to C++14
+# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 14)
+ set(CMAKE_CXX_STANDARD 17)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -23,7 +23,6 @@ find_package(geometry_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/DynamixelCommand.action"
"action/MotionCommand.action"
- "action/ResistanceMeter.action"
"msgs/Scoreboard.msg"
"action/VacuumPumpCommand.action"
"msgs/TemporalObstacle.msg"
diff --git a/mep3_msgs/action/ResistanceMeter.action b/mep3_msgs/action/ResistanceMeter.action
deleted file mode 100644
index d8238a22f..000000000
--- a/mep3_msgs/action/ResistanceMeter.action
+++ /dev/null
@@ -1,3 +0,0 @@
----
-int32 resistance # Ohms
----
diff --git a/mep3_navigation/CMakeLists.txt b/mep3_navigation/CMakeLists.txt
index d73cf4de9..1975bd94a 100644
--- a/mep3_navigation/CMakeLists.txt
+++ b/mep3_navigation/CMakeLists.txt
@@ -44,52 +44,6 @@ find_package(laser_filters REQUIRED)
find_package(diagnostic_msgs REQUIRED)
include_directories(include)
-
-# Distance angle regulator
-add_executable(distance_angle_regulator
- src/distance_angle/distance_angle_regulator.cpp
- src/distance_angle/pid_regulator.c
- src/distance_angle/motion_profile.cpp
-)
-
-target_link_libraries(distance_angle_regulator ruckig::ruckig)
-
-ament_target_dependencies(distance_angle_regulator
- rclcpp
- rclcpp_action
- rclcpp_components
- nav_msgs
- nav2_msgs
- nav2_util
- nav2_core
- nav_2d_utils
- nav_2d_msgs
- geometry_msgs
- mep3_msgs
- std_msgs
- ruckig
- tf2
- tf2_ros
- can_msgs
-)
-###
-
-# Laser inflator with scan constraining
-add_executable(laser_inflator src/laser_inflator/laser_inflator.cpp)
-target_include_directories(
- laser_inflator
- PRIVATE
- include
-)
-
-ament_target_dependencies(
- laser_inflator
- rclcpp
- sensor_msgs
- geometry_msgs
- tf2
- tf2_ros
-)
###
set(dependencies
@@ -113,16 +67,6 @@ ament_target_dependencies(regulated_pure_pursuit_controller
${dependencies}
)
-# BT condition
-add_library(nav2_globally_updated_goal_condition_bt_node SHARED src/plugins/condition/globally_updated_goal_condition.cpp)
-ament_target_dependencies(nav2_globally_updated_goal_condition_bt_node ${dependencies})
-target_compile_definitions(nav2_globally_updated_goal_condition_bt_node PRIVATE BT_PLUGIN_EXPORT)
-install(TARGETS nav2_globally_updated_goal_condition_bt_node
- ARCHIVE DESTINATION lib
- LIBRARY DESTINATION lib
- RUNTIME DESTINATION bin
-)
-
# temporal_obstacle_layer
add_library(
temporal_obstacle_layer
@@ -153,16 +97,6 @@ install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
install(DIRECTORY behavior_trees DESTINATION share/${PROJECT_NAME})
-install(TARGETS
- distance_angle_regulator
- DESTINATION lib/${PROJECT_NAME}
-)
-
-install(
- TARGETS laser_inflator
- DESTINATION lib/${PROJECT_NAME}
-)
-
install(TARGETS
regulated_pure_pursuit_controller
DESTINATION lib/${PROJECT_NAME}
diff --git a/mep3_navigation/include/mep3_navigation/distance_angle/distance_angle_regulator.hpp b/mep3_navigation/include/mep3_navigation/distance_angle/distance_angle_regulator.hpp
deleted file mode 100644
index e46ccac06..000000000
--- a/mep3_navigation/include/mep3_navigation/distance_angle/distance_angle_regulator.hpp
+++ /dev/null
@@ -1,160 +0,0 @@
-// Copyright 2021 Memristor Robotics
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MEP3_NAVIGATION__DISTANCE_ANGLE__DISTANCE_ANGLE_REGULATOR_HPP_
-#define MEP3_NAVIGATION__DISTANCE_ANGLE__DISTANCE_ANGLE_REGULATOR_HPP_
-
-#define RUN_EACH_NTH_CYCLES(counter_type, nth, run) \
- { \
- static counter_type _cycle_ = 0; \
- if (nth > 0 && ++_cycle_ >= nth) { \
- _cycle_ = 0; \
- run; \
- } \
- }
-
-#include
-#include
-#include
-#include
-#include
-#include
-
-extern "C" {
-#include "mep3_navigation/distance_angle/pid_regulator.h"
-}
-
-#include "geometry_msgs/msg/pose2_d.hpp"
-#include "geometry_msgs/msg/twist.hpp"
-#include "mep3_msgs/action/motion_command.hpp"
-#include "std_msgs/msg/float64.hpp"
-#include "mep3_navigation/distance_angle/motion_profile.hpp"
-#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
-#include "nav2_msgs/action/navigate_to_pose.hpp"
-#include "mep3_navigation/simple_action_server/simple_action_server.hpp"
-#include "nav_msgs/msg/odometry.hpp"
-#include "rcl_interfaces/msg/set_parameters_result.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "ruckig/ruckig.hpp"
-#include "std_msgs/msg/string.hpp"
-#include "tf2/exceptions.h"
-#include "tf2/utils.h"
-#include "tf2_ros/buffer.h"
-#include "tf2_ros/transform_listener.h"
-#include "can_msgs/msg/frame.hpp"
-
-using std::placeholders::_1;
-
-template
-int sgn(T val)
-{
- return val >= T(0) ? T(1) : T(-1);
-}
-
-class DistanceAngleRegulator : public rclcpp::Node
-{
-public:
- explicit DistanceAngleRegulator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
- rcl_interfaces::msg::SetParametersResult parameters_callback(
- const std::vector & parameters);
- void init();
- ~DistanceAngleRegulator();
- using NavigatoToPoseT = nav2_msgs::action::NavigateToPose;
- using NavigateToPoseServer = nav2_util::SimpleActionServer;
- using MotionCommandT = mep3_msgs::action::MotionCommand;
- using MotionCommandServer = nav2_util::SimpleActionServer;
-
-private:
- rclcpp::Subscription::SharedPtr odometry_subscription_;
- rclcpp::Publisher::SharedPtr twist_publisher_;
- pid_regulator_t regulator_distance_;
- pid_regulator_t regulator_angle_;
- double odom_robot_x_;
- double odom_robot_y_;
- double map_robot_x_;
- double map_robot_y_;
- double odom_robot_distance_;
- double odom_robot_angle_;
- double odom_robot_speed_linear_;
- double odom_robot_speed_angular_;
- double map_robot_angle_;
- double prev_odom_robot_x_;
- double prev_odom_robot_y_;
- double distance_goal_tolerance_;
- double angle_goal_tolerance_;
- bool position_initialized_;
- bool debug_;
- bool action_running_;
- bool output_enabled_;
- std::atomic_bool run_process_frame_thread_;
- std::thread process_frame_thread_;
- std::shared_ptr transform_listener_{nullptr};
- std::unique_ptr tf_buffer_;
- rclcpp::TimerBase::SharedPtr timer_;
- int64_t system_time_; // ms
-
- ruckig::Ruckig<2> * motion_profile_;
- ruckig::InputParameter<2> motion_profile_input_;
- ruckig::OutputParameter<2> motion_profile_output_;
- ruckig::Result motion_profile_result_;
-
- double goal_distance_;
- OnSetParametersCallbackHandle::SharedPtr parameters_callback_handle_;
-
- std::unique_ptr navigate_to_pose_server_;
- std::unique_ptr motion_command_server_;
-
- std::mutex data_mutex_;
-
- void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
- void process_robot_frame();
- void control_loop();
-
- double angle_normalize(double angle);
- void forward(double distance);
- void rotate_relative(double angle);
- void rotate_absolute(double angle);
- void softstop();
-
- bool distance_regulator_finished();
- bool angle_regulator_finished();
- bool motion_profile_finished();
- void reset_regulation();
-
- void navigate_to_pose();
- void motion_command();
-
- geometry_msgs::msg::Pose2D projectPose(
- geometry_msgs::msg::Pose2D pose, geometry_msgs::msg::Twist twist, double projection_time);
- std::shared_ptr costmap_sub_;
- std::shared_ptr footprint_sub_;
- std::shared_ptr collision_checker_;
- std::string robot_base_frame_ = "base_link";
- double transform_tolerance_ = 0.8;
- bool check_collision_;
-
- bool stuck_enabled_ = true;
- bool robot_stuck_ = false;
- int distance_fail_count_ = 0;
- int angle_fail_count_ = 0;
- rclcpp::Publisher::SharedPtr can_publisher_;
- void reset_stuck();
- // Publishers for regulator tuning and visualization
- rclcpp::Publisher::SharedPtr distance_setpoint_publisher_;
- rclcpp::Publisher::SharedPtr distance_publisher_;
- rclcpp::Publisher::SharedPtr angle_setpoint_publisher_;
- rclcpp::Publisher::SharedPtr angle_publisher_;
-};
-
-#endif // MEP3_NAVIGATION__DISTANCE_ANGLE__DISTANCE_ANGLE_REGULATOR_HPP_
diff --git a/mep3_navigation/include/mep3_navigation/distance_angle/motion_profile.hpp b/mep3_navigation/include/mep3_navigation/distance_angle/motion_profile.hpp
deleted file mode 100644
index 5e332de74..000000000
--- a/mep3_navigation/include/mep3_navigation/distance_angle/motion_profile.hpp
+++ /dev/null
@@ -1,63 +0,0 @@
-// Copyright 2021 Memristor Robotics
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MEP3_NAVIGATION__DISTANCE_ANGLE__MOTION_PROFILE_HPP_
-#define MEP3_NAVIGATION__DISTANCE_ANGLE__MOTION_PROFILE_HPP_
-
-#include "rclcpp/time.hpp"
-
-class MotionProfile
-{
-public:
- MotionProfile();
- MotionProfile(double position_initial, double velocity_max, double acceleration_max);
- void plan(
- double position_initial, double setpoint, double velocity_initial, double velocity_final,
- rclcpp::Time time);
- double update(rclcpp::Time time);
- double get_position();
- double get_velocity();
- double get_setpoint();
- double get_velocity_max();
- void set_velocity_max(double velocity_max);
- double get_acceleration_max();
- void set_acceleration_max(double acceleration_max);
- bool finished();
- enum class ProfileState { ACCELERATION, CRUISING, DECELERATION, FINISHED };
- ProfileState get_state();
-
-private:
- double position_;
- double position_initial_;
- double setpoint_;
- double velocity_max_;
- double acceleration_max_;
- double velocity_initial_;
- double velocity_cruising_;
- double velocity_final_;
- double velocity_current_;
- double acceleration_;
- double deceleration_;
- double acceleration_current_;
-
- double t0_, t1_, t2_, t3_;
- double y1_, y2_;
-
- rclcpp::Time time_initial_;
-
- ProfileState state_;
- bool finished_;
-};
-
-#endif // MEP3_NAVIGATION__DISTANCE_ANGLE__MOTION_PROFILE_HPP_
diff --git a/mep3_navigation/include/mep3_navigation/distance_angle/pid_regulator.h b/mep3_navigation/include/mep3_navigation/distance_angle/pid_regulator.h
deleted file mode 100644
index 0a16bb30b..000000000
--- a/mep3_navigation/include/mep3_navigation/distance_angle/pid_regulator.h
+++ /dev/null
@@ -1,46 +0,0 @@
-// Copyright 2021 Memristor Robotics
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MEP3_NAVIGATION__DISTANCE_ANGLE__PID_REGULATOR_H_
-#define MEP3_NAVIGATION__DISTANCE_ANGLE__PID_REGULATOR_H_
-
-#include
-#include
-
-typedef struct
-{
- double reference;
- double feedback;
- double feedback_old;
- double error;
- double integrator;
- double integrator_min;
- double integrator_max;
- double d_term_filtered;
- double d_term_filter_coefficient;
- double kp;
- double ki;
- double kd;
- double clamp_min;
- double clamp_max;
- double command;
-
- bool angle_mode;
-} pid_regulator_t; // pid positional form
-
-void pid_regulator_update(pid_regulator_t * reg);
-void pid_regulator_reset(pid_regulator_t * reg);
-void pid_regulator_set_gains(pid_regulator_t * reg, double kp, double ki, double kd);
-
-#endif // MEP3_NAVIGATION__DISTANCE_ANGLE__PID_REGULATOR_H_
diff --git a/mep3_navigation/include/mep3_navigation/plugins/condition/globally_updated_goal_condition.hpp b/mep3_navigation/include/mep3_navigation/plugins/condition/globally_updated_goal_condition.hpp
deleted file mode 100644
index 27803eae6..000000000
--- a/mep3_navigation/include/mep3_navigation/plugins/condition/globally_updated_goal_condition.hpp
+++ /dev/null
@@ -1,73 +0,0 @@
-// Copyright (c) 2021 Joshua Wallace
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef MEP3_NAVIGATION__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_
-#define MEP3_NAVIGATION__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_
-
-#include
-#include
-
-#include "rclcpp/rclcpp.hpp"
-
-#include "behaviortree_cpp_v3/condition_node.h"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-
-
-namespace mep3_navigation
-{
-/**
- * @brief A BT::ConditionNode that returns SUCCESS when goal is
- * updated on the blackboard and FAILURE otherwise
- */
-class GloballyUpdatedGoalCondition : public BT::ConditionNode
-{
-public:
- /**
- * @brief A constructor for mep3_navigation::GloballyUpdatedGoalCondition
- * @param condition_name Name for the XML tag for this node
- * @param conf BT node configuration
- */
- GloballyUpdatedGoalCondition(
- const std::string & condition_name,
- const BT::NodeConfiguration & conf);
-
- GloballyUpdatedGoalCondition() = delete;
-
- /**
- * @brief The main override required by a BT action
- * @return BT::NodeStatus Status of tick execution
- */
- BT::NodeStatus tick() override;
-
-
- /**
- * @brief Creates list of BT ports
- * @return BT::PortsList Containing node-specific ports
- */
- static BT::PortsList providedPorts()
- {
- return {};
- }
-
-private:
- bool first_time;
- rclcpp::Node::SharedPtr node_;
- geometry_msgs::msg::PoseStamped goal_;
- std::vector goals_;
-};
-
-} // namespace mep3_navigation
-
-
-#endif // MEP3_NAVIGATION__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_
diff --git a/mep3_navigation/include/mep3_navigation/regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/mep3_navigation/include/mep3_navigation/regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
index c72d8bfbc..479c7b142 100644
--- a/mep3_navigation/include/mep3_navigation/regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
+++ b/mep3_navigation/include/mep3_navigation/regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
@@ -59,8 +59,8 @@ class RegulatedPurePursuitController : public nav2_core::Controller
*/
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
- std::string name, const std::shared_ptr & tf,
- const std::shared_ptr & costmap_ros) override;
+ std::string name, std::shared_ptr tf,
+ std::shared_ptr costmap_ros) override;
/**
* @brief Cleanup controller state machine
diff --git a/mep3_navigation/include/mep3_navigation/simple_action_server/simple_action_server.hpp b/mep3_navigation/include/mep3_navigation/simple_action_server/simple_action_server.hpp
deleted file mode 100644
index 680bd0c8b..000000000
--- a/mep3_navigation/include/mep3_navigation/simple_action_server/simple_action_server.hpp
+++ /dev/null
@@ -1,587 +0,0 @@
-// Copyright (c) 2019 Intel Corporation
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_
-#define NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_
-
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include "nav2_util/node_thread.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
-
-namespace nav2_util
-{
-/**
- * @class nav2_util::SimpleActionServer
- * @brief An action server wrapper to make applications simpler using Actions
- */
-template
-class SimpleActionServer
-{
-public:
- // Callback function to complete main work. This should itself deal with its
- // own exceptions, but if for some reason one is thrown, it will be caught
- // in SimpleActionServer and terminate the action itself.
- typedef std::function ExecuteCallback;
-
- // Callback function to notify the user that an exception was thrown that
- // the simple action server caught (or another failure) and the action was
- // terminated. To avoid using, catch exceptions in your application such that
- // the SimpleActionServer will never need to terminate based on failed action
- // ExecuteCallback.
- typedef std::function CompletionCallback;
-
- /**
- * @brief An constructor for SimpleActionServer
- * @param node Ptr to node to make actions
- * @param action_name Name of the action to call
- * @param execute_callback Execution callback function of Action
- * @param server_timeout Timeout to to react to stop or preemption requests
- * @param spin_thread Whether to spin with a dedicated thread internally
- * @param options Options to pass to the underlying rcl_action_server_t
- */
- template
- explicit SimpleActionServer(
- NodeT node, const std::string & action_name, ExecuteCallback execute_callback,
- CompletionCallback completion_callback = nullptr,
- std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500),
- bool spin_thread = false,
- const rcl_action_server_options_t & options = rcl_action_server_get_default_options())
- : SimpleActionServer(
- node->get_node_base_interface(), node->get_node_clock_interface(),
- node->get_node_logging_interface(), node->get_node_waitables_interface(), action_name,
- execute_callback, completion_callback, server_timeout, spin_thread, options)
- {
- }
-
- /**
- * @brief An constructor for SimpleActionServer
- * @param Abstract node interfaces to make actions
- * @param action_name Name of the action to call
- * @param execute_callback Execution callback function of Action
- * @param server_timeout Timeout to to react to stop or preemption requests
- * @param spin_thread Whether to spin with a dedicated thread internally
- * @param options Options to pass to the underlying rcl_action_server_t
- */
- explicit SimpleActionServer(
- rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
- rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
- rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
- rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface,
- const std::string & action_name, ExecuteCallback execute_callback,
- CompletionCallback completion_callback = nullptr,
- std::chrono::milliseconds server_timeout = std::chrono::milliseconds(500),
- bool spin_thread = false,
- const rcl_action_server_options_t & options = rcl_action_server_get_default_options())
- : node_base_interface_(node_base_interface),
- node_clock_interface_(node_clock_interface),
- node_logging_interface_(node_logging_interface),
- node_waitables_interface_(node_waitables_interface),
- action_name_(action_name),
- execute_callback_(execute_callback),
- completion_callback_(completion_callback),
- server_timeout_(server_timeout),
- spin_thread_(spin_thread)
- {
- using namespace std::placeholders; // NOLINT
- if (spin_thread_) {
- callback_group_ = node_base_interface->create_callback_group(
- rclcpp::CallbackGroupType::MutuallyExclusive, false);
- }
- action_server_ = rclcpp_action::create_server(
- node_base_interface_, node_clock_interface_, node_logging_interface_,
- node_waitables_interface_, action_name_,
- std::bind(&SimpleActionServer::handle_goal, this, _1, _2),
- std::bind(&SimpleActionServer::handle_cancel, this, _1),
- std::bind(&SimpleActionServer::handle_accepted, this, _1), options, callback_group_);
- if (spin_thread_) {
- executor_ = std::make_shared();
- executor_->add_callback_group(callback_group_, node_base_interface_);
- executor_thread_ = std::make_unique(executor_);
- }
- }
-
- /**
- * @brief handle the goal requested: accept or reject. This implementation always accepts.
- * @param uuid Goal ID
- * @param Goal A shared pointer to the specific goal
- * @return GoalResponse response of the goal processed
- */
- rclcpp_action::GoalResponse handle_goal(
- const rclcpp_action::GoalUUID & /*uuid*/,
- std::shared_ptr /*goal*/)
- {
- std::lock_guard lock(update_mutex_);
-
- if (!server_active_) {
- return rclcpp_action::GoalResponse::REJECT;
- }
-
- debug_msg("Received request for goal acceptance");
- return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
- }
-
- /**
- * @brief Accepts cancellation requests of action server.
- * @param uuid Goal ID
- * @param Goal A server goal handle to cancel
- * @return CancelResponse response of the goal cancelled
- */
- rclcpp_action::CancelResponse handle_cancel(
- const std::shared_ptr> handle)
- {
- std::lock_guard lock(update_mutex_);
-
- if (!handle->is_active()) {
- warn_msg(
- "Received request for goal cancellation,"
- "but the handle is inactive, so reject the request");
- return rclcpp_action::CancelResponse::REJECT;
- }
-
- debug_msg("Received request for goal cancellation");
- return rclcpp_action::CancelResponse::ACCEPT;
- }
-
- /**
- * @brief Handles accepted goals and adds to preempted queue to switch to
- * @param Goal A server goal handle to cancel
- */
- void handle_accepted(const std::shared_ptr> handle)
- {
- std::lock_guard lock(update_mutex_);
- debug_msg("Receiving a new goal");
-
- if (is_active(current_handle_) || is_running()) {
- debug_msg("An older goal is active, moving the new goal to a pending slot.");
-
- if (is_active(pending_handle_)) {
- debug_msg(
- "The pending slot is occupied."
- " The previous pending goal will be terminated and replaced.");
- terminate(pending_handle_);
- }
- pending_handle_ = handle;
- preempt_requested_ = true;
- } else {
- if (is_active(pending_handle_)) {
- // Shouldn't reach a state with a pending goal but no current one.
- error_msg("Forgot to handle a preemption. Terminating the pending goal.");
- terminate(pending_handle_);
- preempt_requested_ = false;
- }
-
- current_handle_ = handle;
-
- // Return quickly to avoid blocking the executor, so spin up a new thread
- debug_msg("Executing goal asynchronously.");
- execution_future_ = std::async(std::launch::async, [this]() { work(); });
- }
- }
-
- /**
- * @brief Computed background work and processes stop requests
- */
- void work()
- {
- while (rclcpp::ok() && !stop_execution_ && is_active(current_handle_)) {
- debug_msg("Executing the goal...");
- try {
- execute_callback_();
- } catch (std::exception & ex) {
- RCLCPP_ERROR(
- node_logging_interface_->get_logger(),
- "Action server failed while executing action callback: \"%s\"", ex.what());
- terminate_all();
- completion_callback_();
- return;
- }
-
- debug_msg("Blocking processing of new goal handles.");
- std::lock_guard lock(update_mutex_);
-
- if (stop_execution_) {
- warn_msg("Stopping the thread per request.");
- terminate_all();
- completion_callback_();
- break;
- }
-
- if (is_active(current_handle_)) {
- warn_msg("Current goal was not completed successfully.");
- terminate(current_handle_);
- completion_callback_();
- }
-
- if (is_active(pending_handle_)) {
- debug_msg("Executing a pending handle on the existing thread.");
- accept_pending_goal();
- } else {
- debug_msg("Done processing available goals.");
- break;
- }
- }
- debug_msg("Worker thread done.");
- }
-
- /**
- * @brief Active action server
- */
- void activate()
- {
- std::lock_guard lock(update_mutex_);
- server_active_ = true;
- stop_execution_ = false;
- }
-
- /**
- * @brief Deactive action server
- */
- void deactivate()
- {
- debug_msg("Deactivating...");
-
- {
- std::lock_guard lock(update_mutex_);
- server_active_ = false;
- stop_execution_ = true;
- }
-
- if (!execution_future_.valid()) {
- return;
- }
-
- if (is_running()) {
- warn_msg(
- "Requested to deactivate server but goal is still executing."
- " Should check if action server is running before deactivating.");
- }
-
- using namespace std::chrono; //NOLINT
- auto start_time = steady_clock::now();
- while (execution_future_.wait_for(milliseconds(100)) != std::future_status::ready) {
- info_msg("Waiting for async process to finish.");
- if (steady_clock::now() - start_time >= server_timeout_) {
- terminate_all();
- completion_callback_();
- throw std::runtime_error("Action callback is still running and missed deadline to stop");
- }
- }
-
- debug_msg("Deactivation completed.");
- }
-
- /**
- * @brief Whether the action server is munching on a goal
- * @return bool If its running or not
- */
- bool is_running()
- {
- return execution_future_.valid() && (execution_future_.wait_for(std::chrono::milliseconds(0)) ==
- std::future_status::timeout);
- }
-
- /**
- * @brief Whether the action server is active or not
- * @return bool If its active or not
- */
- bool is_server_active()
- {
- std::lock_guard lock(update_mutex_);
- return server_active_;
- }
-
- /**
- * @brief Whether the action server has been asked to be preempted with a new goal
- * @return bool If there's a preemption request or not
- */
- bool is_preempt_requested() const
- {
- std::lock_guard lock(update_mutex_);
- return preempt_requested_;
- }
-
- /**
- * @brief Accept pending goals
- * @return Goal Ptr to the goal that's going to be accepted
- */
- const std::shared_ptr accept_pending_goal()
- {
- std::lock_guard lock(update_mutex_);
-
- if (!pending_handle_ || !pending_handle_->is_active()) {
- error_msg("Attempting to get pending goal when not available");
- return std::shared_ptr();
- }
-
- if (is_active(current_handle_) && current_handle_ != pending_handle_) {
- debug_msg("Cancelling the previous goal");
- current_handle_->abort(empty_result());
- }
-
- current_handle_ = pending_handle_;
- pending_handle_.reset();
- preempt_requested_ = false;
-
- debug_msg("Preempted goal");
-
- return current_handle_->get_goal();
- }
-
- /**
- * @brief Terminate pending goals
- */
- void terminate_pending_goal()
- {
- std::lock_guard lock(update_mutex_);
-
- if (!pending_handle_ || !pending_handle_->is_active()) {
- error_msg("Attempting to terminate pending goal when not available");
- return;
- }
-
- terminate(pending_handle_);
- preempt_requested_ = false;
-
- debug_msg("Pending goal terminated");
- }
-
- /**
- * @brief Get the current goal object
- * @return Goal Ptr to the goal that's being processed currently
- */
- const std::shared_ptr get_current_goal() const
- {
- std::lock_guard lock(update_mutex_);
-
- if (!is_active(current_handle_)) {
- error_msg("A goal is not available or has reached a final state");
- return std::shared_ptr();
- }
-
- return current_handle_->get_goal();
- }
-
- /**
- * @brief Get the pending goal object
- * @return Goal Ptr to the goal that's pending
- */
- const std::shared_ptr get_pending_goal() const
- {
- std::lock_guard lock(update_mutex_);
-
- if (!pending_handle_ || !pending_handle_->is_active()) {
- error_msg("Attempting to get pending goal when not available");
- return std::shared_ptr();
- }
-
- return pending_handle_->get_goal();
- }
-
- /**
- * @brief Whether or not a cancel command has come in
- * @return bool Whether a cancel command has been requested or not
- */
- bool is_cancel_requested() const
- {
- std::lock_guard lock(update_mutex_);
-
- // A cancel request is assumed if either handle is canceled by the client.
-
- if (current_handle_ == nullptr) {
- error_msg("Checking for cancel but current goal is not available");
- return false;
- }
-
- if (pending_handle_ != nullptr) {
- return pending_handle_->is_canceling();
- }
-
- return current_handle_->is_canceling();
- }
-
- /**
- * @brief Terminate all pending and active actions
- * @param result A result object to send to the terminated actions
- */
- void terminate_all(
- typename std::shared_ptr result =
- std::make_shared())
- {
- std::lock_guard lock(update_mutex_);
- terminate(current_handle_, result);
- terminate(pending_handle_, result);
- preempt_requested_ = false;
- }
-
- /**
- * @brief Terminate the active action
- * @param result A result object to send to the terminated action
- */
- void terminate_current(
- typename std::shared_ptr result =
- std::make_shared())
- {
- std::lock_guard lock(update_mutex_);
- terminate(current_handle_, result);
- }
-
- /**
- * @brief Return success of the active action
- * @param result A result object to send to the terminated actions
- */
- void succeeded_current(
- typename std::shared_ptr result =
- std::make_shared())
- {
- std::lock_guard lock(update_mutex_);
-
- if (is_active(current_handle_)) {
- debug_msg("Setting succeed on current goal.");
- current_handle_->succeed(result);
- current_handle_.reset();
- }
- }
-
- /**
- * @brief Publish feedback to the action server clients
- * @param feedback A feedback object to send to the clients
- */
- void publish_feedback(typename std::shared_ptr feedback)
- {
- if (!is_active(current_handle_)) {
- error_msg("Trying to publish feedback when the current goal handle is not active");
- return;
- }
-
- current_handle_->publish_feedback(feedback);
- }
-
-protected:
- // The SimpleActionServer isn't itself a node, so it needs interfaces to one
- rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;
- rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface_;
- rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_;
- rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface_;
- std::string action_name_;
-
- ExecuteCallback execute_callback_;
- CompletionCallback completion_callback_;
- std::future execution_future_;
- bool stop_execution_{false};
-
- mutable std::recursive_mutex update_mutex_;
- bool server_active_{false};
- bool preempt_requested_{false};
- std::chrono::milliseconds server_timeout_;
-
- std::shared_ptr> current_handle_;
- std::shared_ptr> pending_handle_;
-
- typename rclcpp_action::Server::SharedPtr action_server_;
- bool spin_thread_;
- rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr};
- rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
- std::unique_ptr executor_thread_;
-
- /**
- * @brief Generate an empty result object for an action type
- */
- constexpr auto empty_result() const { return std::make_shared(); }
-
- /**
- * @brief Whether a given goal handle is currently active
- * @param handle Goal handle to check
- * @return Whether this goal handle is active
- */
- constexpr bool is_active(
- const std::shared_ptr> handle) const
- {
- return handle != nullptr && handle->is_active();
- }
-
- /**
- * @brief Terminate a particular action with a result
- * @param handle goal handle to terminate
- * @param the Results object to terminate the action with
- */
- void terminate(
- std::shared_ptr> handle,
- typename std::shared_ptr result =
- std::make_shared())
- {
- std::lock_guard lock(update_mutex_);
-
- if (is_active(handle)) {
- if (handle->is_canceling()) {
- warn_msg("Client requested to cancel the goal. Cancelling.");
- handle->canceled(result);
- } else {
- warn_msg("Aborting handle.");
- handle->abort(result);
- }
- handle.reset();
- }
- }
-
- /**
- * @brief Info logging
- */
- void info_msg(const std::string & msg) const
- {
- RCLCPP_INFO(
- node_logging_interface_->get_logger(), "[%s] [ActionServer] %s", action_name_.c_str(),
- msg.c_str());
- }
-
- /**
- * @brief Debug logging
- */
- void debug_msg(const std::string & msg) const
- {
- RCLCPP_DEBUG(
- node_logging_interface_->get_logger(), "[%s] [ActionServer] %s", action_name_.c_str(),
- msg.c_str());
- }
-
- /**
- * @brief Error logging
- */
- void error_msg(const std::string & msg) const
- {
- RCLCPP_ERROR(
- node_logging_interface_->get_logger(), "[%s] [ActionServer] %s", action_name_.c_str(),
- msg.c_str());
- }
-
- /**
- * @brief Warn logging
- */
- void warn_msg(const std::string & msg) const
- {
- RCLCPP_WARN(
- node_logging_interface_->get_logger(), "[%s] [ActionServer] %s", action_name_.c_str(),
- msg.c_str());
- }
-};
-
-} // namespace nav2_util
-
-#endif // NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_
diff --git a/mep3_navigation/launch/navigation_launch.py b/mep3_navigation/launch/navigation_launch.py
index 707dac245..bf02c6ce8 100644
--- a/mep3_navigation/launch/navigation_launch.py
+++ b/mep3_navigation/launch/navigation_launch.py
@@ -17,12 +17,10 @@
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
-from launch.actions import (DeclareLaunchArgument, GroupAction, SetEnvironmentVariable)
-from launch.conditions import IfCondition
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-from launch_ros.actions import PushRosNamespace
-from nav2_common.launch import RewrittenYaml
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
def generate_launch_description():
@@ -31,39 +29,17 @@ def generate_launch_description():
# Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
- use_namespace = LaunchConfiguration('use_namespace')
- map_yaml_file = LaunchConfiguration('map')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
- autostart = LaunchConfiguration('autostart')
nav2_bt_xml_file = LaunchConfiguration('nav2_bt_xml_file')
+ log_level = LaunchConfiguration('log_level')
lifecycle_nodes = ['controller_server',
'planner_server',
- 'recoveries_server',
- 'bt_navigator',
- 'map_server']
-
- # Map fully qualified names to relative ones so the node's namespace can be prepended.
- # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
- # /~https://github.com/ros/geometry2/issues/32
- # /~https://github.com/ros/robot_state_publisher/pull/30
- # TODO(orduno) Substitute with `PushNodeRemapping`
- # /~https://github.com/ros2/launch_ros/issues/56
- remappings = [('/tf', 'tf'),
- ('/tf_static', 'tf_static')]
-
- # Create our own temporary YAML files that include substitutions
- param_substitutions = {
- 'default_nav_to_pose_bt_xml': nav2_bt_xml_file,
- 'use_sim_time': use_sim_time,
- 'yaml_filename': map_yaml_file}
-
- configured_params = RewrittenYaml(
- source_file=params_file,
- root_key=namespace,
- param_rewrites=param_substitutions,
- convert_types=True)
+ 'behavior_server',
+ 'bt_navigator']
+ remappings = [('/tf', ['tf']),
+ ('/tf_static', ['tf_static'])]
stdout_linebuf_envvar = SetEnvironmentVariable(
'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
@@ -75,28 +51,14 @@ def generate_launch_description():
'behavior_trees',
'navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml',
),
- description='Full path to the parameter YAML file to use for configuring the behavior tree'
+ description='Full path to the YAML file used with navigation behavior'
)
declare_namespace_cmd = DeclareLaunchArgument(
'namespace',
- default_value='',
+ default_value='big',
description='Top-level namespace')
- declare_use_namespace_cmd = DeclareLaunchArgument(
- 'use_namespace',
- default_value='false',
- description='Whether to apply a namespace to the navigation stack')
-
- declare_slam_cmd = DeclareLaunchArgument(
- 'slam',
- default_value='False',
- description='Whether run a SLAM')
-
- declare_map_yaml_cmd = DeclareLaunchArgument(
- 'map',
- description='Full path to map yaml file to load')
-
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='false',
@@ -108,82 +70,74 @@ def generate_launch_description():
mep3_navigation_dir, 'params', 'nav2_params_big.yaml'),
description='Full path to the ROS2 parameters file to use for all launched nodes')
- declare_autostart_cmd = DeclareLaunchArgument(
- 'autostart', default_value='true',
- description='Automatically startup the nav2 stack')
-
- # Specify the actions
- bringup_cmd_group = GroupAction([
- PushRosNamespace(
- condition=IfCondition(use_namespace),
- namespace=namespace),
- Node(
- package='nav2_map_server',
- executable='map_server',
- name='map_server',
- output='screen',
- parameters=[configured_params],
- remappings=remappings),
- Node(
- package='nav2_controller',
- executable='controller_server',
- output='screen',
- parameters=[configured_params],
- remappings=remappings),
-
- Node(
- package='nav2_planner',
- executable='planner_server',
- name='planner_server',
- output='screen',
- parameters=[configured_params],
- remappings=remappings),
-
- Node(
- package='nav2_recoveries',
- executable='recoveries_server',
- name='recoveries_server',
- output='screen',
- parameters=[configured_params],
- remappings=remappings),
+ # TODO: Switch to warn later
+ declare_log_level_cmd = DeclareLaunchArgument(
+ 'log_level', default_value='warn',
+ description='log level')
+
+ load_composable_nodes = ComposableNodeContainer(
+ namespace=namespace,
+ remappings=remappings,
+ parameters=[params_file],
+ name='nav2_container',
+ package='rclcpp_components',
+ executable='component_container',
+ arguments=['--ros-args', '--log-level', log_level],
+ composable_node_descriptions=[
+ ComposableNode(
+ package='nav2_controller',
+ plugin='nav2_controller::ControllerServer',
+ name='controller_server',
+ namespace=namespace,
+ parameters=[
+ params_file
+ ],
+ remappings=remappings),
+ ComposableNode(
+ package='nav2_planner',
+ plugin='nav2_planner::PlannerServer',
+ name='planner_server',
+ namespace=namespace,
+ parameters=[
+ params_file
+ ],
+ remappings=remappings),
+ ComposableNode(
+ package='nav2_behaviors',
+ plugin='behavior_server::BehaviorServer',
+ name='behavior_server',
+ namespace=namespace,
+ parameters=[
+ params_file
+ ],
+ remappings=remappings),
+ ComposableNode(
+ package='nav2_bt_navigator',
+ plugin='nav2_bt_navigator::BtNavigator',
+ name='bt_navigator',
+ namespace=namespace,
+ parameters=[
+ params_file,
+ {'default_nav_to_pose_bt_xml': nav2_bt_xml_file}
+ ],
+ remappings=remappings),
+ ComposableNode(
+ package='nav2_lifecycle_manager',
+ plugin='nav2_lifecycle_manager::LifecycleManager',
+ name='lifecycle_manager_navigation',
+ namespace=namespace,
+ parameters=[{'use_sim_time': use_sim_time,
+ 'autostart': True,
+ 'node_names': lifecycle_nodes}]),
+ ],
+ )
- Node(
- package='nav2_bt_navigator',
- executable='bt_navigator',
- name='bt_navigator',
- output='screen',
- parameters=[configured_params],
- remappings=remappings),
- Node(
- package='nav2_lifecycle_manager',
- executable='lifecycle_manager',
- name='lifecycle_manager_navigation',
- output='screen',
- parameters=[
- {'use_sim_time': use_sim_time},
- {'autostart': autostart},
- {'node_names': lifecycle_nodes},
- {'bond_timeout': 8.0}
- ]),
+ return LaunchDescription([
+ stdout_linebuf_envvar,
+ declare_namespace_cmd,
+ declare_use_sim_time_cmd,
+ declare_params_file_cmd,
+ nav2_bt_xml_file_cmd,
+ declare_log_level_cmd,
+ load_composable_nodes
])
-
- # Create the launch description and populate
- ld = LaunchDescription()
-
- # Set environment variables
- ld.add_action(stdout_linebuf_envvar)
-
- # Declare the launch options
- ld.add_action(declare_namespace_cmd)
- ld.add_action(declare_use_namespace_cmd)
- ld.add_action(declare_slam_cmd)
- ld.add_action(declare_map_yaml_cmd)
- ld.add_action(declare_use_sim_time_cmd)
- ld.add_action(declare_params_file_cmd)
- ld.add_action(declare_autostart_cmd)
- ld.add_action(nav2_bt_xml_file_cmd)
-
- # Add the actions to launch all of the navigation nodes
- ld.add_action(bringup_cmd_group)
-
- return ld
diff --git a/mep3_navigation/params/config_regulator_big.yaml b/mep3_navigation/params/config_regulator_big.yaml
deleted file mode 100644
index 05de29e9a..000000000
--- a/mep3_navigation/params/config_regulator_big.yaml
+++ /dev/null
@@ -1,17 +0,0 @@
-big:
- distance_angle_regulator:
- ros__parameters:
- kp_distance: 5.0
- ki_distance: 0.0
- kd_distance: 3.0
- d_term_filter_distance: 0.1
-
- kp_angle: 8.0
- ki_angle: 0.0
- kd_angle: 4.0
- d_term_filter_angle: 0.1
-
- distance_goal_tolerance: 0.002
- angle_goal_tolerance: 0.018
-
- control_frequency: 100.0
\ No newline at end of file
diff --git a/mep3_navigation/params/config_regulator_small.yaml b/mep3_navigation/params/config_regulator_small.yaml
deleted file mode 100644
index 4778c6415..000000000
--- a/mep3_navigation/params/config_regulator_small.yaml
+++ /dev/null
@@ -1,17 +0,0 @@
-small:
- distance_angle_regulator:
- ros__parameters:
- kp_distance: 11.0
- ki_distance: 0.0
- kd_distance: 3.0
- d_term_filter_distance: 0.1
-
- kp_angle: 9.0
- ki_angle: 0.0
- kd_angle: 2.0
- d_term_filter_angle: 0.1
-
- distance_goal_tolerance: 0.002
- angle_goal_tolerance: 0.018
-
- control_frequency: 100.0
diff --git a/mep3_navigation/params/nav2_params_big.yaml b/mep3_navigation/params/nav2_params_big.yaml
index 88ae7d19f..e806e616e 100644
--- a/mep3_navigation/params/nav2_params_big.yaml
+++ b/mep3_navigation/params/nav2_params_big.yaml
@@ -1,79 +1,77 @@
-bt_navigator:
+/big/bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
- odom_topic: /odom
- bt_loop_duration: 5
- default_server_timeout: 100
- enable_groot_monitoring: False
- # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
- # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
- # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
- # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+ odom_topic: /big/odom
+ bt_loop_duration: 10
+ default_server_timeout: 20
default_nav_to_pose_bt_xml: navigate_to_pose_w_replanning_and_recovery.xml
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
+ - nav2_smooth_path_action_bt_node
- nav2_follow_path_action_bt_node
- - nav2_back_up_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
+ - nav2_back_up_action_bt_node
+ - nav2_drive_on_heading_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
+ - nav2_globally_updated_goal_condition_bt_node
+ - nav2_is_path_valid_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
+ - nav2_truncate_path_local_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
+ - nav2_path_expiring_timer_condition
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- - nav2_is_battery_low_condition_bt_node
+ - nav2_goal_updated_controller_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
- - nav2_globally_updated_goal_condition_bt_node
+ - nav2_controller_cancel_bt_node
+ - nav2_path_longer_on_approach_bt_node
+ - nav2_wait_cancel_bt_node
+ - nav2_spin_cancel_bt_node
+ - nav2_back_up_cancel_bt_node
+ - nav2_drive_on_heading_cancel_bt_node
-bt_navigator_rclcpp_node:
+/big/controller_server:
ros__parameters:
use_sim_time: True
-
-controller_server:
- ros__parameters:
- use_sim_time: True
- controller_frequency: 40.0
+ controller_frequency: 20.0
min_x_velocity_threshold: 0.001
- min_y_velocity_threshold: 0.001
+ min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
+ failure_tolerance: 0.3
progress_checker_plugin: "progress_checker"
- goal_checker_plugin: "goal_checker"
+ goal_checker_plugins: ["general_goal_checker"]
controller_plugins: ["FollowPath"]
-
- # Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.6
movement_time_allowance: 10.0
- # Goal checker parameters
- goal_checker:
+ general_goal_checker:
+ stateful: True
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.08
yaw_goal_tolerance: 0.05
- stateful: true
- # Controller parameters
- # RPP Custom controller for Memristor
FollowPath:
plugin: "mep3_navigation::RegulatedPurePursuitController"
desired_linear_vel: 0.8
@@ -104,40 +102,8 @@ controller_server:
cost_scaling_dist: 0.7
cost_scaling_gain: 1.0
inflation_cost_scaling_factor: 5.0
- # RPP Controller from Navigation2 Package
- NAV2RPPFollowPath:
- plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
- desired_linear_vel: 0.4
- # acceleration limiting is not implemented yet
- max_linear_accel: 0.0001
- max_linear_decel: 0.0001
- lookahead_dist: 0.2
- min_lookahead_dist: 0.05
- max_lookahead_dist: 0.4
- lookahead_time: 1.0
- rotate_to_heading_angular_vel: 2.5
- transform_tolerance: 0.1
- use_velocity_scaled_lookahead_dist: true
- min_approach_linear_velocity: 0.05
- use_approach_linear_velocity_scaling: true
- max_allowed_time_to_collision: 0.6
- use_regulated_linear_velocity_scaling: true
- use_cost_regulated_linear_velocity_scaling: false
- regulated_linear_scaling_min_radius: 0.2
- regulated_linear_scaling_min_speed: 0.1
- allow_reversing: false
- use_rotate_to_heading: true
- rotate_to_heading_min_angle: 0.785
- max_angular_accel: 3.5
- cost_scaling_dist: 0.7
- cost_scaling_gain: 1.0
- inflation_cost_scaling_factor: 5.0
-
-controller_server_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-local_costmap:
+/big/local_costmap:
local_costmap:
ros__parameters:
update_frequency: 10.0
@@ -146,36 +112,34 @@ local_costmap:
robot_base_frame: base_link
use_sim_time: True
rolling_window: True
- width: 3
- height: 3
- resolution: 0.01
+ width: 1
+ height: 1
+ resolution: 0.07
robot_radius: 0.17
- # footprint: "[ [0.1, 0.15], [0.1, -0.15], [-0.1, -0.15], [-0.1, 0.15] ]"
+ use_maximum: true
plugins: ["obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
- topic: /big/scan_inflated
+ topic: /big/scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
inf_is_valid: True
+ raytrace_max_range: 3.0
+ raytrace_min_range: 0.0
+ obstacle_max_range: 2.5
+ obstacle_min_range: 0.0
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 4.0
inflation_radius: 1.0
always_send_full_costmap: True
- local_costmap_client:
- ros__parameters:
- use_sim_time: True
- local_costmap_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-global_costmap:
+/big/global_costmap:
global_costmap:
ros__parameters:
update_frequency: 10.0
@@ -183,24 +147,26 @@ global_costmap:
global_frame: map
robot_base_frame: base_link
use_sim_time: True
+ rolling_window: True
robot_radius: 0.2
- resolution: 0.01
- track_unknown_space: true
- plugins: ["static_layer", "obstacle_layer", "inflation_layer", "temporal_obstacle_layer"]
+ resolution: 0.07
+ use_maximum: true
+ plugins: ["obstacle_layer", "inflation_layer", "temporal_obstacle_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
- topic: /big/scan_inflated
+ topic: /big/scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
inf_is_valid: True
- static_layer:
- plugin: "nav2_costmap_2d::StaticLayer"
- map_subscribe_transient_local: True
+ raytrace_max_range: 3.0
+ raytrace_min_range: 0.0
+ obstacle_max_range: 2.5
+ obstacle_min_range: 0.0
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 4.0
@@ -210,27 +176,8 @@ global_costmap:
add_obstacle_topic: /big/add_obstacle
remove_obstacle_topic: /big/remove_obstacle
always_send_full_costmap: True
- global_costmap_client:
- ros__parameters:
- use_sim_time: True
- global_costmap_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-map_server:
- ros__parameters:
- use_sim_time: True
- yaml_filename: "map.yaml"
-
-map_saver:
- ros__parameters:
- use_sim_time: True
- save_map_timeout: 5.0
- free_thresh_default: 0.25
- occupied_thresh_default: 0.65
- map_subscribe_transient_local: True
-
-planner_server:
+/big/planner_server:
ros__parameters:
use_sim_time: True
planner_plugins: ["GridBased"]
@@ -238,58 +185,29 @@ planner_server:
GridBased:
plugin: 'nav2_navfn_planner/NavfnPlanner'
use_astar: True
- allow_unknown: false
+ allow_unknown: true
use_final_approach_orientation: False
tolerance: 0.025
- _GridBased:
- plugin: "nav2_smac_planner/SmacPlanner2D"
- tolerance: 0.02
- downsample_costmap: false # whether or not to downsample the map
- downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
- allow_unknown: true # allow traveling in unknown space
- max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
- max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance
- max_planning_time: 0.2 # max time in s for planner to plan, smooth
- motion_model_for_search: "MOORE" # 2D Moore, Von Neumann
- cost_travel_multiplier: 5.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
- use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true)
- smoother:
- max_iterations: 1000
- w_smooth: 0.4
- w_data: 0.2
- tolerance: 1e-10
-planner_server_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-
-recoveries_server:
+/big/behavior_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
- recovery_plugins: ["spin", "backup", "wait"]
+ behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
spin:
- plugin: "nav2_recoveries/Spin"
+ plugin: "nav2_behaviors/Spin"
backup:
- plugin: "nav2_recoveries/BackUp"
+ plugin: "nav2_behaviors/BackUp"
+ drive_on_heading:
+ plugin: "nav2_behaviors/DriveOnHeading"
wait:
- plugin: "nav2_recoveries/Wait"
+ plugin: "nav2_behaviors/Wait"
global_frame: odom
robot_base_frame: base_link
- transform_timeout: 0.1
+ transform_tolerance: 0.1
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
-
-waypoint_follower:
- ros__parameters:
- loop_rate: 20
- stop_on_failure: false
- waypoint_task_executor_plugin: "wait_at_waypoint"
- wait_at_waypoint:
- plugin: "nav2_waypoint_follower::WaitAtWaypoint"
- enabled: True
- waypoint_pause_duration: 200
diff --git a/mep3_navigation/params/nav2_params_small.yaml b/mep3_navigation/params/nav2_params_small.yaml
index 32dee7621..0b14b2d3b 100644
--- a/mep3_navigation/params/nav2_params_small.yaml
+++ b/mep3_navigation/params/nav2_params_small.yaml
@@ -1,79 +1,77 @@
-bt_navigator:
+/small/bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
- odom_topic: /odom
- bt_loop_duration: 5
- default_server_timeout: 100
- enable_groot_monitoring: False
- # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
- # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
- # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
- # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+ odom_topic: /small/odom
+ bt_loop_duration: 10
+ default_server_timeout: 20
default_nav_to_pose_bt_xml: navigate_to_pose_w_replanning_and_recovery.xml
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
+ - nav2_smooth_path_action_bt_node
- nav2_follow_path_action_bt_node
- - nav2_back_up_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
+ - nav2_back_up_action_bt_node
+ - nav2_drive_on_heading_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
+ - nav2_globally_updated_goal_condition_bt_node
+ - nav2_is_path_valid_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
+ - nav2_truncate_path_local_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
+ - nav2_path_expiring_timer_condition
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- - nav2_is_battery_low_condition_bt_node
+ - nav2_goal_updated_controller_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
- - nav2_globally_updated_goal_condition_bt_node
+ - nav2_controller_cancel_bt_node
+ - nav2_path_longer_on_approach_bt_node
+ - nav2_wait_cancel_bt_node
+ - nav2_spin_cancel_bt_node
+ - nav2_back_up_cancel_bt_node
+ - nav2_drive_on_heading_cancel_bt_node
-bt_navigator_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-
-controller_server:
+/small/controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
- min_y_velocity_threshold: 0.001
+ min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
+ failure_tolerance: 0.3
progress_checker_plugin: "progress_checker"
- goal_checker_plugin: "goal_checker"
- controller_plugins: ["FollowPath", "FollowPathFast"]
-
- # Progress checker parameters
+ goal_checker_plugins: ["general_goal_checker"]
+ controller_plugins: ["FollowPath"]
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
- required_movement_radius: 0.5
+ required_movement_radius: 0.6
movement_time_allowance: 10.0
- # Goal checker parameters
- goal_checker:
+ general_goal_checker:
+ stateful: True
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.08
yaw_goal_tolerance: 0.05
- stateful: true
- # Controller parameters
- # RPP Custom controller for Memristor
FollowPath:
plugin: "mep3_navigation::RegulatedPurePursuitController"
desired_linear_vel: 0.8
@@ -83,8 +81,8 @@ controller_server:
max_angular_jerk: 99999.0
lookahead_dist: 0.2
min_lookahead_dist: 0.15
- max_lookahead_dist: 0.6
- lookahead_time: 0.75
+ max_lookahead_dist: 0.5
+ lookahead_time: 0.625
rotate_to_heading_angular_vel: 15.0
transform_tolerance: 0.1
use_velocity_scaled_lookahead_dist: true
@@ -98,165 +96,88 @@ controller_server:
allow_reversing: true
use_rotate_to_heading: false
use_rotate_to_goal: true
- rotate_to_heading_min_angle: 0.785
- max_angular_accel: 15.0
- cost_scaling_dist: 0.7
- cost_scaling_gain: 1.0
- inflation_cost_scaling_factor: 5.0
- # RPP Custom controller for Memristor - VERY FAST
- FollowPathFast:
- plugin: "mep3_navigation::RegulatedPurePursuitController"
- desired_linear_vel: 2.0
- max_linear_accel: 2.4
- max_linear_decel: 2.4
- max_linear_jerk: 99999999.0
- max_angular_jerk: 99999999.0
- lookahead_dist: 0.9
- min_lookahead_dist: 0.15
- max_lookahead_dist: 0.9
- lookahead_time: 0.45
- rotate_to_heading_angular_vel: 14.0
- transform_tolerance: 0.1
- use_velocity_scaled_lookahead_dist: false
- min_approach_linear_velocity: 0.05
- use_approach_linear_velocity_scaling: true
- max_allowed_time_to_collision: 0.6
- use_regulated_linear_velocity_scaling: false
- use_cost_regulated_linear_velocity_scaling: false
- regulated_linear_scaling_min_radius: 0.2
- regulated_linear_scaling_min_speed: 0.0
- allow_reversing: true
- use_rotate_to_heading: false
- use_rotate_to_goal: true
kp_angle: 6.0
rotate_to_heading_min_angle: 0.785
max_angular_accel: 15.0
cost_scaling_dist: 0.7
cost_scaling_gain: 1.0
inflation_cost_scaling_factor: 5.0
- # RPP Controller from Navigation2 Package
- NAV2RPPFollowPath:
- plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
- desired_linear_vel: 0.4
- # acceleration limiting is not implemented yet
- max_linear_accel: 0.0001
- max_linear_decel: 0.0001
- lookahead_dist: 0.2
- min_lookahead_dist: 0.05
- max_lookahead_dist: 0.4
- lookahead_time: 1.0
- rotate_to_heading_angular_vel: 2.5
- transform_tolerance: 0.1
- use_velocity_scaled_lookahead_dist: true
- min_approach_linear_velocity: 0.05
- use_approach_linear_velocity_scaling: true
- max_allowed_time_to_collision: 0.6
- use_regulated_linear_velocity_scaling: true
- use_cost_regulated_linear_velocity_scaling: false
- regulated_linear_scaling_min_radius: 0.2
- regulated_linear_scaling_min_speed: 0.1
- allow_reversing: false
- use_rotate_to_heading: true
- rotate_to_heading_min_angle: 0.785
- max_angular_accel: 3.5
- cost_scaling_dist: 0.7
- cost_scaling_gain: 1.0
- inflation_cost_scaling_factor: 5.0
-
-controller_server_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-local_costmap:
+/small/local_costmap:
local_costmap:
ros__parameters:
- update_frequency: 5.0
+ update_frequency: 10.0
publish_frequency: 5.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
rolling_window: True
- width: 3
- height: 3
- resolution: 0.01
+ width: 1
+ height: 1
+ resolution: 0.07
robot_radius: 0.17
- # footprint: "[ [0.1, 0.15], [0.1, -0.15], [-0.1, -0.15], [-0.1, 0.15] ]"
+ use_maximum: true
plugins: ["obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
- topic: /small/scan_inflated
+ topic: /small/scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
inf_is_valid: True
+ raytrace_max_range: 3.0
+ raytrace_min_range: 0.0
+ obstacle_max_range: 2.5
+ obstacle_min_range: 0.0
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 4.0
inflation_radius: 1.0
always_send_full_costmap: True
- local_costmap_client:
- ros__parameters:
- use_sim_time: True
- local_costmap_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-global_costmap:
+/small/global_costmap:
global_costmap:
ros__parameters:
- update_frequency: 2.0
+ update_frequency: 10.0
publish_frequency: 2.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
+ rolling_window: True
robot_radius: 0.2
- resolution: 0.02
- track_unknown_space: true
- plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+ resolution: 0.07
+ use_maximum: true
+ plugins: ["obstacle_layer", "inflation_layer", "temporal_obstacle_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
- topic: /small/scan_inflated
+ topic: /small/scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
inf_is_valid: True
- static_layer:
- plugin: "nav2_costmap_2d::StaticLayer"
- map_subscribe_transient_local: True
+ raytrace_max_range: 3.0
+ raytrace_min_range: 0.0
+ obstacle_max_range: 2.5
+ obstacle_min_range: 0.0
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 4.0
inflation_radius: 1.0
+ temporal_obstacle_layer:
+ plugin: "mep3_navigation::TemporalObstacleLayer"
+ add_obstacle_topic: /small/add_obstacle
+ remove_obstacle_topic: /small/remove_obstacle
always_send_full_costmap: True
- global_costmap_client:
- ros__parameters:
- use_sim_time: True
- global_costmap_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-map_server:
- ros__parameters:
- use_sim_time: True
- yaml_filename: "map.yaml"
-
-map_saver:
- ros__parameters:
- use_sim_time: True
- save_map_timeout: 5.0
- free_thresh_default: 0.25
- occupied_thresh_default: 0.65
- map_subscribe_transient_local: True
-
-planner_server:
+/small/planner_server:
ros__parameters:
use_sim_time: True
planner_plugins: ["GridBased"]
@@ -264,58 +185,29 @@ planner_server:
GridBased:
plugin: 'nav2_navfn_planner/NavfnPlanner'
use_astar: True
- allow_unknown: false
+ allow_unknown: true
use_final_approach_orientation: False
- tolerance: 0.005
- _GridBased:
- plugin: "nav2_smac_planner/SmacPlanner2D"
- tolerance: 0.02
- downsample_costmap: false # whether or not to downsample the map
- downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
- allow_unknown: true # allow traveling in unknown space
- max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
- max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance
- max_planning_time: 0.2 # max time in s for planner to plan, smooth
- motion_model_for_search: "MOORE" # 2D Moore, Von Neumann
- cost_travel_multiplier: 5.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
- use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true)
- smoother:
- max_iterations: 1000
- w_smooth: 0.4
- w_data: 0.2
- tolerance: 1e-10
+ tolerance: 0.025
-planner_server_rclcpp_node:
- ros__parameters:
- use_sim_time: True
-
-recoveries_server:
+/small/behavior_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
- recovery_plugins: ["spin", "backup", "wait"]
+ behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
spin:
- plugin: "nav2_recoveries/Spin"
+ plugin: "nav2_behaviors/Spin"
backup:
- plugin: "nav2_recoveries/BackUp"
+ plugin: "nav2_behaviors/BackUp"
+ drive_on_heading:
+ plugin: "nav2_behaviors/DriveOnHeading"
wait:
- plugin: "nav2_recoveries/Wait"
+ plugin: "nav2_behaviors/Wait"
global_frame: odom
robot_base_frame: base_link
- transform_timeout: 0.1
+ transform_tolerance: 0.1
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
-
-waypoint_follower:
- ros__parameters:
- loop_rate: 20
- stop_on_failure: false
- waypoint_task_executor_plugin: "wait_at_waypoint"
- wait_at_waypoint:
- plugin: "nav2_waypoint_follower::WaitAtWaypoint"
- enabled: True
- waypoint_pause_duration: 200
diff --git a/mep3_navigation/src/distance_angle/.clang-format b/mep3_navigation/src/distance_angle/.clang-format
deleted file mode 100644
index 33bf2a3b9..000000000
--- a/mep3_navigation/src/distance_angle/.clang-format
+++ /dev/null
@@ -1,137 +0,0 @@
----
-Language: Cpp
-# BasedOnStyle: LLVM
-AccessModifierOffset: -2
-AlignAfterOpenBracket: Align
-AlignConsecutiveMacros: false
-AlignConsecutiveAssignments: false
-AlignConsecutiveDeclarations: false
-AlignEscapedNewlines: Right
-AlignOperands: true
-AlignTrailingComments: true
-AllowAllArgumentsOnNextLine: true
-AllowAllConstructorInitializersOnNextLine: true
-AllowAllParametersOfDeclarationOnNextLine: true
-AllowShortBlocksOnASingleLine: Never
-AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: All
-AllowShortLambdasOnASingleLine: All
-AllowShortIfStatementsOnASingleLine: Never
-AllowShortLoopsOnASingleLine: false
-AlwaysBreakAfterDefinitionReturnType: None
-AlwaysBreakAfterReturnType: None
-AlwaysBreakBeforeMultilineStrings: false
-AlwaysBreakTemplateDeclarations: MultiLine
-BinPackArguments: true
-BinPackParameters: true
-BraceWrapping:
- AfterCaseLabel: false
- AfterClass: false
- AfterControlStatement: false
- AfterEnum: false
- AfterFunction: false
- AfterNamespace: false
- AfterObjCDeclaration: false
- AfterStruct: false
- AfterUnion: false
- AfterExternBlock: false
- BeforeCatch: false
- BeforeElse: false
- IndentBraces: false
- SplitEmptyFunction: true
- SplitEmptyRecord: true
- SplitEmptyNamespace: true
-BreakBeforeBinaryOperators: None
-BreakBeforeBraces: Attach
-BreakBeforeInheritanceComma: false
-BreakInheritanceList: BeforeColon
-BreakBeforeTernaryOperators: true
-BreakConstructorInitializersBeforeComma: false
-BreakConstructorInitializers: BeforeColon
-BreakAfterJavaFieldAnnotations: false
-BreakStringLiterals: true
-ColumnLimit: 80
-CommentPragmas: '^ IWYU pragma:'
-CompactNamespaces: false
-ConstructorInitializerAllOnOneLineOrOnePerLine: false
-ConstructorInitializerIndentWidth: 4
-ContinuationIndentWidth: 4
-Cpp11BracedListStyle: true
-DeriveLineEnding: true
-DerivePointerAlignment: false
-DisableFormat: false
-ExperimentalAutoDetectBinPacking: false
-FixNamespaceComments: true
-ForEachMacros:
- - foreach
- - Q_FOREACH
- - BOOST_FOREACH
-IncludeBlocks: Preserve
-IncludeCategories:
- - Regex: '^"(llvm|llvm-c|clang|clang-c)/'
- Priority: 2
- SortPriority: 0
- - Regex: '^(<|"(gtest|gmock|isl|json)/)'
- Priority: 3
- SortPriority: 0
- - Regex: '.*'
- Priority: 1
- SortPriority: 0
-IncludeIsMainRegex: '(Test)?$'
-IncludeIsMainSourceRegex: ''
-IndentCaseLabels: false
-IndentGotoLabels: true
-IndentPPDirectives: None
-IndentWidth: 2
-IndentWrappedFunctionNames: false
-JavaScriptQuotes: Leave
-JavaScriptWrapImports: true
-KeepEmptyLinesAtTheStartOfBlocks: true
-MacroBlockBegin: ''
-MacroBlockEnd: ''
-MaxEmptyLinesToKeep: 1
-NamespaceIndentation: None
-ObjCBinPackProtocolList: Auto
-ObjCBlockIndentWidth: 2
-ObjCSpaceAfterProperty: false
-ObjCSpaceBeforeProtocolList: true
-PenaltyBreakAssignment: 2
-PenaltyBreakBeforeFirstCallParameter: 19
-PenaltyBreakComment: 300
-PenaltyBreakFirstLessLess: 120
-PenaltyBreakString: 1000
-PenaltyBreakTemplateDeclaration: 10
-PenaltyExcessCharacter: 1000000
-PenaltyReturnTypeOnItsOwnLine: 60
-PointerAlignment: Right
-ReflowComments: true
-SortIncludes: true
-SortUsingDeclarations: true
-SpaceAfterCStyleCast: false
-SpaceAfterLogicalNot: false
-SpaceAfterTemplateKeyword: true
-SpaceBeforeAssignmentOperators: true
-SpaceBeforeCpp11BracedList: false
-SpaceBeforeCtorInitializerColon: true
-SpaceBeforeInheritanceColon: true
-SpaceBeforeParens: ControlStatements
-SpaceBeforeRangeBasedForLoopColon: true
-SpaceInEmptyBlock: false
-SpaceInEmptyParentheses: false
-SpacesBeforeTrailingComments: 1
-SpacesInAngles: false
-SpacesInConditionalStatement: false
-SpacesInContainerLiterals: true
-SpacesInCStyleCastParentheses: false
-SpacesInParentheses: false
-SpacesInSquareBrackets: false
-SpaceBeforeSquareBrackets: false
-Standard: Latest
-StatementMacros:
- - Q_UNUSED
- - QT_REQUIRE_VERSION
-TabWidth: 8
-UseCRLF: false
-UseTab: Never
-...
-
diff --git a/mep3_navigation/src/distance_angle/distance_angle_regulator.cpp b/mep3_navigation/src/distance_angle/distance_angle_regulator.cpp
deleted file mode 100644
index d0335fe30..000000000
--- a/mep3_navigation/src/distance_angle/distance_angle_regulator.cpp
+++ /dev/null
@@ -1,989 +0,0 @@
-// Copyright 2021 Memristor Robotics
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-// http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include "mep3_navigation/distance_angle/distance_angle_regulator.hpp"
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include