From 35a91eaeab52f3050a33a6398b2fd34000ab2dec Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Thu, 2 Nov 2023 16:08:18 +0100 Subject: [PATCH] refreshed the examples for the new system --- .ci/build.sh | 12 - .ci/install.sh | 32 - .github/workflows/noetic.yml | 42 -- .github/workflows/ros_build_test.yml | 25 + .gitignore | 2 - .gitman.yml | 102 --- README.md | 31 +- controller_plugin/CMakeLists.txt | 82 +++ controller_plugin/README.md | 3 + controller_plugin/cfg/example_controller.cfg | 21 + .../config/example_controller.yaml | 4 + controller_plugin/include/pid.hpp | 100 +++ controller_plugin/package.xml | 29 + controller_plugin/plugins.xml | 5 + controller_plugin/src/example_controller.cpp | 378 ++++++++++ .../tmux/config/custom_config.yaml | 55 ++ controller_plugin/tmux/config/hw_api.yaml | 28 + .../tmux/config/network_config.yaml | 15 + controller_plugin/tmux/config/simulator.yaml | 18 + .../tmux/config/world_config.yaml | 34 + controller_plugin/tmux/layout.json | 50 ++ controller_plugin/tmux/session.yml | 54 ++ controller_plugin/tmux/start.sh | 27 + pluginlib_example/README.md | 27 + .../example_plugin_manager/CMakeLists.txt | 70 ++ .../config/example_plugin_manager.yaml | 12 + .../config/plugins.yaml | 12 + .../example_plugin_manager/common_handlers.h | 33 + .../example_plugin_manager/plugin_interface.h | 27 + .../launch/example_plugin_manager.launch | 22 + .../example_plugin_manager/nodelets.xml | 5 + .../example_plugin_manager/package.xml | 24 + .../src/example_plugin_manager.cpp | 272 ++++++++ .../example_plugins/CMakeLists.txt | 73 ++ .../config/example_plugin.yaml | 1 + pluginlib_example/example_plugins/package.xml | 25 + pluginlib_example/example_plugins/plugins.xml | 5 + .../example_plugins/src/example_plugin.cpp | 139 ++++ repurpose_package.sh | 112 --- ros_packages/example_ros_pluginlib | 1 - ros_packages/example_ros_uav | 1 - ros_packages/example_ros_uav_simple | 1 - ros_packages/example_ros_vision | 1 - tmux_scripts/waypoint_flier/.gitignore | 1 - tmux_scripts/waypoint_flier/layout.json | 190 ----- tmux_scripts/waypoint_flier/session.yml | 62 -- tmux_scripts/waypoint_flier/start.sh | 19 - tracker_plugin/CMakeLists.txt | 85 +++ tracker_plugin/README.md | 2 + tracker_plugin/cfg/example_tracker.cfg | 15 + tracker_plugin/config/example_tracker.yaml | 1 + .../include/.gitkeep | 0 tracker_plugin/package.xml | 28 + tracker_plugin/plugins.xml | 5 + tracker_plugin/src/example_tracker.cpp | 520 ++++++++++++++ tracker_plugin/tmux/config/custom_config.yaml | 29 + tracker_plugin/tmux/config/hw_api.yaml | 28 + .../tmux/config/network_config.yaml | 17 + tracker_plugin/tmux/config/simulator.yaml | 18 + tracker_plugin/tmux/config/world_config.yaml | 34 + tracker_plugin/tmux/layout.json | 50 ++ tracker_plugin/tmux/session.yml | 54 ++ tracker_plugin/tmux/start.sh | 27 + waypoint_flier/CMakeLists.txt | 77 +++ waypoint_flier/README.md | 75 ++ waypoint_flier/config/dynparam.cfg | 14 + waypoint_flier/config/waypoint_flier.yaml | 28 + waypoint_flier/launch/waypoint_flier.launch | 52 ++ waypoint_flier/nodelets.xml | 5 + waypoint_flier/package.xml | 30 + waypoint_flier/src/waypoint_flier.cpp | 649 ++++++++++++++++++ .../tmux/config/automatic_start.yaml | 5 + waypoint_flier/tmux/config/custom_config.yaml | 39 ++ waypoint_flier/tmux/config/hw_api.yaml | 28 + .../tmux/config/network_config.yaml | 20 + waypoint_flier/tmux/config/simulator.yaml | 20 + waypoint_flier/tmux/config/world_config.yaml | 34 + waypoint_flier/tmux/kill.sh | 14 + waypoint_flier/tmux/layout.json | 50 ++ waypoint_flier/tmux/session.yml | 56 ++ waypoint_flier/tmux/start.sh | 27 + waypoint_flier_simple/CMakeLists.txt | 66 ++ waypoint_flier_simple/README.md | 23 + .../config/waypoint_flier.yaml | 3 + .../launch/waypoint_flier_simple.launch | 41 ++ waypoint_flier_simple/nodelets.xml | 5 + waypoint_flier_simple/package.xml | 29 + .../src/waypoint_flier_simple.cpp | 238 +++++++ .../tmux/config/automatic_start.yaml | 5 + .../tmux/config/custom_config.yaml | 39 ++ waypoint_flier_simple/tmux/config/hw_api.yaml | 28 + .../tmux/config/network_config.yaml | 20 + .../tmux/config/simulator.yaml | 20 + .../tmux/config/world_config.yaml | 34 + waypoint_flier_simple/tmux/kill.sh | 14 + waypoint_flier_simple/tmux/layout.json | 50 ++ waypoint_flier_simple/tmux/session.yml | 54 ++ waypoint_flier_simple/tmux/start.sh | 27 + 98 files changed, 4527 insertions(+), 589 deletions(-) delete mode 100755 .ci/build.sh delete mode 100755 .ci/install.sh delete mode 100644 .github/workflows/noetic.yml create mode 100644 .github/workflows/ros_build_test.yml delete mode 100644 .gitman.yml create mode 100644 controller_plugin/CMakeLists.txt create mode 100644 controller_plugin/README.md create mode 100755 controller_plugin/cfg/example_controller.cfg create mode 100644 controller_plugin/config/example_controller.yaml create mode 100644 controller_plugin/include/pid.hpp create mode 100644 controller_plugin/package.xml create mode 100644 controller_plugin/plugins.xml create mode 100644 controller_plugin/src/example_controller.cpp create mode 100644 controller_plugin/tmux/config/custom_config.yaml create mode 100644 controller_plugin/tmux/config/hw_api.yaml create mode 100644 controller_plugin/tmux/config/network_config.yaml create mode 100644 controller_plugin/tmux/config/simulator.yaml create mode 100644 controller_plugin/tmux/config/world_config.yaml create mode 100644 controller_plugin/tmux/layout.json create mode 100644 controller_plugin/tmux/session.yml create mode 100755 controller_plugin/tmux/start.sh create mode 100644 pluginlib_example/README.md create mode 100644 pluginlib_example/example_plugin_manager/CMakeLists.txt create mode 100644 pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml create mode 100644 pluginlib_example/example_plugin_manager/config/plugins.yaml create mode 100644 pluginlib_example/example_plugin_manager/include/example_plugin_manager/common_handlers.h create mode 100644 pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h create mode 100644 pluginlib_example/example_plugin_manager/launch/example_plugin_manager.launch create mode 100644 pluginlib_example/example_plugin_manager/nodelets.xml create mode 100644 pluginlib_example/example_plugin_manager/package.xml create mode 100644 pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp create mode 100644 pluginlib_example/example_plugins/CMakeLists.txt create mode 100644 pluginlib_example/example_plugins/config/example_plugin.yaml create mode 100644 pluginlib_example/example_plugins/package.xml create mode 100644 pluginlib_example/example_plugins/plugins.xml create mode 100644 pluginlib_example/example_plugins/src/example_plugin.cpp delete mode 100755 repurpose_package.sh delete mode 120000 ros_packages/example_ros_pluginlib delete mode 120000 ros_packages/example_ros_uav delete mode 120000 ros_packages/example_ros_uav_simple delete mode 120000 ros_packages/example_ros_vision delete mode 100644 tmux_scripts/waypoint_flier/.gitignore delete mode 100644 tmux_scripts/waypoint_flier/layout.json delete mode 100644 tmux_scripts/waypoint_flier/session.yml delete mode 100755 tmux_scripts/waypoint_flier/start.sh create mode 100644 tracker_plugin/CMakeLists.txt create mode 100644 tracker_plugin/README.md create mode 100755 tracker_plugin/cfg/example_tracker.cfg create mode 100644 tracker_plugin/config/example_tracker.yaml rename .gitmodules => tracker_plugin/include/.gitkeep (100%) create mode 100644 tracker_plugin/package.xml create mode 100644 tracker_plugin/plugins.xml create mode 100644 tracker_plugin/src/example_tracker.cpp create mode 100644 tracker_plugin/tmux/config/custom_config.yaml create mode 100644 tracker_plugin/tmux/config/hw_api.yaml create mode 100644 tracker_plugin/tmux/config/network_config.yaml create mode 100644 tracker_plugin/tmux/config/simulator.yaml create mode 100644 tracker_plugin/tmux/config/world_config.yaml create mode 100644 tracker_plugin/tmux/layout.json create mode 100644 tracker_plugin/tmux/session.yml create mode 100755 tracker_plugin/tmux/start.sh create mode 100644 waypoint_flier/CMakeLists.txt create mode 100644 waypoint_flier/README.md create mode 100755 waypoint_flier/config/dynparam.cfg create mode 100644 waypoint_flier/config/waypoint_flier.yaml create mode 100644 waypoint_flier/launch/waypoint_flier.launch create mode 100644 waypoint_flier/nodelets.xml create mode 100644 waypoint_flier/package.xml create mode 100644 waypoint_flier/src/waypoint_flier.cpp create mode 100644 waypoint_flier/tmux/config/automatic_start.yaml create mode 100644 waypoint_flier/tmux/config/custom_config.yaml create mode 100644 waypoint_flier/tmux/config/hw_api.yaml create mode 100644 waypoint_flier/tmux/config/network_config.yaml create mode 100644 waypoint_flier/tmux/config/simulator.yaml create mode 100644 waypoint_flier/tmux/config/world_config.yaml create mode 100755 waypoint_flier/tmux/kill.sh create mode 100644 waypoint_flier/tmux/layout.json create mode 100644 waypoint_flier/tmux/session.yml create mode 100755 waypoint_flier/tmux/start.sh create mode 100644 waypoint_flier_simple/CMakeLists.txt create mode 100644 waypoint_flier_simple/README.md create mode 100644 waypoint_flier_simple/config/waypoint_flier.yaml create mode 100644 waypoint_flier_simple/launch/waypoint_flier_simple.launch create mode 100644 waypoint_flier_simple/nodelets.xml create mode 100644 waypoint_flier_simple/package.xml create mode 100644 waypoint_flier_simple/src/waypoint_flier_simple.cpp create mode 100644 waypoint_flier_simple/tmux/config/automatic_start.yaml create mode 100644 waypoint_flier_simple/tmux/config/custom_config.yaml create mode 100644 waypoint_flier_simple/tmux/config/hw_api.yaml create mode 100644 waypoint_flier_simple/tmux/config/network_config.yaml create mode 100644 waypoint_flier_simple/tmux/config/simulator.yaml create mode 100644 waypoint_flier_simple/tmux/config/world_config.yaml create mode 100755 waypoint_flier_simple/tmux/kill.sh create mode 100644 waypoint_flier_simple/tmux/layout.json create mode 100644 waypoint_flier_simple/tmux/session.yml create mode 100755 waypoint_flier_simple/tmux/start.sh diff --git a/.ci/build.sh b/.ci/build.sh deleted file mode 100755 index 17d8f8d..0000000 --- a/.ci/build.sh +++ /dev/null @@ -1,12 +0,0 @@ -#!/bin/bash -set -e - -distro=`lsb_release -r | awk '{ print $2 }'` -[ "$distro" = "18.04" ] && ROS_DISTRO="melodic" -[ "$distro" = "20.04" ] && ROS_DISTRO="noetic" - -echo "Starting build" -cd ~/catkin_ws -source /opt/ros/$ROS_DISTRO/setup.bash -catkin build --limit-status-rate 0.2 --summarize -echo "Ended build" diff --git a/.ci/install.sh b/.ci/install.sh deleted file mode 100755 index ce28cd9..0000000 --- a/.ci/install.sh +++ /dev/null @@ -1,32 +0,0 @@ -#!/bin/bash -set -e - -distro=`lsb_release -r | awk '{ print $2 }'` -[ "$distro" = "18.04" ] && ROS_DISTRO="melodic" -[ "$distro" = "20.04" ] && ROS_DISTRO="noetic" - -echo "Starting install" - -MY_PATH=`pwd` - -sudo apt-get -y install git - -echo "clone uav_core" -cd -git clone https://github.com/ctu-mrs/uav_core.git -cd uav_core - -echo "running the main install.sh" -./installation/install.sh - -cd $MY_PATH -gitman install - -mkdir -p ~/catkin_ws/src -cd ~/catkin_ws/src -ln -s ~/uav_core -ln -s "$MY_PATH" example_packages -source /opt/ros/$ROS_DISTRO/setup.bash -cd ~/catkin_ws - -echo "install ended" diff --git a/.github/workflows/noetic.yml b/.github/workflows/noetic.yml deleted file mode 100644 index 5322066..0000000 --- a/.github/workflows/noetic.yml +++ /dev/null @@ -1,42 +0,0 @@ -name: Noetic - -on: - push: - branches: [ master ] - paths-ignore: - - '**/README.md' - - '**.yaml' - - '**.launch' - pull_request: - branches: [ master ] - - # Allows you to run this workflow manually from the Actions tab - workflow_dispatch: - -jobs: - - cancel: - - name: Cancel Previous Runs - runs-on: ubuntu-latest - steps: - - name: Cancel Previous Runs - uses: styfle/cancel-workflow-action@0.8.0 - with: - access_token: ${{ github.token }} - - build: - runs-on: ubuntu-20.04 - - steps: - - # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it - - uses: actions/checkout@v2 - with: - fetch-depth: 0 # fetch the whole history - - - name: Install - run: ./.ci/install.sh - - - name: Build - run: ./.ci/build.sh diff --git a/.github/workflows/ros_build_test.yml b/.github/workflows/ros_build_test.yml new file mode 100644 index 0000000..883b589 --- /dev/null +++ b/.github/workflows/ros_build_test.yml @@ -0,0 +1,25 @@ +name: ros_build_test + +on: + + push: + branches: [ master ] + + paths-ignore: + - '**/README.md' + + pull_request: + branches: [ master ] + + workflow_dispatch: + +concurrency: + group: ${{ github.ref }} + cancel-in-progress: true + +jobs: + + build: + uses: ctu-mrs/ci_scripts/.github/workflows/ros_build_test.yml@master + secrets: + PUSH_TOKEN: ${{ secrets.PUSH_TOKEN }} diff --git a/.gitignore b/.gitignore index 118e2a7..3819313 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,2 @@ -.gitman - *.swp *.swo diff --git a/.gitman.yml b/.gitman.yml deleted file mode 100644 index d1e05fd..0000000 --- a/.gitman.yml +++ /dev/null @@ -1,102 +0,0 @@ -location: .gitman -sources: - - repo: https://github.com/ctu-mrs/example_ros_pluginlib.git - name: example_ros_pluginlib - rev: master - type: git - params: - sparse_paths: - - - links: - - source: '' - target: ros_packages/example_ros_pluginlib - scripts: - - git submodule update --init --recursive - - repo: https://github.com/ctu-mrs/example_ros_uav.git - name: example_ros_uav - rev: master - type: git - params: - sparse_paths: - - - links: - - source: '' - target: ros_packages/example_ros_uav - scripts: - - git submodule update --init --recursive - - repo: https://github.com/ctu-mrs/example_ros_vision.git - name: example_ros_vision - rev: master - type: git - params: - sparse_paths: - - - links: - - source: '' - target: ros_packages/example_ros_vision - scripts: - - git submodule update --init --recursive - - repo: https://github.com/ctu-mrs/example_ros_uav_simple.git - name: example_ros_uav_simple - rev: master - type: git - params: - sparse_paths: - - - links: - - source: '' - target: ros_packages/example_ros_uav_simple - scripts: - - git submodule update --init --recursive -sources_locked: - - repo: https://github.com/ctu-mrs/example_ros_pluginlib.git - name: example_ros_pluginlib - rev: b566f9b9cef82f1f69508fd45b59e63c8a27b817 - type: git - params: - sparse_paths: - - - links: - - source: '' - target: ros_packages/example_ros_pluginlib - scripts: - - git submodule update --init --recursive - - repo: https://github.com/ctu-mrs/example_ros_uav.git - name: example_ros_uav - rev: b32c86338faea2cb9b4bfea143ce3bcb5e1fd9b8 - type: git - params: - sparse_paths: - - - links: - - source: '' - target: ros_packages/example_ros_uav - scripts: - - git submodule update --init --recursive - - repo: https://github.com/ctu-mrs/example_ros_vision.git - name: example_ros_vision - rev: 1f5614dcace6d535fc36881ba136a7afb68f2d7a - type: git - params: - sparse_paths: - - - links: - - source: '' - target: ros_packages/example_ros_vision - scripts: - - git submodule update --init --recursive - - repo: https://github.com/ctu-mrs/example_ros_uav_simple.git - name: example_ros_uav_simple - rev: c250e958e9bfc0b8f14056683e183491683efe64 - type: git - params: - sparse_paths: - - - links: - - source: '' - target: ros_packages/example_ros_uav_simple - scripts: - - git submodule update --init --recursive -default_group: '' -groups: - - diff --git a/README.md b/README.md index 6e6b73c..7bdb84e 100644 --- a/README.md +++ b/README.md @@ -1,15 +1,24 @@ -# Example packages +# MRS Core Examples -| Build status | [![Build Status](https://github.com/ctu-mrs/example_ros_packages/workflows/Melodic/badge.svg)](https://github.com/ctu-mrs/example_ros_packages/actions) | [![Build Status](https://github.com/ctu-mrs/example_ros_packages/workflows/Noetic/badge.svg)](https://github.com/ctu-mrs/example_ros_packages/actions) | -|--------------|---------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------| - -This repository includes other ROS packages, demonstrating some basic functionality to get you started. -You can test these in simulation (see our [simulation tutorial](https://ctu-mrs.github.io/docs/simulation/howto.html)). -Follow our **[How to manager ROS workspaces](https://ctu-mrs.github.io/docs/system/managing_ros_workspaces.html)** guide to learn **how to set up** a custom workspace. +This repository includes Core ROS examples for the [MRS UAV System](https://github.com/ctu-mrs/mrs_uav_system). ## Packages -* [example_ros_uav_simple](https://github.com/ctu-mrs/example_ros_uav_simple) - Example of a simple nodelet -* [example_ros_uav](https://github.com/ctu-mrs/example_ros_uav) - Example of UAV control -* [example_ros_vision](https://github.com/ctu-mrs/example_ros_vision) - Example of UAV computer vision -* [example_ros_pluginlib](https://github.com/ctu-mrs/example_ros_pluginlib) - Example of ROS Pluginlib, similar to how it is used in the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers) +* [waypoint_flier_simple](./waypoint_flier_simple) - Minimalistic C++ Nodelet with "_vanilla_" ROS features +* [waypoint_flier](./waypoint_flier) - Full C++ Nodelet with "_MRS_" libraries and wrappers +* [controller_plugin](./controller_plugin) - Example of Controller plugin for the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers) +* [tracker_plugin](./tracker_plugin) - Example of Tracker plugin for the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers) +* [pluginlib_example](./pluginlib_example) - Example of ROS Pluginlib, similar to how it is used in the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers) + +# Disclaimer + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/controller_plugin/CMakeLists.txt b/controller_plugin/CMakeLists.txt new file mode 100644 index 0000000..ef3285e --- /dev/null +++ b/controller_plugin/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.1.2) +project(example_controller_plugin) + +set(CATKIN_DEPENDENCIES + cmake_modules + roscpp + rospy + pluginlib + geometry_msgs + dynamic_reconfigure + mrs_uav_managers + mrs_msgs + mrs_lib + ) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_DEPENDENCIES} + ) + +generate_dynamic_reconfigure_options( + cfg/example_controller.cfg + ) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") + +find_package(Eigen3 REQUIRED) +set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) +set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) + +set(LIBRARIES + ExampleController + ) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} + LIBRARIES ${LIBRARIES} + DEPENDS Eigen + ) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake + ) + +# Example Controller + +add_library(ExampleController + src/example_controller.cpp + ) + +add_dependencies(ExampleController + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ${PROJECT_NAME}_gencfg + ) + +target_link_libraries(ExampleController + ${catkin_LIBRARIES} + ) + +## -------------------------------------------------------------- +## | Install | +## -------------------------------------------------------------- + +install(TARGETS ${LIBRARIES} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +install(FILES plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) diff --git a/controller_plugin/README.md b/controller_plugin/README.md new file mode 100644 index 0000000..08be633 --- /dev/null +++ b/controller_plugin/README.md @@ -0,0 +1,3 @@ +# example_controller_plugin + +Example controller plugin for the MRS Control Manager. diff --git a/controller_plugin/cfg/example_controller.cfg b/controller_plugin/cfg/example_controller.cfg new file mode 100755 index 0000000..5dff729 --- /dev/null +++ b/controller_plugin/cfg/example_controller.cfg @@ -0,0 +1,21 @@ +#!/usr/bin/env python +PACKAGE = "example_controller_plugin" + +import roslib; +roslib.load_manifest(PACKAGE) + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +orientation = gen.add_group("Desired orientation"); + +orientation.add("roll", double_t, 0, "Desired roll", 0.0, -3.14, 3.14) +orientation.add("pitch", double_t, 0, "Desired pitch", 0.0, -3.14, 3.14) +orientation.add("yaw", double_t, 0, "Desired pitch", 0.0, -3.14, 3.14) + +force = gen.add_group("Desired force relative to hover"); + +force.add("force", double_t, 0, "Desired force", 0.0, -10.0, 10.0) + +exit(gen.generate(PACKAGE, "ExampleController", "example_controller")) diff --git a/controller_plugin/config/example_controller.yaml b/controller_plugin/config/example_controller.yaml new file mode 100644 index 0000000..dde46ff --- /dev/null +++ b/controller_plugin/config/example_controller.yaml @@ -0,0 +1,4 @@ +desired_roll: 0.03 # [rad] +desired_pitch: 0.02 # [rad] +desired_yaw: 0.1 # [rad] +desired_thrust_force: 0.2 # [N], additional to gravity compensation diff --git a/controller_plugin/include/pid.hpp b/controller_plugin/include/pid.hpp new file mode 100644 index 0000000..1bd7ed9 --- /dev/null +++ b/controller_plugin/include/pid.hpp @@ -0,0 +1,100 @@ +#ifndef PID_H +#define PID_H + +#include + +namespace example_controller_plugin +{ + +class PIDController { + +private: + // | ----------------------- parameters ----------------------- | + + // gains + double _kp_ = 0; // proportional gain + double _kd_ = 0; // derivative gain + double _ki_ = 0; // integral gain + + // we remember the last control error, to calculate the difference + double last_error_ = 0; + double integral_ = 0; + + double saturation = -1; + double antiwindup = -1; + +public: + PIDController(); + + void setParams(const double &kp, const double &kd, const double &ki, const double &saturation, const double &antiwindup); + + void setSaturation(const double saturation = -1); + + void reset(void); + + double update(const double &error, const double &dt); +}; + +// -------------------------------------------------------------- +// | implementation | +// -------------------------------------------------------------- + +PIDController::PIDController() { + + this->reset(); +} + +void PIDController::setParams(const double &kp, const double &kd, const double &ki, const double &saturation, const double &antiwindup) { + + this->_kp_ = kp; + this->_kd_ = kd; + this->_ki_ = ki; + this->saturation = saturation; + this->antiwindup = antiwindup; +} + +void PIDController::setSaturation(const double saturation) { + + this->saturation = saturation; +} + +void PIDController::reset(void) { + + this->last_error_ = 0; + this->integral_ = 0; +} + +double PIDController::update(const double &error, const double &dt) { + + // calculate the control error difference + double difference = (error - last_error_) / dt; + last_error_ = error; + + double p_component = _kp_ * error; // proportional feedback + double d_component = _kd_ * difference; // derivative feedback + double i_component = _ki_ * integral_; // derivative feedback + + double sum = p_component + d_component + i_component; + + if (saturation > 0) { + if (sum >= saturation) { + sum = saturation; + } else if (sum <= -saturation) { + sum = -saturation; + } + } + + if (antiwindup > 0) { + if (std::abs(sum) < antiwindup) { + // add to the integral + integral_ += error * dt; + } + } + + // return the summ of the components + return sum; +} + +} // namespace example_controller_plugin + +#endif // PID_H diff --git a/controller_plugin/package.xml b/controller_plugin/package.xml new file mode 100644 index 0000000..9c841f6 --- /dev/null +++ b/controller_plugin/package.xml @@ -0,0 +1,29 @@ + + + + example_controller_plugin + 1.0.0 + The example_controller_plugin package + + Tomas Baca + Tomas Baca + + BSD 3-Clause + + catkin + + cmake_modules + roscpp + rospy + pluginlib + geometry_msgs + dynamic_reconfigure + mrs_uav_managers + mrs_msgs + mrs_lib + + + + + + diff --git a/controller_plugin/plugins.xml b/controller_plugin/plugins.xml new file mode 100644 index 0000000..a8d713b --- /dev/null +++ b/controller_plugin/plugins.xml @@ -0,0 +1,5 @@ + + + This is the Example Controller. + + diff --git a/controller_plugin/src/example_controller.cpp b/controller_plugin/src/example_controller.cpp new file mode 100644 index 0000000..58a3066 --- /dev/null +++ b/controller_plugin/src/example_controller.cpp @@ -0,0 +1,378 @@ +/* includes //{ */ + +#include +#include + +#include + +#include + +#include +#include + +#include +#include +#include + +//} + +namespace example_controller_plugin +{ + +namespace example_controller +{ + +/* //{ class ExampleController */ + +class ExampleController : public mrs_uav_managers::Controller { + +public: + bool initialize(const ros::NodeHandle& nh, std::shared_ptr common_handlers, + std::shared_ptr private_handlers); + + bool activate(const ControlOutput& last_control_output); + + void deactivate(void); + + void updateInactive(const mrs_msgs::UavState& uav_state, const std::optional& tracker_command); + + ControlOutput updateActive(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command); + + const mrs_msgs::ControllerStatus getStatus(); + + void switchOdometrySource(const mrs_msgs::UavState& new_uav_state); + + void resetDisturbanceEstimators(void); + + const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd); + +private: + ros::NodeHandle nh_; + + bool is_initialized_ = false; + bool is_active_ = false; + + std::shared_ptr common_handlers_; + std::shared_ptr private_handlers_; + + // | ------------------------ uav state ----------------------- | + + mrs_msgs::UavState uav_state_; + std::mutex mutex_uav_state_; + + // | --------------- dynamic reconfigure server --------------- | + + boost::recursive_mutex mutex_drs_; + typedef example_controller_plugin::example_controllerConfig DrsConfig_t; + typedef dynamic_reconfigure::Server Drs_t; + boost::shared_ptr drs_; + void callbackDrs(example_controller_plugin::example_controllerConfig& config, uint32_t level); + DrsConfig_t drs_params_; + std::mutex mutex_drs_params_; + + // | ----------------------- constraints ---------------------- | + + mrs_msgs::DynamicsConstraints constraints_; + std::mutex mutex_constraints_; + + // | --------- throttle generation and mass estimation -------- | + + double _uav_mass_; + + // | ------------------ activation and output ----------------- | + + ControlOutput last_control_output_; + ControlOutput activation_control_output_; + + ros::Time last_update_time_; + std::atomic first_iteration_ = true; +}; + +//} + +// -------------------------------------------------------------- +// | controller's interface | +// -------------------------------------------------------------- + +/* //{ initialize() */ + +bool ExampleController::initialize(const ros::NodeHandle& nh, std::shared_ptr common_handlers, + std::shared_ptr private_handlers) { + + nh_ = nh; + + common_handlers_ = common_handlers; + private_handlers_ = private_handlers; + + _uav_mass_ = common_handlers->getMass(); + + last_update_time_ = ros::Time(0); + + ros::Time::waitForValid(); + + // | ------------------- loading parameters ------------------- | + + bool success = true; + + // FYI + // This method will load the file using `rosparam get` + // Pros: you can the full power of the official param loading + // Cons: it is slower + // + // Alternatives: + // You can load the file directly into the ParamLoader as shown below. + + success *= private_handlers->loadConfigFile(ros::package::getPath("example_controller_plugin") + "/config/example_controller.yaml"); + + if (!success) { + return false; + } + + mrs_lib::ParamLoader param_loader(nh_, "ExampleController"); + + // This is the alternaive way of loading the config file. + // + // Files loaded using this method are prioritized over ROS params. + // + // param_loader.addYamlFile(ros::package::getPath("example_tracker_plugin") + "/config/example_tracker.yaml"); + + param_loader.loadParam("desired_roll", drs_params_.roll); + param_loader.loadParam("desired_pitch", drs_params_.pitch); + param_loader.loadParam("desired_yaw", drs_params_.yaw); + param_loader.loadParam("desired_thrust_force", drs_params_.force); + + // | ------------------ finish loading params ----------------- | + + if (!param_loader.loadedSuccessfully()) { + ROS_ERROR("[ExampleController]: could not load all parameters!"); + return false; + } + + // | --------------- dynamic reconfigure server --------------- | + + drs_.reset(new Drs_t(mutex_drs_, nh_)); + drs_->updateConfig(drs_params_); + Drs_t::CallbackType f = boost::bind(&ExampleController::callbackDrs, this, _1, _2); + drs_->setCallback(f); + + // | ----------------------- finish init ---------------------- | + + ROS_INFO("[ExampleController]: initialized"); + + is_initialized_ = true; + + return true; +} + +//} + +/* //{ activate() */ + +bool ExampleController::activate(const ControlOutput& last_control_output) { + + activation_control_output_ = last_control_output; + + first_iteration_ = true; + + is_active_ = true; + + ROS_INFO("[ExampleController]: activated"); + + return true; +} + +//} + +/* //{ deactivate() */ + +void ExampleController::deactivate(void) { + + is_active_ = false; + first_iteration_ = false; + + ROS_INFO("[ExampleController]: deactivated"); +} + +//} + +/* updateInactive() //{ */ + +void ExampleController::updateInactive(const mrs_msgs::UavState& uav_state, [[maybe_unused]] const std::optional& tracker_command) { + + mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_); + + last_update_time_ = uav_state.header.stamp; + + first_iteration_ = false; +} + +//} + +/* //{ updateActive() */ + +ExampleController::ControlOutput ExampleController::updateActive(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) { + + auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_); + + mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_); + + // clear all the optional parts of the result + last_control_output_.desired_heading_rate = {}; + last_control_output_.desired_orientation = {}; + last_control_output_.desired_unbiased_acceleration = {}; + last_control_output_.control_output = {}; + + if (!is_active_) { + return last_control_output_; + } + + // | ---------- calculate dt from the last iteration ---------- | + + double dt; + + if (first_iteration_) { + dt = 0.01; + first_iteration_ = false; + } else { + dt = (uav_state.header.stamp - last_update_time_).toSec(); + } + + last_update_time_ = uav_state.header.stamp; + + if (fabs(dt) < 0.001) { + + ROS_DEBUG("[ExampleController]: the last odometry message came too close (%.2f s)!", dt); + dt = 0.01; + } + + // | -------- check for the available output modalities ------- | + + // you can decide what to return, but it needs to be available + if (common_handlers_->control_output_modalities.attitude) { + ROS_INFO_THROTTLE(1.0, "[ExampleController]: desired attitude output modality is available"); + } + + // | ---------- extract the detailed model parameters --------- | + + if (common_handlers_->detailed_model_params) { + + mrs_uav_managers::control_manager::DetailedModelParams_t detailed_model_params = common_handlers_->detailed_model_params.value(); + + ROS_INFO_STREAM_THROTTLE(1.0, "[ExampleController]: UAV inertia is: " << detailed_model_params.inertia); + } + + // | -------------- prepare the control reference ------------- | + + geometry_msgs::PoseStamped position_reference; + + position_reference.header = tracker_command.header; + position_reference.pose.position = tracker_command.position; + position_reference.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0).setHeading(tracker_command.heading); + + + // | ---------------- prepare the control output --------------- | + + mrs_msgs::HwApiAttitudeCmd attitude_cmd; + + attitude_cmd.orientation = mrs_lib::AttitudeConverter(drs_params.roll, drs_params.pitch, drs_params.yaw); + attitude_cmd.throttle = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, + common_handlers_->getMass() * common_handlers_->g + drs_params.force); + + + // | ----------------- set the control output ----------------- | + + last_control_output_.control_output = attitude_cmd; + + // | --------------- fill in the optional parts --------------- | + + //// it is recommended to fill the optinal parts if you know them + + /// this is used for: + // * plotting the orientation in the control_refence topic (optional) + // * checking for attitude control error + last_control_output_.desired_orientation = mrs_lib::AttitudeConverter(drs_params.roll, drs_params.pitch, drs_params.yaw); + + /// IMPORANT + // The acceleration and heading rate in 3D (expressed in the "fcu" frame of reference) that the UAV will actually undergo due to the control action. + last_control_output_.desired_unbiased_acceleration = Eigen::Vector3d(0, 0, 0); + last_control_output_.desired_heading_rate = 0; + + // | ----------------- fill in the diagnostics ---------------- | + + last_control_output_.diagnostics.controller = "ExampleController"; + + return last_control_output_; +} + +//} + +/* //{ getStatus() */ + +const mrs_msgs::ControllerStatus ExampleController::getStatus() { + + mrs_msgs::ControllerStatus controller_status; + + controller_status.active = is_active_; + + return controller_status; +} + +//} + +/* switchOdometrySource() //{ */ + +void ExampleController::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState& new_uav_state) { +} + +//} + +/* resetDisturbanceEstimators() //{ */ + +void ExampleController::resetDisturbanceEstimators(void) { +} + +//} + +/* setConstraints() //{ */ + +const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr ExampleController::setConstraints([ + [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) { + + if (!is_initialized_) { + return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse()); + } + + mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_); + + ROS_INFO("[ExampleController]: updating constraints"); + + mrs_msgs::DynamicsConstraintsSrvResponse res; + res.success = true; + res.message = "constraints updated"; + + return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res)); +} + +//} + +// -------------------------------------------------------------- +// | callbacks | +// -------------------------------------------------------------- + +/* //{ callbackDrs() */ + +void ExampleController::callbackDrs(example_controller_plugin::example_controllerConfig& config, [[maybe_unused]] uint32_t level) { + + mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_); + + ROS_INFO("[ExampleController]: dynamic reconfigure params updated"); +} + +//} + +} // namespace example_controller + +} // namespace example_controller_plugin + +#include +PLUGINLIB_EXPORT_CLASS(example_controller_plugin::example_controller::ExampleController, mrs_uav_managers::Controller) diff --git a/controller_plugin/tmux/config/custom_config.yaml b/controller_plugin/tmux/config/custom_config.yaml new file mode 100644 index 0000000..89ada76 --- /dev/null +++ b/controller_plugin/tmux/config/custom_config.yaml @@ -0,0 +1,55 @@ +mrs_uav_managers: + + estimation_manager: + + # loaded state estimator plugins + state_estimators: [ + "gps_baro", + "ground_truth", + ] + + initial_state_estimator: "gps_baro" # will be used as the first state estimator + agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) + + control_manager: + + ExampleController: + address: "example_controller_plugin/ExampleController" + namespace: "example_controller" + eland_threshold: 20.0 # [m], position error triggering eland + failsafe_threshold: 30.0 # [m], position error triggering failsafe land + odometry_innovation_threshold: 1.5 # [m], position odometry innovation threshold + human_switchable: true + + # which outputs the controller can provide + outputs: + actuators: false + control_group: false + attitude_rate: false + attitude: true + acceleration_hdg_rate: false + acceleration_hdg: false + velocity_hdg_rate: false + velocity_hdg: false + position: false + + # list of names of dynamically loaded controllers + controllers : [ + "ExampleController", + ] + + uav_manager: + + takeoff: + after_takeoff: + controller: "ExampleController" + + midair_activation: + after_activation: + controller: "ExampleController" + + min_height_checking: + enabled: false + + max_height_checking: + enabled: false diff --git a/controller_plugin/tmux/config/hw_api.yaml b/controller_plugin/tmux/config/hw_api.yaml new file mode 100644 index 0000000..d98d5e5 --- /dev/null +++ b/controller_plugin/tmux/config/hw_api.yaml @@ -0,0 +1,28 @@ +input_mode: + actuators: false + control_group: false + attitude_rate: true + attitude: true + acceleration_hdg_rate: false + acceleration_hdg: false + velocity_hdg_rate: false + velocity_hdg: false + position: false + + feedforward: true + +outputs: + distance_sensor: true + gnss: true + rtk: false + imu: false + altitude: true + magnetometer_heading: true + rc_channels: false + battery_state: false + position: true + orientation: true + velocity: true + angular_velocity: true + odometry: false + ground_truth: true diff --git a/controller_plugin/tmux/config/network_config.yaml b/controller_plugin/tmux/config/network_config.yaml new file mode 100644 index 0000000..5b3ad0a --- /dev/null +++ b/controller_plugin/tmux/config/network_config.yaml @@ -0,0 +1,15 @@ +# 1. This list is used by NimbroNetwork for mutual communication of the UAVs +# The names of the robots have to match hostnames described in /etc/hosts. +# +# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. +# The names should match the true "UAV_NAMES" (the topic prefixes). +# +# network_config:=~/config/network_config.yaml +# +# to the core.launch and nimbro.launch. + +network: + + robot_names: [ + uav1 + ] diff --git a/controller_plugin/tmux/config/simulator.yaml b/controller_plugin/tmux/config/simulator.yaml new file mode 100644 index 0000000..b8d614b --- /dev/null +++ b/controller_plugin/tmux/config/simulator.yaml @@ -0,0 +1,18 @@ +simulation_rate: 250 +clock_rate: 250 +realtime_factor: 1.0 + +uav_names: [ + "uav1", +] + +individual_takeoff_platform: + enabled: false + +uav1: + type: "x500" + spawn: + x: 10.0 + y: 15.0 + z: 2.0 + heading: 0 diff --git a/controller_plugin/tmux/config/world_config.yaml b/controller_plugin/tmux/config/world_config.yaml new file mode 100644 index 0000000..9b55067 --- /dev/null +++ b/controller_plugin/tmux/config/world_config.yaml @@ -0,0 +1,34 @@ +world_origin: + + units: "LATLON" # {"UTM, "LATLON"} + + origin_x: 47.397743 + origin_y: 8.545594 + +safety_area: + + enabled: true + + horizontal: + + # the frame of reference in which the points are expressed + frame_name: "world_origin" + + # polygon + # + # x, y [m] for any frame_name except latlon_origin + # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" + points: [ + -50, -50, + 50, -50, + 50, 50, + -50, 50, + ] + + vertical: + + # the frame of reference in which the max&min z is expressed + frame_name: "world_origin" + + max_z: 15.0 + min_z: 0.5 diff --git a/controller_plugin/tmux/layout.json b/controller_plugin/tmux/layout.json new file mode 100644 index 0000000..5664ebd --- /dev/null +++ b/controller_plugin/tmux/layout.json @@ -0,0 +1,50 @@ +[ +{ + "border": "normal", + "floating": "auto_off", + "fullscreen_mode": 0, + "layout": "splitv", + "percent": 0.285714285714286, + "type": "con", + "nodes": [ + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 1030, + "width": 936, + "x": 4806, + "y": 32 + }, + "name": "rviz.rviz* - RViz", + "percent": 0.5, + "swallows": [ + { + "instance": "^rviz$" + } + ], + "type": "con" + }, + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 460, + "width": 724, + "x": 0, + "y": 0 + }, + "name": "./start.sh", + "percent": 0.5, + "swallows": [ + { + "instance": "^urxvt$" + } + ], + "type": "con" + } + ] +} +] diff --git a/controller_plugin/tmux/session.yml b/controller_plugin/tmux/session.yml new file mode 100644 index 0000000..5a12297 --- /dev/null +++ b/controller_plugin/tmux/session.yml @@ -0,0 +1,54 @@ +# do not modify these +root: ./ +name: simulation +socket_name: mrs +attach: false +tmux_options: -f /etc/ctu-mrs/tmux.conf +# you can modify these +pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500 +startup_window: control +windows: + - roscore: + layout: tiled + panes: + - roscore + - simulator: + layout: tiled + panes: + - waitForRos; roslaunch mrs_multirotor_simulator multirotor_simulator.launch custom_config:=./config/simulator.yaml + - hw_api: + layout: tiled + panes: + - waitForTime; roslaunch mrs_multirotor_simulator hw_api.launch custom_config:=./config/hw_api.yaml + - takeoff: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_autostart automatic_start.launch + - waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard + # - midair_activation: + # # disabled the "takeoff" section and turn on the "individual_takeoff_platform" in the simulator config + # layout: tiled + # panes: + # - waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; rosservice call /$UAV_NAME/uav_manager/midair_activation + - status: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_status status.launch + - control: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_core core.launch + platform_config:=`rospack find mrs_multirotor_simulator`/config/mrs_uav_system/$UAV_TYPE.yaml + custom_config:=./config/custom_config.yaml + world_config:=./config/world_config.yaml + network_config:=./config/network_config.yaml + - rviz: + layout: tiled + panes: + - waitForControl; roslaunch mrs_uav_core rviz.launch + - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch + - waitForControl; waitForControl; roslaunch mrs_rviz_plugins rviz_interface.launch + - layout: + layout: tiled + panes: + - waitForControl; sleep 3; ~/.i3/layout_manager.sh ./layout.json diff --git a/controller_plugin/tmux/start.sh b/controller_plugin/tmux/start.sh new file mode 100755 index 0000000..5bb2275 --- /dev/null +++ b/controller_plugin/tmux/start.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +# Absolute path to this script. /home/user/bin/foo.sh +SCRIPT=$(readlink -f $0) +# Absolute path this script is in. /home/user/bin +SCRIPTPATH=`dirname $SCRIPT` +cd "$SCRIPTPATH" + +export TMUX_SESSION_NAME=simulation +export TMUX_SOCKET_NAME=mrs + +# start tmuxinator +tmuxinator start -p ./session.yml + +# if we are not in tmux +if [ -z $TMUX ]; then + + # just attach to the session + tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME + +# if we are in tmux +else + + # switch to the newly-started session + tmux detach-client -E "tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME" + +fi diff --git a/pluginlib_example/README.md b/pluginlib_example/README.md new file mode 100644 index 0000000..1d9ca3e --- /dev/null +++ b/pluginlib_example/README.md @@ -0,0 +1,27 @@ +# Pluginlib Example + +A working example showcasing the [pluginlib](http://wiki.ros.org/pluginlib) feature of ROS, simular to what is used between the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers#controlmanager) and the [trackers](https://github.com/ctu-mrs/mrs_uav_trackers#mrs-uav-trackers-) and [controllers](https://github.com/ctu-mrs/mrs_uav_controllers#mrs-uav-controllers-) within the [MRS UAV System](https://github.com/ctu-mrs/mrs_uav_system). + +## example_plugin_manager package + +* defines the [interface](https://github.com/ctu-mrs/example_ros_pluginlib/blob/master/example_plugin_manager/include/example_plugin_manager/plugin_interface.h) for plugins +* defines a [common handlers](https://github.com/ctu-mrs/example_ros_pluginlib/blob/master/example_plugin_manager/include/example_plugin_manager/common_handlers.h), which are passed to the plugins +* dynamically loads the plugins defined in [plugins.yaml](https://github.com/ctu-mrs/example_ros_pluginlib/blob/master/example_plugin_manager/config/plugins.yaml) and [example_plugin_manager.yaml](https://github.com/ctu-mrs/example_ros_pluginlib/blob/master/example_plugin_manager/config/example_plugin_manager.yaml) +* activates the plugin defined in [example_plugin_manager.yaml](https://github.com/ctu-mrs/example_ros_pluginlib/blob/master/example_plugin_manager/config/example_plugin_manager.yaml) +* regularly updates the active plugin and queries a result + +## example_plugins package + +* defines a plugin complying with the [interface](https://github.com/ctu-mrs/example_ros_pluginlib/blob/master/example_plugin_manager/include/example_plugin_manager/plugin_interface.h) +* the plugin loads its params and prepares itself for activation +* it calculates results in its `update()` method and returns them to the manager + +# How to start it? + +```bash +roslaunch example_plugin_manager example_plugin_manager.launch +``` + +# Dependencies + +* [mrs_lib](https://github.com/ctu-mrs/mrs_lib) for param loading and mutexing diff --git a/pluginlib_example/example_plugin_manager/CMakeLists.txt b/pluginlib_example/example_plugin_manager/CMakeLists.txt new file mode 100644 index 0000000..a018708 --- /dev/null +++ b/pluginlib_example/example_plugin_manager/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.15.0) +project(example_plugin_manager) + +set(CATKIN_DEPENDENCIES + roscpp + cmake_modules + mrs_lib + nodelet + ) + +set(LIBRARIES + ExamplePluginManager + ) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_DEPENDENCIES} + ) + +find_package(Eigen3 REQUIRED) +set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) +set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${LIBRARIES} + CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} + ) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ) + +# ExamplePluginManager + +add_library(ExamplePluginManager src/example_plugin_manager.cpp) + +add_dependencies(ExamplePluginManager + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + +target_link_libraries(ExamplePluginManager + ${catkin_LIBRARIES} + ) + +## -------------------------------------------------------------- +## | Install | +## -------------------------------------------------------------- + +install(TARGETS ${LIBRARIES} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +install(DIRECTORY include/example_plugin_manager/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + ) + +install(FILES nodelets.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) diff --git a/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml b/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml new file mode 100644 index 0000000..3ac0fe4 --- /dev/null +++ b/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml @@ -0,0 +1,12 @@ +# the list of "names" of the loaded plugins +# each plugin's properties are defined in 'plugins.yaml' +plugins : [ + "ExamplePlugin", + "ExamplePlugin2", +] + +# the first loaded plugin +# should be within "plugins" +initial_plugin: "ExamplePlugin2" + +update_timer_rate: 1 # [Hz] diff --git a/pluginlib_example/example_plugin_manager/config/plugins.yaml b/pluginlib_example/example_plugin_manager/config/plugins.yaml new file mode 100644 index 0000000..0537828 --- /dev/null +++ b/pluginlib_example/example_plugin_manager/config/plugins.yaml @@ -0,0 +1,12 @@ +# define "running instances" of plugins, there can be more than one for each + +ExamplePlugin: + address: "example_plugins/ExamplePlugin" # this is taken from the plugin's package.xml + name_space: "example_plugin" # useful for loading parameters into difference instances of the same plugin + some_property: 6.28 # this can be anything specific the manager needs to hold for each instance of each plugin + +# We can run another instance of the same plugin +ExamplePlugin2: + address: "example_plugins/ExamplePlugin" # this is taken from the plugin's package.xml + name_space: "example_plugin_2" + some_property: 10.1 diff --git a/pluginlib_example/example_plugin_manager/include/example_plugin_manager/common_handlers.h b/pluginlib_example/example_plugin_manager/include/example_plugin_manager/common_handlers.h new file mode 100644 index 0000000..58a75f6 --- /dev/null +++ b/pluginlib_example/example_plugin_manager/include/example_plugin_manager/common_handlers.h @@ -0,0 +1,33 @@ +#ifndef COMMON_HANDLERS_H +#define COMMON_HANDLERS_H + +#include +#include + +namespace example_plugin_manager +{ + +//} + +// | ---- logical units of supplied variables and functions --- | + +typedef boost::function vectorNorm_t; + +struct VectorCalculator_t +{ + bool enabled; + example_plugin_manager::vectorNorm_t vectorNorm; +}; + +// | ---------------------------------------------------------- | + +struct CommonHandlers_t +{ + std::shared_ptr some_shared_object; + VectorCalculator_t vector_calculator; +}; + + +} // namespace example_plugin_manager + +#endif // COMMON_HANDLERS_H diff --git a/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h b/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h new file mode 100644 index 0000000..e30af54 --- /dev/null +++ b/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h @@ -0,0 +1,27 @@ +#ifndef PLUGIN_INTERFACE_H +#define PLUGIN_INTERFACE_H + +#include + +#include + +namespace example_plugin_manager +{ + +class Plugin { +public: + virtual void initialize(const ros::NodeHandle& parent_nh, const std::string& name, const std::string& name_space, + std::shared_ptr common_handlers) = 0; + + virtual bool activate(const int& some_number) = 0; + + virtual void deactivate(void) = 0; + + virtual const std::optional update(const Eigen::Vector3d& input) = 0; + + virtual ~Plugin() = default; +}; + +} // namespace example_plugin_manager + +#endif diff --git a/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.launch b/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.launch new file mode 100644 index 0000000..c87ab11 --- /dev/null +++ b/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/pluginlib_example/example_plugin_manager/nodelets.xml b/pluginlib_example/example_plugin_manager/nodelets.xml new file mode 100644 index 0000000..65bb793 --- /dev/null +++ b/pluginlib_example/example_plugin_manager/nodelets.xml @@ -0,0 +1,5 @@ + + + ExamplePluginManager nodelet + + diff --git a/pluginlib_example/example_plugin_manager/package.xml b/pluginlib_example/example_plugin_manager/package.xml new file mode 100644 index 0000000..c985e76 --- /dev/null +++ b/pluginlib_example/example_plugin_manager/package.xml @@ -0,0 +1,24 @@ + + + + example_plugin_manager + 1.0.0 + The example_plugin_manager package + + Tomas Baca + Tomas Baca + + MIT + + catkin + + cmake_modules + roscpp + nodelet + mrs_lib + + + + + + diff --git a/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp b/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp new file mode 100644 index 0000000..72a7c76 --- /dev/null +++ b/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp @@ -0,0 +1,272 @@ +#include +#include + +#include + +#include +#include + +#include + +namespace example_plugin_manager +{ + +/* //{ class ExamplePluginManager */ + +/* class PluginParams() //{ */ + +class PluginParams { + +public: + PluginParams(const std::string& address, const std::string& name_space, const double& some_property); + +public: + std::string address; + std::string name_space; + double some_property; +}; + +PluginParams::PluginParams(const std::string& address, const std::string& name_space, const double& some_property) { + + this->address = address; + this->name_space = name_space; + this->some_property = some_property; +} + +//} + +class ExamplePluginManager : public nodelet::Nodelet { + +public: + virtual void onInit(); + +private: + ros::NodeHandle nh_; + bool is_initialized_ = false; + + // | ---------------------- update timer ---------------------- | + + ros::Timer timer_update_; + double _rate_timer_update_; + + // | -------- an object we want to share to our plugins ------- | + + std::string example_of_a_shared_object_; + + // | --------------------- common handlers -------------------- | + + std::shared_ptr common_handlers_; + + // | --------------- dynamic loading of plugins --------------- | + + std::unique_ptr> plugin_loader_; // pluginlib loader + std::vector _plugin_names_; + std::map plugins_; // map between plugin names and plugin params + std::vector> plugin_list_; // list of plugins, routines are callable from this + std::mutex mutex_plugins_; + + std::string _initial_plugin_name_; + int _initial_plugin_idx_ = 0; + + int active_plugin_idx_ = 0; + + // | ------------------------ routines ------------------------ | + + double vectorNorm(const Eigen::Vector3d& input); + + // | ------------------------- timers ------------------------- | + + void timerUpdate(const ros::TimerEvent& event); +}; + +//} + +/* onInit() //{ */ + +void ExamplePluginManager::onInit() { + + ros::NodeHandle nh_ = nodelet::Nodelet::getMTPrivateNodeHandle(); + + ros::Time::waitForValid(); + + ROS_INFO("[ExamplePluginManager]: initializing"); + + // -------------------------------------------------------------- + // | params | + // -------------------------------------------------------------- + + mrs_lib::ParamLoader param_loader(nh_, "ExamplePluginManager"); + + param_loader.loadParam("update_timer_rate", _rate_timer_update_); + param_loader.loadParam("initial_plugin", _initial_plugin_name_); + + // | --------------- example of a shared object --------------- | + + example_of_a_shared_object_ = "Hello, this is a shared object"; + + // -------------------------------------------------------------- + // | common handlers | + // -------------------------------------------------------------- + + common_handlers_ = std::make_shared(); + + common_handlers_->some_shared_object = std::make_shared(example_of_a_shared_object_); + + common_handlers_->vector_calculator.vectorNorm = boost::bind(&ExamplePluginManager::vectorNorm, this, _1); + common_handlers_->vector_calculator.enabled = true; + + // -------------------------------------------------------------- + // | load the plugins | + // -------------------------------------------------------------- + + param_loader.loadParam("plugins", _plugin_names_); + + plugin_loader_ = std::make_unique>("example_plugin_manager", "example_plugin_manager::Plugin"); + + // for each plugin in the list + for (int i = 0; i < int(_plugin_names_.size()); i++) { + std::string plugin_name = _plugin_names_[i]; + + // load the plugin parameters + std::string address; + std::string name_space; + double some_property; + + param_loader.loadParam(plugin_name + "/address", address); + param_loader.loadParam(plugin_name + "/name_space", name_space); + param_loader.loadParam(plugin_name + "/some_property", some_property); + + PluginParams new_plugin(address, name_space, some_property); + plugins_.insert(std::pair(plugin_name, new_plugin)); + + try { + ROS_INFO("[ExamplePluginManager]: loading the plugin '%s'", new_plugin.address.c_str()); + plugin_list_.push_back(plugin_loader_->createInstance(new_plugin.address.c_str())); + } + catch (pluginlib::CreateClassException& ex1) { + ROS_ERROR("[ExamplePluginManager]: CreateClassException for the plugin '%s'", new_plugin.address.c_str()); + ROS_ERROR("[ExamplePluginManager]: Error: %s", ex1.what()); + ros::shutdown(); + } + catch (pluginlib::PluginlibException& ex) { + ROS_ERROR("[ExamplePluginManager]: PluginlibException for the plugin '%s'", new_plugin.address.c_str()); + ROS_ERROR("[ExamplePluginManager]: Error: %s", ex.what()); + ros::shutdown(); + } + } + + ROS_INFO("[ExamplePluginManager]: plugins were loaded"); + + for (int i = 0; i < int(plugin_list_.size()); i++) { + try { + std::map::iterator it; + it = plugins_.find(_plugin_names_[i]); + + ROS_INFO("[ExamplePluginManager]: initializing the plugin '%s'", it->second.address.c_str()); + plugin_list_[i]->initialize(nh_, _plugin_names_[i], it->second.name_space, common_handlers_); + } + catch (std::runtime_error& ex) { + ROS_ERROR("[ExamplePluginManager]: exception caught during plugin initialization: '%s'", ex.what()); + } + } + + ROS_INFO("[ExamplePluginManager]: plugins were initialized"); + + // -------------------------------------------------------------- + // | check for existance of the initial plugin | + // -------------------------------------------------------------- + + { + bool check = false; + + for (int i = 0; i < int(_plugin_names_.size()); i++) { + + std::string plugin_name = _plugin_names_[i]; + + if (plugin_name == _initial_plugin_name_) { + check = true; + _initial_plugin_idx_ = i; + break; + } + } + if (!check) { + ROS_ERROR("[ExamplePluginManager]: the initial plugin (%s) is not within the loaded plugins", _initial_plugin_name_.c_str()); + ros::shutdown(); + } + } + + // | ---------- activate the first plugin on the list --------- | + + ROS_INFO("[ExamplePluginManager]: activating plugin with idx %d on the list (named: %s)", _initial_plugin_idx_, _plugin_names_[_initial_plugin_idx_].c_str()); + + int some_activation_input_to_plugin = 1234; + + plugin_list_[_initial_plugin_idx_]->activate(some_activation_input_to_plugin); + active_plugin_idx_ = _initial_plugin_idx_; + + // | ------------------------- timers ------------------------- | + + timer_update_ = nh_.createTimer(ros::Rate(_rate_timer_update_), &ExamplePluginManager::timerUpdate, this); + + // | ----------------------- finish init ---------------------- | + + if (!param_loader.loadedSuccessfully()) { + ROS_ERROR("[ExamplePluginManager]: could not load all parameters!"); + ros::shutdown(); + } + + is_initialized_ = true; + + ROS_INFO("[ExamplePluginManager]: initialized"); +} + +//} + +// | ------------------------- timers ------------------------- | + +/* timerUpdate() //{ */ + +void ExamplePluginManager::timerUpdate([[maybe_unused]] const ros::TimerEvent& event) { + + if (!is_initialized_) + return; + + auto active_plugin_idx = mrs_lib::get_mutexed(mutex_plugins_, active_plugin_idx_); + + // plugin input + Eigen::Vector3d input; + input << 0, 1, 2; + + // call the plugin's update routine + auto result = plugin_list_[active_plugin_idx]->update(input); + + if (result) { + + // print the result + ROS_INFO("[ExamplePluginManager]: plugin update() returned: %.2f", result.value()); + + } else { + + ROS_ERROR("[ExamplePluginManager]: plugin update failed!"); + } +} + +//} + +// | ------------------------ routines ------------------------ | + +/* vectorNorm() //{ */ + +double ExamplePluginManager::vectorNorm(const Eigen::Vector3d& input) { + + ROS_INFO("[ExamplePluginManager]: somebody called my vectorNorm() function, probably some plugin"); + + return input.norm(); +} + +//} + +} // namespace example_plugin_manager + +#include +PLUGINLIB_EXPORT_CLASS(example_plugin_manager::ExamplePluginManager, nodelet::Nodelet) diff --git a/pluginlib_example/example_plugins/CMakeLists.txt b/pluginlib_example/example_plugins/CMakeLists.txt new file mode 100644 index 0000000..4e50e68 --- /dev/null +++ b/pluginlib_example/example_plugins/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.15.0) +project(example_plugins) + +set(CATKIN_DEPENDENCIES + roscpp + cmake_modules + mrs_lib + example_plugin_manager + pluginlib + ) + +set(LIBRARIES + ExamplePlugin + ) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_DEPENDENCIES} + ) + +find_package(Eigen3 REQUIRED) +set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) +set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +add_definitions(-Wall) +add_definitions(-Wextra) +add_definitions(-Wpedantic) + +catkin_package( + LIBRARIES ${LIBRARIES} + CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} + DEPENDS Eigen + ) + +include_directories( + ${catkin_INCLUDE_DIRS} + ) + +# ExamplePlugin + +add_library(ExamplePlugin + src/example_plugin.cpp + ) + +add_dependencies(ExamplePlugin + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + +target_link_libraries(ExamplePlugin + ${catkin_LIBRARIES} + ${Eigen_LIBRARIES} + ) + +## -------------------------------------------------------------- +## | Install | +## -------------------------------------------------------------- + +install(TARGETS ${LIBRARIES} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +install(FILES plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) diff --git a/pluginlib_example/example_plugins/config/example_plugin.yaml b/pluginlib_example/example_plugins/config/example_plugin.yaml new file mode 100644 index 0000000..046d626 --- /dev/null +++ b/pluginlib_example/example_plugins/config/example_plugin.yaml @@ -0,0 +1 @@ +pi: 3.141592653 diff --git a/pluginlib_example/example_plugins/package.xml b/pluginlib_example/example_plugins/package.xml new file mode 100644 index 0000000..a6d331d --- /dev/null +++ b/pluginlib_example/example_plugins/package.xml @@ -0,0 +1,25 @@ + + + + example_plugins + 1.0.0 + The example_plugins package + + Tomas Baca + Tomas Baca + + MIT + + catkin + + pluginlib + roscpp + cmake_modules + mrs_lib + example_plugin_manager + + + + + + diff --git a/pluginlib_example/example_plugins/plugins.xml b/pluginlib_example/example_plugins/plugins.xml new file mode 100644 index 0000000..7158edc --- /dev/null +++ b/pluginlib_example/example_plugins/plugins.xml @@ -0,0 +1,5 @@ + + + This is ExamplePlugin. + + diff --git a/pluginlib_example/example_plugins/src/example_plugin.cpp b/pluginlib_example/example_plugins/src/example_plugin.cpp new file mode 100644 index 0000000..bae7683 --- /dev/null +++ b/pluginlib_example/example_plugins/src/example_plugin.cpp @@ -0,0 +1,139 @@ +#include +#include + +#include + +#include + +namespace example_plugins +{ + +namespace example_plugin +{ + +/* class ExamplePlugin //{ */ + +class ExamplePlugin : public example_plugin_manager::Plugin { + +public: + void initialize(const ros::NodeHandle& parent_nh, const std::string& name, const std::string& name_space, + std::shared_ptr common_handlers); + + bool activate(const int& some_number); + void deactivate(void); + + const std::optional update(const Eigen::Vector3d& input); + + // parameter from a config file + double _pi_; + + std::string _name_; + +private: + bool is_initialized_ = false; + bool is_active_ = false; + + std::shared_ptr common_handlers_; +}; + +//} + +// | -------------------- plugin interface -------------------- | + +/* initialize() //{ */ + +void ExamplePlugin::initialize(const ros::NodeHandle& parent_nh, const std::string& name, const std::string& name_space, + std::shared_ptr common_handlers) { + + // nh_ will behave just like normal NodeHandle + ros::NodeHandle nh_(parent_nh, name_space); + + _name_ = name; + + // I can use this to get stuff from the manager interactively + common_handlers_ = common_handlers; + + ros::Time::waitForValid(); + + // | ------------------- loading parameters ------------------- | + + mrs_lib::ParamLoader param_loader(nh_, "ExamplePlugin"); + + param_loader.addYamlFile(ros::package::getPath("example_plugins") + "/config/example_plugin.yaml"); + + // can load params like in a ROS node + param_loader.loadParam("pi", _pi_); + + if (!param_loader.loadedSuccessfully()) { + ROS_ERROR("[%s]: could not load all parameters!", _name_.c_str()); + ros::shutdown(); + } + + ROS_INFO("[%s]: loaded custom parameter: pi=%f", _name_.c_str(), _pi_); + + // | ----------------------- finish init ---------------------- | + + ROS_INFO("[%s]: initialized under the name '%s', and namespace '%s'", _name_.c_str(), name.c_str(), name_space.c_str()); + + is_initialized_ = true; +} + +//} + +/* activate() //{ */ + +bool ExamplePlugin::activate(const int& some_number) { + + ROS_INFO("[%s]: activated with some_number=%d", _name_.c_str(), some_number); + + is_active_ = true; + + return true; +} + +//} + +/* deactivate() //{ */ + +void ExamplePlugin::deactivate(void) { + + is_active_ = false; + + ROS_INFO("[%s]: deactivated", _name_.c_str()); +} + +//} + +/* update() //{ */ + +const std::optional ExamplePlugin::update(const Eigen::Vector3d& input) { + + if (!is_active_) { + return false; + } + + ROS_INFO_STREAM("[" << _name_ << "]: update() was called, let's find out the size of the vector [" << input.transpose() << "]"); + + // check some property from the "manager" + if (common_handlers_->vector_calculator.enabled) { + + // use a function from the common_handlers + double vector_norm = common_handlers_->vector_calculator.vectorNorm(input); + + // we calculated our result, just return it to the manager + return vector_norm; + + } else { + + return false; + } +} + +//} + +} // namespace example_plugin + +} // namespace example_plugins + +#include +PLUGINLIB_EXPORT_CLASS(example_plugins::example_plugin::ExamplePlugin, example_plugin_manager::Plugin) diff --git a/repurpose_package.sh b/repurpose_package.sh deleted file mode 100755 index 01c2e60..0000000 --- a/repurpose_package.sh +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env bash - -if [ $# -le 1 ]; then - echo "usage: ./repurpose_package.sh [--dry-run]" - if [ $1 == "--help" ]; then - echo " - Removes the .git directory, initializes a new one, - replaces all occurences of the original package name with the new one in all files withing all subfolders, - replaces all occurences of the original package name with the new one wthinin all filenames." - fi - exit 1 -fi - -ORIG_NAME=$1 -NEW_NAME=$2 - -if [ "$3" == "--dry-run" ]; then - echo "Dry run mode on - nothing will be changed" -fi - -# Remove .git and initialize a new repository - -echo "** CREATING NEW GIT REPOSITORY **" - -# Ask for confirmation -echo History of the old repository will be deleted. -read -p "Are you sure? " -r - -if [[ $REPLY =~ ^[Yy]$ ]] -then - - if [ "$3" != "--dry-run" ]; then - rm -rf .git - git init - fi - -fi - -# Replace occurences within files -readarray -d '' within_files < <(grep -rl "$ORIG_NAME" --null) - -echo "** REPLACING OCCURENCES WITHIN FILES **" - -if [[ -z "${within_files[@]}" ]]; then - - echo "No matches found for \"$1\" within files!" - -else - - # Ask for confirmation - echo These files will be modified: - for file in "${within_files[@]}" - do - echo " - $file" - done - read -p "Are you sure? " -r - - if [[ $REPLY =~ ^[Yy]$ ]] - then - - for file in "${within_files[@]}" - do - echo " - Replacing \"$1\" with \"$2\" in file \"$file\"." - if [ "$3" != "--dry-run" ]; then - sed -i "s/$ORIG_NAME/$NEW_NAME/g" "$file" - fi - done - - fi - -fi - -# Replace occurences in file names -readarray -d '' files < <(find -name "*$ORIG_NAME*" -print0) - -echo "** REPLACING OCCURENCES WITHIN FILE NAMES **" - -if [[ -z "${files[@]}" ]]; then - - echo "No matches found for \"$1\" in file names!" - -else - - # Ask for confirmation - echo These files will be modified: - for file in "${files[@]}" - do - echo " - $file" - done - read -p "Are you sure? " -r - - if [[ $REPLY =~ ^[Yy]$ ]] - then - - - for file in "${files[@]}" - do - new_file=${file//$ORIG_NAME/$NEW_NAME} - echo " - Renaming file \"$file\" to \"$new_file\"." - if [ "$3" != "--dry-run" ]; then - mv "$file" "$new_file" - fi - done - - fi - -fi - -if [ "$3" == "--dry-run" ]; then - echo "Dry run mode on - nothing was changed" -fi - diff --git a/ros_packages/example_ros_pluginlib b/ros_packages/example_ros_pluginlib deleted file mode 120000 index f975b99..0000000 --- a/ros_packages/example_ros_pluginlib +++ /dev/null @@ -1 +0,0 @@ -../.gitman/example_ros_pluginlib/. \ No newline at end of file diff --git a/ros_packages/example_ros_uav b/ros_packages/example_ros_uav deleted file mode 120000 index 44dfa07..0000000 --- a/ros_packages/example_ros_uav +++ /dev/null @@ -1 +0,0 @@ -../.gitman/example_ros_uav/. \ No newline at end of file diff --git a/ros_packages/example_ros_uav_simple b/ros_packages/example_ros_uav_simple deleted file mode 120000 index 20c1eb1..0000000 --- a/ros_packages/example_ros_uav_simple +++ /dev/null @@ -1 +0,0 @@ -../.gitman/example_ros_uav_simple/. \ No newline at end of file diff --git a/ros_packages/example_ros_vision b/ros_packages/example_ros_vision deleted file mode 120000 index 99d27a4..0000000 --- a/ros_packages/example_ros_vision +++ /dev/null @@ -1 +0,0 @@ -../.gitman/example_ros_vision/. \ No newline at end of file diff --git a/tmux_scripts/waypoint_flier/.gitignore b/tmux_scripts/waypoint_flier/.gitignore deleted file mode 100644 index f66f424..0000000 --- a/tmux_scripts/waypoint_flier/.gitignore +++ /dev/null @@ -1 +0,0 @@ -.tmuxinator.yml diff --git a/tmux_scripts/waypoint_flier/layout.json b/tmux_scripts/waypoint_flier/layout.json deleted file mode 100644 index cb0f507..0000000 --- a/tmux_scripts/waypoint_flier/layout.json +++ /dev/null @@ -1,190 +0,0 @@ -[ - { - "border": "normal", - "floating": "auto_off", - "fullscreen_mode": 0, - "layout": "splitv", - "percent": null, - "type": "con", - "nodes": [ - { - "border": "normal", - "floating": "auto_off", - "layout": "splith", - "percent": 1, - "type": "con", - "nodes": [ - { - "border": "normal", - "floating": "auto_off", - "layout": "splith", - "percent": 1, - "type": "con", - "nodes": [ - { - "border": "normal", - "floating": "auto_off", - "layout": "splitv", - "percent": 0.333333333333333, - "type": "con", - "nodes": [ - { - "border": "pixel", - "current_border_width": 3, - "floating": "auto_off", - "geometry": { - "height": 460, - "width": 724, - "x": 0, - "y": 0 - }, - "name": "runVim session.yml", - "percent": 0.5, - "swallows": [ - { - "instance": "^urxvt$" - } - ], - "type": "con" - }, - { - "border": "pixel", - "current_border_width": 3, - "floating": "auto_off", - "geometry": { - "height": 1060, - "width": 1920, - "x": 0, - "y": 0 - }, - "name": "Gazebo", - "percent": 0.5, - "swallows": [ - { - "instance": "^gazebo$" - } - ], - "type": "con" - } - ] - }, - { - "border": "normal", - "floating": "auto_off", - "layout": "splitv", - "percent": 0.333333333333333, - "type": "con", - "nodes": [ - { - "border": "pixel", - "current_border_width": 3, - "floating": "auto_off", - "geometry": { - "height": 460, - "width": 724, - "x": 0, - "y": 0 - }, - "name": "uav1.rviz* - RViz", - "percent": 0.5, - "swallows": [ - { - "instance": "^rviz$" - } - ], - "type": "con" - }, - { - "border": "pixel", - "current_border_width": 3, - "floating": "auto_off", - "geometry": { - "height": 460, - "width": 724, - "x": 0, - "y": 0 - }, - "name": "rqt_reconfigure__Param - rqt", - "percent": 0.5, - "swallows": [ - { - "instance": "^rqt_reconfigure$" - } - ], - "type": "con" - } - ] - }, - { - "border": "normal", - "floating": "auto_off", - "layout": "splitv", - "percent": 0.333333333333333, - "type": "con", - "nodes": [ - { - "border": "pixel", - "current_border_width": 3, - "floating": "auto_off", - "geometry": { - "height": 460, - "width": 724, - "x": 0, - "y": 0 - }, - "name": "edges", - "percent": 0.333333333333333, - "swallows": [ - { - "instance": "^original$" - } - ], - "type": "con" - }, - { - "border": "pixel", - "current_border_width": 3, - "floating": "auto_off", - "geometry": { - "height": 460, - "width": 724, - "x": 0, - "y": 0 - }, - "name": "world_point", - "percent": 0.333333333333333, - "swallows": [ - { - "instance": "^original$" - } - ], - "type": "con" - }, - { - "border": "pixel", - "current_border_width": 3, - "floating": "auto_off", - "geometry": { - "height": 460, - "width": 724, - "x": 0, - "y": 0 - }, - "name": "original", - "percent": 0.333333333333333, - "swallows": [ - { - "instance": "^original$" - } - ], - "type": "con" - } - ] - } - ] - } - ] - } - ] - } -] diff --git a/tmux_scripts/waypoint_flier/session.yml b/tmux_scripts/waypoint_flier/session.yml deleted file mode 100644 index c15b44b..0000000 --- a/tmux_scripts/waypoint_flier/session.yml +++ /dev/null @@ -1,62 +0,0 @@ -name: simulation -root: ./ -startup_window: status -pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=f550; export ODOMETRY_TYPE=gps; export SENSORS="garmin_down" -windows: - - roscore: - layout: even-vertical - panes: - - roscore - - gazebo: - layout: even-vertical - panes: - - waitForRos; roslaunch mrs_simulation simulation.launch gui:=true - - status: - layout: even-vertical - panes: - - waitForSimulation; roslaunch mrs_uav_status status.launch - - spawn: - layout: even-vertical - panes: - - waitForSimulation; rosservice call /mrs_drone_spawner/spawn "1 $UAV_TYPE --enable-rangefinder --enable-ground-truth --enable-mobius-camera-front" - - control: - layout: even-vertical - panes: - - waitForOdometry; roslaunch mrs_uav_general core.launch - - takeoff: - layout: even-vertical - panes: - - waitForSimulation; roslaunch mrs_uav_general automatic_start.launch - - 'waitForControl; - rosservice call /$UAV_NAME/mavros/cmd/arming 1; - sleep 2; - rosservice call /$UAV_NAME/mavros/set_mode 0 offboard' - - goto: - layout: even-vertical - panes: - - 'history -s rosservice call /$UAV_NAME/control_manager/goto \"goal: \[0.0, 10.0, 1.5, 0.0\]\"' - - edge_detect: - layout: even-vertical - panes: - - waitForControl; roslaunch vision_example vision_example.launch - - waypoint_flier: - layout: even-vertical - panes: - - waitForControl; roslaunch waypoint_flier waypoint_flier.launch - - history -s rosservice call /$UAV_NAME/waypoint_flier/fly_to_first_waypoint - - history -s rosservice call /$UAV_NAME/waypoint_flier/start_waypoints_following - - history -s rosservice call /$UAV_NAME/waypoint_flier/stop_waypoints_following - - dynamic_reconfigure: - layout: even-vertical - panes: - - waitForControl; rosrun rqt_reconfigure rqt_reconfigure - - gazebo_camera_follow: - layout: even-vertical - panes: - - waitForOdometry; gz camera -c gzclient_camera -f uav1; history -s gz camera -c gzclient_camera -f uav1 - - waitForControl; sleep 3; ~/.i3/layout_manager.sh layout.json - - rviz: - layout: even-vertical - panes: - - waitForControl; roslaunch mrs_uav_testing rviz.launch - - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch diff --git a/tmux_scripts/waypoint_flier/start.sh b/tmux_scripts/waypoint_flier/start.sh deleted file mode 100755 index 8deef36..0000000 --- a/tmux_scripts/waypoint_flier/start.sh +++ /dev/null @@ -1,19 +0,0 @@ -#!/bin/bash - -if_is_link=`readlink $0` -command_name=$0 -if [[ if_is_link ]]; then - command_name=$if_is_link -fi -MY_PATH=`dirname "$if_is_link"` -MY_PATH=`( cd "$MY_PATH" && pwd )` -cd $MY_PATH - -# remove the old link -rm .tmuxinator.yml - -# link the session file to .tmuxinator.yml -ln session.yml .tmuxinator.yml - -# start tmuxinator -tmuxinator diff --git a/tracker_plugin/CMakeLists.txt b/tracker_plugin/CMakeLists.txt new file mode 100644 index 0000000..c617bdd --- /dev/null +++ b/tracker_plugin/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.1.2) +project(example_tracker_plugin) + +set(CATKIN_DEPENDENCIES + cmake_modules + roscpp + rospy + dynamic_reconfigure + mrs_lib + mrs_msgs + mrs_uav_managers + ) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_DEPENDENCIES} + ) + +generate_dynamic_reconfigure_options( + cfg/example_tracker.cfg + ) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") + +# include Eigen3 +find_package(Eigen3 REQUIRED) +set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) +set(Eigen_LIBRARIES ${EIGEN3_LIBRARIES}) + +set(LIBRARIES + ExampleTracker + ) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} + LIBRARIES ${LIBRARIES} + DEPENDS Eigen + ) + +## -------------------------------------------------------------- +## | Build | +## -------------------------------------------------------------- + +include_directories( + include + ${EIGEN_INCLUDE_DIR} + ${catkin_INCLUDE_DIRS} + ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake + ) + +# MPC Tracker + +add_library(ExampleTracker + src/example_tracker.cpp + ) + +add_dependencies(ExampleTracker + ${catkin_EXPORTED_TARGETS} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ) + +target_link_libraries(ExampleTracker + ${catkin_LIBRARIES} + ) + +## -------------------------------------------------------------- +## | Install | +## -------------------------------------------------------------- + +install(TARGETS ${LIBRARIES} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +install(FILES plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) diff --git a/tracker_plugin/README.md b/tracker_plugin/README.md new file mode 100644 index 0000000..836b859 --- /dev/null +++ b/tracker_plugin/README.md @@ -0,0 +1,2 @@ +# example_tracker_plugin +Example tracker plugin for the MRS Control manager. diff --git a/tracker_plugin/cfg/example_tracker.cfg b/tracker_plugin/cfg/example_tracker.cfg new file mode 100755 index 0000000..934c156 --- /dev/null +++ b/tracker_plugin/cfg/example_tracker.cfg @@ -0,0 +1,15 @@ +#!/usr/bin/env python +PACKAGE = "example_tracker_plugin" + +import roslib; +roslib.load_manifest(PACKAGE) + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +dynamics = gen.add_group("Dynamics"); + +dynamics.add("some_parameter", double_t, 0, "Some parameter", 0.0, -10.0, 10.0) + +exit(gen.generate(PACKAGE, "ExampleTracker", "example_tracker")) diff --git a/tracker_plugin/config/example_tracker.yaml b/tracker_plugin/config/example_tracker.yaml new file mode 100644 index 0000000..a3bbd39 --- /dev/null +++ b/tracker_plugin/config/example_tracker.yaml @@ -0,0 +1 @@ +some_parameter: 1.0 # [-] diff --git a/.gitmodules b/tracker_plugin/include/.gitkeep similarity index 100% rename from .gitmodules rename to tracker_plugin/include/.gitkeep diff --git a/tracker_plugin/package.xml b/tracker_plugin/package.xml new file mode 100644 index 0000000..adbf645 --- /dev/null +++ b/tracker_plugin/package.xml @@ -0,0 +1,28 @@ + + + + example_tracker_plugin + 1.0.0 + The example_controller_plugin package + + Tomas Baca + Tomas Baca + + BSD 3-Clause + + catkin + + cmake_modules + rospy + pluginlib + roscpp + mrs_msgs + mrs_lib + mrs_uav_managers + dynamic_reconfigure + + + + + + diff --git a/tracker_plugin/plugins.xml b/tracker_plugin/plugins.xml new file mode 100644 index 0000000..3dffad5 --- /dev/null +++ b/tracker_plugin/plugins.xml @@ -0,0 +1,5 @@ + + + This is the example tracker. + + diff --git a/tracker_plugin/src/example_tracker.cpp b/tracker_plugin/src/example_tracker.cpp new file mode 100644 index 0000000..3bfbe3d --- /dev/null +++ b/tracker_plugin/src/example_tracker.cpp @@ -0,0 +1,520 @@ +/* includes //{ */ + +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include + +//} + +namespace example_tracker_plugin +{ + +namespace example_tracker +{ + +/* //{ class ExampleTracker */ + +class ExampleTracker : public mrs_uav_managers::Tracker { +public: + bool initialize(const ros::NodeHandle &nh, std::shared_ptr common_handlers, + std::shared_ptr private_handlers); + + std::tuple activate(const std::optional &last_tracker_cmd); + void deactivate(void); + bool resetStatic(void); + + std::optional update(const mrs_msgs::UavState &uav_state, const mrs_uav_managers::Controller::ControlOutput &last_control_output); + const mrs_msgs::TrackerStatus getStatus(); + const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd); + const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState &new_uav_state); + + const mrs_msgs::ReferenceSrvResponse::ConstPtr setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd); + const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd); + const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd); + + const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd); + + const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr &cmd); + const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd); + const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd); + const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd); + const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr &cmd); + +private: + ros::NodeHandle nh_; + + bool callbacks_enabled_ = true; + + std::string _uav_name_; + + std::shared_ptr common_handlers_; + std::shared_ptr private_handlers_; + + // | ------------------------ uav state ----------------------- | + + mrs_msgs::UavState uav_state_; + bool got_uav_state_ = false; + std::mutex mutex_uav_state_; + + // | ------------------ dynamics constriants ------------------ | + + mrs_msgs::DynamicsConstraints constraints_; + std::mutex mutex_constraints_; + + // | ----------------------- goal state ----------------------- | + + double goal_x_ = 0; + double goal_y_ = 0; + double goal_z_ = 0; + double goal_heading_ = 0; + + // | ---------------- the tracker's inner state --------------- | + + std::atomic is_initialized_ = false; + std::atomic is_active_ = false; + + double pos_x_ = 0; + double pos_y_ = 0; + double pos_z_ = 0; + double heading_ = 0; + + // | ------------------- for calculating dt ------------------- | + + ros::Time last_update_time_; + std::atomic first_iteration_ = true; + + // | --------------- dynamic reconfigure server --------------- | + + boost::recursive_mutex mutex_drs_; + typedef example_tracker_plugin::example_trackerConfig DrsConfig_t; + typedef dynamic_reconfigure::Server Drs_t; + boost::shared_ptr drs_; + void callbackDrs(example_tracker_plugin::example_trackerConfig &config, uint32_t level); + DrsConfig_t drs_params_; + std::mutex mutex_drs_params_; +}; + +//} + +// | -------------- tracker's interface routines -------------- | + +/* //{ initialize() */ + +bool ExampleTracker::initialize(const ros::NodeHandle &nh, std::shared_ptr common_handlers, + std::shared_ptr private_handlers) { + + this->common_handlers_ = common_handlers; + this->private_handlers_ = private_handlers; + + _uav_name_ = common_handlers->uav_name; + + nh_ = nh; + + ros::Time::waitForValid(); + + last_update_time_ = ros::Time(0); + + // -------------------------------------------------------------- + // | loading parameters | + // -------------------------------------------------------------- + + // | -------------------- load param files -------------------- | + + bool success = true; + + // FYI + // This method will load the file using `rosparam get` + // Pros: you can the full power of the official param loading + // Cons: it is slower + // + // Alternatives: + // You can load the file directly into the ParamLoader as shown below. + + success *= private_handlers->loadConfigFile(ros::package::getPath("example_tracker_plugin") + "/config/example_tracker.yaml"); + + if (!success) { + return false; + } + + // | ---------------- load plugin's parameters ---------------- | + + mrs_lib::ParamLoader param_loader(nh_, "ExampleTracker"); + + // This is the alternaive way of loading the config file. + // + // Files loaded using this method are prioritized over ROS params. + // + // param_loader.addYamlFile(ros::package::getPath("example_tracker_plugin") + "/config/example_tracker.yaml"); + + param_loader.loadParam("some_parameter", drs_params_.some_parameter); + + + if (!param_loader.loadedSuccessfully()) { + ROS_ERROR("[ExampleTracker]: could not load all parameters!"); + return false; + } + + // | --------------- dynamic reconfigure server --------------- | + + drs_.reset(new Drs_t(mutex_drs_, nh_)); + drs_->updateConfig(drs_params_); + Drs_t::CallbackType f = boost::bind(&ExampleTracker::callbackDrs, this, _1, _2); + drs_->setCallback(f); + + // | --------------------- finish the init -------------------- | + + is_initialized_ = true; + + ROS_INFO("[ExampleTracker]: initialized"); + + return true; +} + +//} + +/* //{ activate() */ + +std::tuple ExampleTracker::activate([[maybe_unused]] const std::optional &last_tracker_cmd) { + + if (last_tracker_cmd) { + + // actually, you should actually check if these entries are filled in + pos_x_ = last_tracker_cmd->position.x; + pos_y_ = last_tracker_cmd->position.y; + pos_z_ = last_tracker_cmd->position.z; + heading_ = last_tracker_cmd->heading; + + goal_x_ = pos_x_; + goal_y_ = pos_y_; + goal_z_ = pos_z_; + goal_heading_ = heading_; + + } else { + + auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_); + + pos_x_ = uav_state.pose.position.x; + pos_y_ = uav_state.pose.position.y; + pos_z_ = uav_state.pose.position.z; + + try { + heading_ = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading(); + } + catch (...) { + heading_ = 0; + } + } + + std::stringstream ss; + ss << "activated"; + ROS_INFO_STREAM("[ExampleTracker]: " << ss.str()); + + is_active_ = true; + + return std::tuple(true, ss.str()); +} + +//} + +/* //{ deactivate() */ + +void ExampleTracker::deactivate(void) { + + is_active_ = false; + + ROS_INFO("[ExampleTracker]: deactivated"); +} + +//} + +/* //{ update() */ + +std::optional ExampleTracker::update(const mrs_msgs::UavState & uav_state, + [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput &last_control_output) { + + { + std::scoped_lock lock(mutex_uav_state_); + + uav_state_ = uav_state; + + got_uav_state_ = true; + } + + // | ---------- calculate dt from the last iteration ---------- | + + double dt; + + if (first_iteration_) { + dt = 0.01; + first_iteration_ = false; + } else { + dt = (uav_state.header.stamp - last_update_time_).toSec(); + } + + last_update_time_ = uav_state.header.stamp; + + if (fabs(dt) < 0.001) { + + ROS_DEBUG("[ExampleTracker]: the last odometry message came too close (%.2f s)!", dt); + dt = 0.01; + } + + // up to this part the update() method is evaluated even when the tracker is not active + if (!is_active_) { + return {}; + } + + // | ------------------ move the inner state ------------------ | + + auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_); + + Eigen::Vector2d vec_to_goal_horizontal(goal_x_ - pos_x_, goal_y_ - pos_y_); + double to_goal_vertical = goal_z_ - pos_z_; + double to_goal_heading = mrs_lib::geometry::sradians::diff(goal_heading_, heading_); + + Eigen::Vector2d step_horizontal = vec_to_goal_horizontal.normalized() * constraints.horizontal_speed * dt; + + double step_vertical; + if (to_goal_vertical >= 0) { + step_vertical = constraints.vertical_ascending_speed * dt; + } else { + step_vertical = -constraints.vertical_descending_speed * dt; + } + + double step_heading = mrs_lib::signum(to_goal_heading) * constraints.heading_speed * dt; + + pos_x_ += step_horizontal(0); + pos_y_ += step_horizontal(1); + pos_z_ += step_vertical; + heading_ += step_heading; + + // | --------------- check for reaching the goal -------------- | + + if (vec_to_goal_horizontal.norm() <= constraints.horizontal_speed * dt) { + pos_x_ = goal_x_; + pos_y_ = goal_y_; + } + + if (to_goal_vertical > 0 && to_goal_vertical <= constraints.vertical_ascending_speed * dt) { + pos_z_ = goal_z_; + } + + if (to_goal_vertical < 0 && -to_goal_vertical <= constraints.vertical_descending_speed * dt) { + pos_z_ = goal_z_; + } + + if (std::abs(to_goal_heading) <= constraints.heading_speed * dt) { + heading_ = goal_heading_; + } + + // | ------------------- fill in the result ------------------- | + + mrs_msgs::TrackerCommand tracker_cmd; + + tracker_cmd.header.stamp = ros::Time::now(); + tracker_cmd.header.frame_id = uav_state.header.frame_id; + + tracker_cmd.position.x = pos_x_; + tracker_cmd.position.y = pos_y_; + tracker_cmd.position.z = pos_z_; + tracker_cmd.heading = heading_; + + tracker_cmd.use_position_vertical = 1; + tracker_cmd.use_position_horizontal = 1; + tracker_cmd.use_heading = 1; + + return {tracker_cmd}; +} + +//} + +/* //{ resetStatic() */ + +bool ExampleTracker::resetStatic(void) { + + return false; +} + +//} + +/* //{ getStatus() */ + +const mrs_msgs::TrackerStatus ExampleTracker::getStatus() { + + mrs_msgs::TrackerStatus tracker_status; + + tracker_status.active = is_active_; + tracker_status.callbacks_enabled = callbacks_enabled_; + + return tracker_status; +} + +//} + +/* //{ enableCallbacks() */ + +const std_srvs::SetBoolResponse::ConstPtr ExampleTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) { + + std_srvs::SetBoolResponse res; + std::stringstream ss; + + if (cmd->data != callbacks_enabled_) { + + callbacks_enabled_ = cmd->data; + + ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled"); + ROS_INFO_STREAM_THROTTLE(1.0, "[ExampleTracker]: " << ss.str()); + + } else { + + ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled"); + ROS_WARN_STREAM_THROTTLE(1.0, "[ExampleTracker]: " << ss.str()); + } + + res.message = ss.str(); + res.success = true; + + return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res)); +} + +//} + +/* switchOdometrySource() //{ */ + +const std_srvs::TriggerResponse::ConstPtr ExampleTracker::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState &new_uav_state) { + + return std_srvs::TriggerResponse::Ptr(); +} + +//} + +/* //{ hover() */ + +const std_srvs::TriggerResponse::ConstPtr ExampleTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) { + + return std_srvs::TriggerResponse::Ptr(); +} + +//} + +/* //{ startTrajectoryTracking() */ + +const std_srvs::TriggerResponse::ConstPtr ExampleTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) { + return std_srvs::TriggerResponse::Ptr(); +} + +//} + +/* //{ stopTrajectoryTracking() */ + +const std_srvs::TriggerResponse::ConstPtr ExampleTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) { + return std_srvs::TriggerResponse::Ptr(); +} + +//} + +/* //{ resumeTrajectoryTracking() */ + +const std_srvs::TriggerResponse::ConstPtr ExampleTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) { + return std_srvs::TriggerResponse::Ptr(); +} + +//} + +/* //{ gotoTrajectoryStart() */ + +const std_srvs::TriggerResponse::ConstPtr ExampleTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) { + return std_srvs::TriggerResponse::Ptr(); +} + +//} + +/* //{ setConstraints() */ + +const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr ExampleTracker::setConstraints([ + [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) { + + { + std::scoped_lock lock(mutex_constraints_); + + constraints_ = cmd->constraints; + } + + mrs_msgs::DynamicsConstraintsSrvResponse res; + + res.success = true; + res.message = "constraints updated"; + + return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res)); +} + +//} + +/* //{ setReference() */ + +const mrs_msgs::ReferenceSrvResponse::ConstPtr ExampleTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) { + + goal_x_ = cmd->reference.position.x; + goal_y_ = cmd->reference.position.y; + goal_z_ = cmd->reference.position.z; + goal_heading_ = cmd->reference.heading; + + mrs_msgs::ReferenceSrvResponse response; + + response.message = "reference set"; + response.success = true; + + return mrs_msgs::ReferenceSrvResponse::ConstPtr(new mrs_msgs::ReferenceSrvResponse(response)); +} + +//} + +/* //{ setVelocityReference() */ + +const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr ExampleTracker::setVelocityReference([ + [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) { + return mrs_msgs::VelocityReferenceSrvResponse::Ptr(); +} + +//} + +/* //{ setTrajectoryReference() */ + +const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr ExampleTracker::setTrajectoryReference([ + [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) { + return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr(); +} + +//} + +// -------------------------------------------------------------- +// | callbacks | +// -------------------------------------------------------------- + +/* //{ callbackDrs() */ + +void ExampleTracker::callbackDrs(example_tracker_plugin::example_trackerConfig &config, [[maybe_unused]] uint32_t level) { + + mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_); + + ROS_INFO("[Exampletracker]: dynamic reconfigure params updated"); +} + +//} + +} // namespace example_tracker + +} // namespace example_tracker_plugin + +#include +PLUGINLIB_EXPORT_CLASS(example_tracker_plugin::example_tracker::ExampleTracker, mrs_uav_managers::Tracker) diff --git a/tracker_plugin/tmux/config/custom_config.yaml b/tracker_plugin/tmux/config/custom_config.yaml new file mode 100644 index 0000000..2aa8f9c --- /dev/null +++ b/tracker_plugin/tmux/config/custom_config.yaml @@ -0,0 +1,29 @@ +mrs_uav_managers: + + control_manager: + + ExampleTracker: + address: "example_tracker_plugin/ExampleTracker" + namespace: "example_tracker" + human_switchable: true + + # list of names of dynamically loaded trackers + trackers : [ + "ExampleTracker", + ] + + uav_manager: + + takeoff: + after_takeoff: + tracker: "ExampleTracker" + + midair_activation: + after_activation: + tracker: "ExampleTracker" + + min_height_checking: + enabled: false + + max_height_checking: + enabled: false diff --git a/tracker_plugin/tmux/config/hw_api.yaml b/tracker_plugin/tmux/config/hw_api.yaml new file mode 100644 index 0000000..d98d5e5 --- /dev/null +++ b/tracker_plugin/tmux/config/hw_api.yaml @@ -0,0 +1,28 @@ +input_mode: + actuators: false + control_group: false + attitude_rate: true + attitude: true + acceleration_hdg_rate: false + acceleration_hdg: false + velocity_hdg_rate: false + velocity_hdg: false + position: false + + feedforward: true + +outputs: + distance_sensor: true + gnss: true + rtk: false + imu: false + altitude: true + magnetometer_heading: true + rc_channels: false + battery_state: false + position: true + orientation: true + velocity: true + angular_velocity: true + odometry: false + ground_truth: true diff --git a/tracker_plugin/tmux/config/network_config.yaml b/tracker_plugin/tmux/config/network_config.yaml new file mode 100644 index 0000000..b42e1fb --- /dev/null +++ b/tracker_plugin/tmux/config/network_config.yaml @@ -0,0 +1,17 @@ +# 1. This list is used by NimbroNetwork for mutual communication of the UAVs +# The names of the robots have to match hostnames described in /etc/hosts. +# +# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. +# The names should match the true "UAV_NAMES" (the topic prefixes). +# +# network_config:=~/config/network_config.yaml +# +# to the core.launch and nimbro.launch. + +network: + + robot_names: [ + + uav1, + + ] diff --git a/tracker_plugin/tmux/config/simulator.yaml b/tracker_plugin/tmux/config/simulator.yaml new file mode 100644 index 0000000..895b991 --- /dev/null +++ b/tracker_plugin/tmux/config/simulator.yaml @@ -0,0 +1,18 @@ +simulation_rate: 250.0 +clock_rate: 250.0 +realtime_factor: 1.0 + +individual_takeoff_platform: + enabled: false + +uav_names: [ + "uav1", +] + +uav1: + type: "x500" + spawn: + x: 10.0 + y: 15.0 + z: 2.0 + heading: 0 diff --git a/tracker_plugin/tmux/config/world_config.yaml b/tracker_plugin/tmux/config/world_config.yaml new file mode 100644 index 0000000..9b55067 --- /dev/null +++ b/tracker_plugin/tmux/config/world_config.yaml @@ -0,0 +1,34 @@ +world_origin: + + units: "LATLON" # {"UTM, "LATLON"} + + origin_x: 47.397743 + origin_y: 8.545594 + +safety_area: + + enabled: true + + horizontal: + + # the frame of reference in which the points are expressed + frame_name: "world_origin" + + # polygon + # + # x, y [m] for any frame_name except latlon_origin + # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" + points: [ + -50, -50, + 50, -50, + 50, 50, + -50, 50, + ] + + vertical: + + # the frame of reference in which the max&min z is expressed + frame_name: "world_origin" + + max_z: 15.0 + min_z: 0.5 diff --git a/tracker_plugin/tmux/layout.json b/tracker_plugin/tmux/layout.json new file mode 100644 index 0000000..5664ebd --- /dev/null +++ b/tracker_plugin/tmux/layout.json @@ -0,0 +1,50 @@ +[ +{ + "border": "normal", + "floating": "auto_off", + "fullscreen_mode": 0, + "layout": "splitv", + "percent": 0.285714285714286, + "type": "con", + "nodes": [ + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 1030, + "width": 936, + "x": 4806, + "y": 32 + }, + "name": "rviz.rviz* - RViz", + "percent": 0.5, + "swallows": [ + { + "instance": "^rviz$" + } + ], + "type": "con" + }, + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 460, + "width": 724, + "x": 0, + "y": 0 + }, + "name": "./start.sh", + "percent": 0.5, + "swallows": [ + { + "instance": "^urxvt$" + } + ], + "type": "con" + } + ] +} +] diff --git a/tracker_plugin/tmux/session.yml b/tracker_plugin/tmux/session.yml new file mode 100644 index 0000000..5a12297 --- /dev/null +++ b/tracker_plugin/tmux/session.yml @@ -0,0 +1,54 @@ +# do not modify these +root: ./ +name: simulation +socket_name: mrs +attach: false +tmux_options: -f /etc/ctu-mrs/tmux.conf +# you can modify these +pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500 +startup_window: control +windows: + - roscore: + layout: tiled + panes: + - roscore + - simulator: + layout: tiled + panes: + - waitForRos; roslaunch mrs_multirotor_simulator multirotor_simulator.launch custom_config:=./config/simulator.yaml + - hw_api: + layout: tiled + panes: + - waitForTime; roslaunch mrs_multirotor_simulator hw_api.launch custom_config:=./config/hw_api.yaml + - takeoff: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_autostart automatic_start.launch + - waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard + # - midair_activation: + # # disabled the "takeoff" section and turn on the "individual_takeoff_platform" in the simulator config + # layout: tiled + # panes: + # - waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; rosservice call /$UAV_NAME/uav_manager/midair_activation + - status: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_status status.launch + - control: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_core core.launch + platform_config:=`rospack find mrs_multirotor_simulator`/config/mrs_uav_system/$UAV_TYPE.yaml + custom_config:=./config/custom_config.yaml + world_config:=./config/world_config.yaml + network_config:=./config/network_config.yaml + - rviz: + layout: tiled + panes: + - waitForControl; roslaunch mrs_uav_core rviz.launch + - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch + - waitForControl; waitForControl; roslaunch mrs_rviz_plugins rviz_interface.launch + - layout: + layout: tiled + panes: + - waitForControl; sleep 3; ~/.i3/layout_manager.sh ./layout.json diff --git a/tracker_plugin/tmux/start.sh b/tracker_plugin/tmux/start.sh new file mode 100755 index 0000000..5bb2275 --- /dev/null +++ b/tracker_plugin/tmux/start.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +# Absolute path to this script. /home/user/bin/foo.sh +SCRIPT=$(readlink -f $0) +# Absolute path this script is in. /home/user/bin +SCRIPTPATH=`dirname $SCRIPT` +cd "$SCRIPTPATH" + +export TMUX_SESSION_NAME=simulation +export TMUX_SOCKET_NAME=mrs + +# start tmuxinator +tmuxinator start -p ./session.yml + +# if we are not in tmux +if [ -z $TMUX ]; then + + # just attach to the session + tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME + +# if we are in tmux +else + + # switch to the newly-started session + tmux detach-client -E "tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME" + +fi diff --git a/waypoint_flier/CMakeLists.txt b/waypoint_flier/CMakeLists.txt new file mode 100644 index 0000000..6a8d3c7 --- /dev/null +++ b/waypoint_flier/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.15.0) +project(waypoint_flier) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +SET(CATKIN_DEPENDENCIES + roscpp + nodelet + dynamic_reconfigure + geometry_msgs + nav_msgs + mrs_msgs + mrs_lib + ) + +SET(LIBRARIES + WaypointFlier + ) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_DEPENDENCIES} + ) + +find_package(Eigen3 REQUIRED) +set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) +set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) + +## Generate dynamic parameters which are loaded by the dynamic reconfigure server +generate_dynamic_reconfigure_options( + config/dynparam.cfg + ) + +catkin_package( + LIBRARIES ${LIBRARIES} + CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} + DEPENDS Eigen + ) + +include_directories( + ${catkin_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS} + ) + +## Declare a C++ library +add_library(WaypointFlier + src/waypoint_flier.cpp + ) + +## Add configure headers for dynamic reconfigure +add_dependencies(WaypointFlier + ${PROJECT_NAME}_gencfg + ) + +## Specify libraries to link a library or executable target against +target_link_libraries(WaypointFlier + ${catkin_LIBRARIES} + ${Eigen_LIBRARIES} + ) + +## -------------------------------------------------------------- +## | Install | +## -------------------------------------------------------------- + +install(TARGETS ${LIBRARIES} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +install(FILES nodelets.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) diff --git a/waypoint_flier/README.md b/waypoint_flier/README.md new file mode 100644 index 0000000..486a3f0 --- /dev/null +++ b/waypoint_flier/README.md @@ -0,0 +1,75 @@ +# MRS ROS example + +This package was created as an example of how to write basic ROS nodelets. +The package is written in C++ and features custom MRS libraries and msgs. +You can test the program in simulation (see our [simulation tutorial](https://ctu-mrs.github.io/docs/simulation/howto.html)). + +## Functionality + +* Desired waypoints are loaded as a matrix from config file +* Service `fly_to_first_waypoint` prepares the UAV by flying to the first waypoint +* Service `start_waypoint_following` causes the UAV to start tracking the waypoints +* Service `stop_waypoint_following` stops adding new waypoints. Flight to the current waypoint is not interrupted. + +## Package structure + +See [ROS packages](http://wiki.ros.org/Packages) + +* `src` directory contains all source files +* `include` directory contains all header files. It is good practice to separate them from source files. +* `launch` directory contains `.launch` files which are used to parametrize the nodelet. Command-line arguments, as well as environment variables, can be loaded from the launch files, the nodelet can be put into the correct namespace (each UAV has its namespace to allow multi-robot applications), config files are loaded, and parameters passed to the nodelet. See [.launch files](http://wiki.ros.org/roslaunch/XML) +* `config` directory contains parameters in `.yaml` files. See [.yaml files](http://wiki.ros.org/rosparam) +* `package.xml` defines properties of the package, such as package name and dependencies. See [package.xml](http://wiki.ros.org/catkin/package.xml) + +## Example features + +* [Nodelet](http://wiki.ros.org/nodelet) initialization +* [Subscriber, publisher](http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29), and [timer](http://wiki.ros.org/roscpp/Overview/Timers) initialization +* [Service servers and clients](http://wiki.ros.org/roscpp/Overview/Services) initialization +* Loading [parameters](http://wiki.ros.org/Parameter%20Server) with `mrs_lib::ParamLoader` class +* Loading [Eigen matrices](https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html) with `mrs_lib::ParamLoader` class +* Checking nodelet initialization status in every callback +* Checking whether subscribed messages are coming +* Throttling [text output](http://wiki.ros.org/roscpp/Overview/Logging) to a terminal +* [Thread-safe access](https://en.cppreference.com/w/cpp/thread/mutex) to variables using `std::lock_scope()` +* Using `ConstPtr` when subscribing to a topic to avoid copying large messages +* Storing and accessing matrices in `Eigen` classes +* [Remapping topics](http://wiki.ros.org/roslaunch/XML/remap) in the launch file + +## Coding style + +For easy orientation in the code, we have agreed to follow the [ROS C++ Style Guide](http://wiki.ros.org/CppStyleGuide) when writing our packages. +Also check out our general [C++ good/bad coding practices tutorial](https://ctu-mrs.github.io/docs/introduction/c_to_cpp.html). + +### Naming variables + +* Member variables are distinguished from local variables by underscore at the end: + - `position_x` - local variable + - `position_x_` - member variable +* Also, we distinguish parameters which are loaded as parameters by underscore at the beginning + - `_simulation_` - parameter +* Descriptive variable names are used. The purpose of the variable should be obvious from the name. + - `sub_odom_uav_` - member subscriber to uav odometry msg type + - `pub_reference_` - member publisher of reference msg type + - `srv_server_start_waypoints_following_` - member service server for starting following of waypoints + - `WaypointFlier::callbackTimerCheckSubscribers()` - callback of timer which checks subscribers + - `mutex_odom_uav_` - mutex locking access to variable containing odometry of the UAV + +### Good practices + +* [Nodelet everything!](https://www.clearpathrobotics.com/assets/guides/ros/Nodelet%20Everything.html) Nodelets compared to nodes do not need to send whole messages. Multiple nodelets running under the same nodelet manager form one process and messages can be passed as pointers. +* Do not use raw pointers! Smart pointers from `` free resources automatically, thus preventing memory leaks. +* Lock access to member variables! Nodelets are multi-thread processes, so it is our responsibility to make our code thread-safe. + - Use `c++17` `scoped_lock` which unlocks the mutex after leaving the scope. This way, you can't forget to unlock the mutex. + ```cpp + { + std::scoped_lock lock(mutex_odom_uav_); + odom_uav_ = *msg; + } + ``` +* Use `ros::Time::waitForValid()` after creating node handle `ros::NodeHandle nh("~")` +* When a nodelet is initialized, the method `onInit()` is called. In the method, the subscribers are initialized, and callbacks are bound to them. The callbacks can run even before the `onInit()` method ends, which can lead to some variables being still not initialized, parameters not loaded, etc. This can be prevented by using an `is_initialized_`, initializing it to `false` at the beginning of `onInit()` and setting it to true at the end. Every callback should check this variable and continue only when it is `true`. +* Use `mrs_lib::ParamLoader` class to load parameters from launch files and config files. This class checks whether the parameter was actually loaded, which can save a lot of debugging. Furthermore, loading matrices into config files becomes much simpler. +* For printing debug info to terminal use `ROS_INFO()`, `ROS_WARN()`, `ROS_ERROR()` macros. Do not spam the terminal by printing a variable every time a callback is called, use for example `ROS_INFO_THROTTLE(1.0, "dog")` to print *dog* not more often than every second. Other animals can also be used for debugging purposes. +* If you need to execute a piece of code periodically, do not use sleep in a loop, or anything similar. The ROS API provides `ros::Timer` class for this purposes, which executes a callback every time the timer expires. +* Always check whether all subscribed messages are coming. If not, print a warning. Then you know the problem is not in your nodelet and you know to look for the problem in topic remapping or the node publishing it. diff --git a/waypoint_flier/config/dynparam.cfg b/waypoint_flier/config/dynparam.cfg new file mode 100755 index 0000000..597d42f --- /dev/null +++ b/waypoint_flier/config/dynparam.cfg @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +PACKAGE='waypoint_flier' +import roslib; +roslib.load_manifest(PACKAGE) + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator(); +params = gen.add_group("waypoint_flier parameters"); + +params.add("waypoint_idle_time", double_t, 1, "The time to wait before flying to the next waypoint", 1, 0, 60); + +exit(gen.generate(PACKAGE, "waypoint_flier", "dynparam")) diff --git a/waypoint_flier/config/waypoint_flier.yaml b/waypoint_flier/config/waypoint_flier.yaml new file mode 100644 index 0000000..132f44a --- /dev/null +++ b/waypoint_flier/config/waypoint_flier.yaml @@ -0,0 +1,28 @@ +# rates of timers in Hz +rate: + publish_dist_to_waypoint: 1 # [Hz] + publish_reference: 10 # [Hz] + check_subscribers: 1 # [Hz] + +# number of times the waypoint sequence should be repeated +n_loops: 1 # [-] + +# should the UAV land after flying through all the waypoints? +land_at_the_end: false + +# how long should the UAV idle at a waypoint? +waypoint_idle_time: 1.0 + +# matrix of waypoints which will be flown through by the UAV +# x [m], y [m], z [m], heading [rad] +waypoints: [-10, -10, 3, 0, + 10, -10, 3, 1.57, + 10, 10, 3, 3.14, + -10, 10, 3, 4.61] + +# in which frame of reference are the points specified? +waypoints_frame: "world_origin" + +# offset will be added to all waypoints +# x [m], y [m], z [m], yaw [rad] +offset: [0, 0, 0, 0] diff --git a/waypoint_flier/launch/waypoint_flier.launch b/waypoint_flier/launch/waypoint_flier.launch new file mode 100644 index 0000000..9901bc4 --- /dev/null +++ b/waypoint_flier/launch/waypoint_flier.launch @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/waypoint_flier/nodelets.xml b/waypoint_flier/nodelets.xml new file mode 100644 index 0000000..c664e48 --- /dev/null +++ b/waypoint_flier/nodelets.xml @@ -0,0 +1,5 @@ + + + WaypointFlier nodelet + + diff --git a/waypoint_flier/package.xml b/waypoint_flier/package.xml new file mode 100644 index 0000000..de947f8 --- /dev/null +++ b/waypoint_flier/package.xml @@ -0,0 +1,30 @@ + + + + waypoint_flier + 1.0.0 + waypoint_flier + + Matej Petrlik + Matej Petrlik + + MIT + + catkin + + roscpp + nodelet + geometry_msgs + nav_msgs + mrs_msgs + mrs_lib + dynamic_reconfigure + + + + + + + + + diff --git a/waypoint_flier/src/waypoint_flier.cpp b/waypoint_flier/src/waypoint_flier.cpp new file mode 100644 index 0000000..07addda --- /dev/null +++ b/waypoint_flier/src/waypoint_flier.cpp @@ -0,0 +1,649 @@ +/* includes //{ */ + +/* each ros package must have these */ +#include +#include +#include + +/* for loading dynamic parameters while the nodelet is running */ +#include + +/* this header file is created during compilation from python script dynparam.cfg */ +#include + +/* for smart pointers (do not use raw pointers) */ +#include + +/* for protecting variables from simultaneous by from multiple threads */ +#include + +/* for writing and reading from streams */ +#include +#include + +/* for storing information about the state of the uav (position) */ +#include +#include + +/* for storing information about the state of the uav (position, twist) + covariances */ +#include + +/* custom msgs of MRS group */ +#include +#include +#include + +/* custom helper functions from our library */ +#include +#include +#include +#include +#include +#include + +/* for calling simple ros services */ +#include + +/* for operations with matrices */ +#include + +//} + +using vec2_t = mrs_lib::geometry::vec_t<2>; +using vec3_t = mrs_lib::geometry::vec_t<3>; + +namespace waypoint_flier +{ + +/* class WaypointFlier //{ */ + +class WaypointFlier : public nodelet::Nodelet { + +public: + /* onInit() is called when nodelet is launched (similar to main() in regular node) */ + virtual void onInit(); + +private: + /* flags */ + std::atomic is_initialized_ = false; + + /* ros parameters */ + std::string _uav_name_; + + // | ---------------------- msg callbacks --------------------- | + + mrs_lib::SubscribeHandler sh_odometry_; + mrs_lib::SubscribeHandler sh_control_manager_diag_; + + void callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnostics::ConstPtr msg); + std::atomic have_goal_ = false; + + // | --------------------- timer callbacks -------------------- | + + void timerPublishDistToWaypoint(const ros::TimerEvent& te); + ros::Publisher pub_dist_to_waypoint_; + ros::Timer timer_publish_dist_to_waypoint_; + int _rate_timer_publish_dist_to_waypoint_; + + void timerPublishSetReference(const ros::TimerEvent& te); + ros::Publisher pub_reference_; + ros::Timer timer_publisher_reference_; + int _rate_timer_publisher_reference_; + + void timerCheckSubscribers(const ros::TimerEvent& te); + ros::Timer timer_check_subscribers_; + int _rate_timer_check_subscribers_; + + // | ---------------- service server callbacks ---------------- | + + bool callbackStartWaypointFollowing(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); + ros::ServiceServer srv_server_start_waypoints_following_; + + bool callbackStopWaypointFollowing(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); + ros::ServiceServer srv_server_stop_waypoints_following_; + + bool callbackFlyToFirstWaypoint(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); + ros::ServiceServer srv_server_fly_to_first_waypoint_; + + // | --------------------- service clients -------------------- | + + ros::ServiceClient srv_client_land_; + bool _land_end_; + + // | -------------------- loading waypoints ------------------- | + + std::vector waypoints_; + std::string _waypoints_frame_; + bool waypoints_loaded_ = false; + mrs_msgs::Reference current_waypoint_; + std::mutex mutex_current_waypoint_; + int idx_current_waypoint_; + int n_waypoints_; + int _n_loops_; + int c_loop_; + std::mutex mutex_waypoint_idle_time_; + Eigen::MatrixXd _offset_; + + // | ------------------- dynamic reconfigure ------------------ | + + typedef waypoint_flier::dynparamConfig Config; + typedef dynamic_reconfigure::Server ReconfigureServer; + boost::recursive_mutex mutex_dynamic_reconfigure_; + boost::shared_ptr reconfigure_server_; + void callbackDynamicReconfigure(Config& config, uint32_t level); + waypoint_flier::dynparamConfig last_drs_config_; + + // | --------------------- waypoint idling -------------------- | + + bool is_idling_ = false; + ros::Timer timer_idling_; + double _waypoint_idle_time_; + void timerIdling(const ros::TimerEvent& te); + + // | -------------------- support functions ------------------- | + + std::vector matrixToPoints(const Eigen::MatrixXd& matrix); + + void offsetPoints(std::vector& points, const Eigen::MatrixXd& offset); + + double distance(const mrs_msgs::Reference& waypoint, const geometry_msgs::Pose& pose); +}; + +//} + +/* onInit() //{ */ + +void WaypointFlier::onInit() { + + // | ---------------- set my booleans to false ---------------- | + // but remember, always set them to their default value in the header file + // because, when you add new one later, you might forger to come back here + + have_goal_ = false; + is_idling_ = false; + waypoints_loaded_ = false; + + /* obtain node handle */ + ros::NodeHandle nh = nodelet::Nodelet::getMTPrivateNodeHandle(); + + /* waits for the ROS to publish clock */ + ros::Time::waitForValid(); + + // | ------------------- load ros parameters ------------------ | + /* (mrs_lib implementation checks whether the parameter was loaded or not) */ + mrs_lib::ParamLoader param_loader(nh, "WaypointFlier"); + + param_loader.loadParam("uav_name", _uav_name_); + param_loader.loadParam("land_at_the_end", _land_end_); + param_loader.loadParam("n_loops", _n_loops_); + param_loader.loadParam("waypoint_idle_time", _waypoint_idle_time_); + param_loader.loadParam("waypoints_frame", _waypoints_frame_); + param_loader.loadParam("rate/publish_dist_to_waypoint", _rate_timer_publish_dist_to_waypoint_); + param_loader.loadParam("rate/publish_reference", _rate_timer_publisher_reference_); + param_loader.loadParam("rate/check_subscribers", _rate_timer_check_subscribers_); + + /* load waypoints as a half-dynamic matrix from config file */ + Eigen::MatrixXd waypoint_matrix; + param_loader.loadMatrixDynamic("waypoints", waypoint_matrix, -1, 4); // -1 indicates the dynamic dimension + waypoints_ = matrixToPoints(waypoint_matrix); + n_waypoints_ = waypoints_.size(); + waypoints_loaded_ = true; + idx_current_waypoint_ = 0; + c_loop_ = 0; + ROS_INFO_STREAM_ONCE("[WaypointFlier]: " << n_waypoints_ << " waypoints loaded"); + ROS_INFO_STREAM_ONCE("[WaypointFlier]: " << _n_loops_ << " loops requested"); + + /* load offset of all waypoints as a static matrix from config file */ + param_loader.loadMatrixStatic("offset", _offset_, 1, 4); + offsetPoints(waypoints_, _offset_); + + if (!param_loader.loadedSuccessfully()) { + ROS_ERROR("[WaypointFlier]: failed to load non-optional parameters!"); + ros::shutdown(); + } + + // | ------------------ initialize subscribers ----------------- | + + mrs_lib::SubscribeHandlerOptions shopts; + shopts.nh = nh; + shopts.node_name = "WaypointFlier"; + shopts.no_message_timeout = ros::Duration(1.0); + shopts.threadsafe = true; + shopts.autostart = true; + shopts.queue_size = 10; + shopts.transport_hints = ros::TransportHints().tcpNoDelay(); + + sh_odometry_ = mrs_lib::SubscribeHandler(shopts, "odom_uav_in"); + sh_control_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "control_manager_diagnostics_in", + &WaypointFlier::callbackControlManagerDiag, this); + + // | ------------------ initialize publishers ----------------- | + + pub_dist_to_waypoint_ = nh.advertise("dist_to_waypoint_out", 1); + pub_reference_ = nh.advertise("reference_out", 1); + + // | -------------------- initialize timers ------------------- | + + timer_publish_dist_to_waypoint_ = nh.createTimer(ros::Rate(_rate_timer_publish_dist_to_waypoint_), &WaypointFlier::timerPublishDistToWaypoint, this); + + timer_check_subscribers_ = nh.createTimer(ros::Rate(_rate_timer_check_subscribers_), &WaypointFlier::timerCheckSubscribers, this); + + // you can disable autostarting of the timer by the last argument + timer_publisher_reference_ = nh.createTimer(ros::Rate(_rate_timer_publisher_reference_), &WaypointFlier::timerPublishSetReference, this, false, false); + + // | --------------- initialize service servers --------------- | + + srv_server_start_waypoints_following_ = nh.advertiseService("start_waypoints_following_in", &WaypointFlier::callbackStartWaypointFollowing, this); + srv_server_stop_waypoints_following_ = nh.advertiseService("stop_waypoints_following_in", &WaypointFlier::callbackStopWaypointFollowing, this); + srv_server_fly_to_first_waypoint_ = nh.advertiseService("fly_to_first_waypoint_in", &WaypointFlier::callbackFlyToFirstWaypoint, this); + + // | --------------- initialize service clients --------------- | + + srv_client_land_ = nh.serviceClient("land_out"); + + // | ---------- initialize dynamic reconfigure server --------- | + + reconfigure_server_.reset(new ReconfigureServer(mutex_dynamic_reconfigure_, nh)); + ReconfigureServer::CallbackType f = boost::bind(&WaypointFlier::callbackDynamicReconfigure, this, _1, _2); + reconfigure_server_->setCallback(f); + + /* set the default value of dynamic reconfigure server to the value of parameter with the same name */ + { + std::scoped_lock lock(mutex_waypoint_idle_time_); + last_drs_config_.waypoint_idle_time = _waypoint_idle_time_; + } + reconfigure_server_->updateConfig(last_drs_config_); + + ROS_INFO_ONCE("[WaypointFlier]: initialized"); + + is_initialized_ = true; +} + +//} + +// | ---------------------- msg callbacks --------------------- | + +/* callbackControlManagerDiag() //{ */ + +void WaypointFlier::callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnostics::ConstPtr diagnostics) { + + /* do not continue if the nodelet is not initialized */ + if (!is_initialized_) { + return; + } + + ROS_INFO_ONCE("[WaypointFlier]: Received first control manager diagnostics msg"); + + // get the variable under the mutex + mrs_msgs::Reference current_waypoint = mrs_lib::get_mutexed(mutex_current_waypoint_, current_waypoint_); + + // extract the pose part of the odometry + geometry_msgs::Pose current_pose = mrs_lib::getPose(sh_odometry_.getMsg()); + + double dist = distance(current_waypoint, current_pose); + ROS_INFO("[WaypointFlier]: Distance to waypoint: %.2f", dist); + + if (have_goal_ && dist < 0.1 && !diagnostics->tracker_status.have_goal) { + + ROS_INFO("[WaypointFlier]: Waypoint reached."); + have_goal_ = false; + + /* start idling at the reached waypoint */ + is_idling_ = true; + + ros::NodeHandle nh("~"); + timer_idling_ = nh.createTimer(ros::Duration(_waypoint_idle_time_), &WaypointFlier::timerIdling, this, + true); // the last boolean argument makes the timer run only once + + ROS_INFO("[WaypointFlier]: Idling for %.2f seconds.", _waypoint_idle_time_); + } +} + +//} + +// | --------------------- timer callbacks -------------------- | + +/* timerPublishSetReference() //{ */ + +void WaypointFlier::timerPublishSetReference([[maybe_unused]] const ros::TimerEvent& te) { + + if (!is_initialized_) { + return; + } + + /* return if the uav is still flying to the previous waypoints */ + if (have_goal_) { + return; + } + + /* return if the UAV is idling at a waypoint */ + if (is_idling_) { + return; + } + + /* shutdown node after flying through all the waypoints (call land service before) */ + if (idx_current_waypoint_ >= n_waypoints_) { + + c_loop_++; + + ROS_INFO("[WaypointFlier]: Finished loop %d/%d", c_loop_, _n_loops_); + + if (c_loop_ >= _n_loops_) { + + ROS_INFO("[WaypointFlier]: Finished %d loops of %d waypoints.", _n_loops_, n_waypoints_); + + if (_land_end_) { + ROS_INFO("[WaypointFlier]: Calling land service."); + std_srvs::Trigger srv_land_call; + srv_client_land_.call(srv_land_call); + } + + ROS_INFO("[WaypointFlier]: Shutting down."); + ros::shutdown(); + return; + + } else { + ROS_INFO("[WaypointFlier]: Starting loop %d/%d", c_loop_ + 1, _n_loops_); + idx_current_waypoint_ = 0; + } + } + + /* create new waypoint msg */ + mrs_msgs::ReferenceStamped new_waypoint; + + // set the frame id in which the reference is expressed + new_waypoint.header.frame_id = _uav_name_ + "/" + _waypoints_frame_; + new_waypoint.header.stamp = ros::Time::now(); + + new_waypoint.reference = waypoints_.at(idx_current_waypoint_); + + // set the variable under the mutex + mrs_lib::set_mutexed(mutex_current_waypoint_, waypoints_.at(idx_current_waypoint_), current_waypoint_); + + ROS_INFO("[WaypointFlier]: Flying to waypoint %d: x: %.2f y: %.2f z: %.2f heading: %.2f", idx_current_waypoint_ + 1, new_waypoint.reference.position.x, + new_waypoint.reference.position.y, new_waypoint.reference.position.z, new_waypoint.reference.heading); + + try { + pub_reference_.publish(new_waypoint); + } + catch (...) { + ROS_ERROR("Exception caught during publishing topic %s.", pub_reference_.getTopic().c_str()); + } + + idx_current_waypoint_++; + + have_goal_ = true; +} + +//} + +/* timerPublishDistToWaypoint() //{ */ + +void WaypointFlier::timerPublishDistToWaypoint([[maybe_unused]] const ros::TimerEvent& te) { + + if (!is_initialized_) { + return; + } + + /* do not publish distance to waypoint when the uav is not flying towards a waypoint */ + if (!have_goal_) { + return; + } + + // this routine can not work without the odometry + if (!sh_odometry_.hasMsg()) { + return; + } + + // get the variable under the mutex + mrs_msgs::Reference current_waypoint = mrs_lib::get_mutexed(mutex_current_waypoint_, current_waypoint_); + + // extract the pose part of the odometry + geometry_msgs::Pose current_pose = mrs_lib::getPose(sh_odometry_.getMsg()); + + double dist = distance(current_waypoint, current_pose); + ROS_INFO("[WaypointFlier]: Distance to waypoint: %.2f", dist); + + mrs_msgs::Float64Stamped dist_msg; + // it is important to set the frame id correctly !! + dist_msg.header.frame_id = _uav_name_ + "/" + _waypoints_frame_; + dist_msg.header.stamp = ros::Time::now(); + dist_msg.value = dist; + + try { + pub_dist_to_waypoint_.publish(dist_msg); + } + catch (...) { + ROS_ERROR("Exception caught during publishing topic %s.", pub_dist_to_waypoint_.getTopic().c_str()); + } +} + +//} + +/* timerCheckSubscribers() //{ */ + +void WaypointFlier::timerCheckSubscribers([[maybe_unused]] const ros::TimerEvent& te) { + + if (!is_initialized_) { + return; + } + + if (!sh_odometry_.hasMsg()) { + ROS_WARN_THROTTLE(1.0, "[WaypointFlier]: Not received uav odom msg since node launch."); + } + + if (!sh_control_manager_diag_.hasMsg()) { + ROS_WARN_THROTTLE(1.0, "[WaypointFlier]: Not received tracker diagnostics msg since node launch."); + } +} + +//} + +/* timerIdling() //{ */ + +void WaypointFlier::timerIdling([[maybe_unused]] const ros::TimerEvent& te) { + + ROS_INFO("[WaypointFlier]: Idling finished"); + is_idling_ = false; +} + +//} + +// | -------------------- service callbacks ------------------- | + +/* //{ callbackStartWaypointFollowing() */ + +bool WaypointFlier::callbackStartWaypointFollowing([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) { + + if (!is_initialized_) { + + res.success = false; + res.message = "Waypoint flier not initialized!"; + ROS_WARN("[WaypointFlier]: Cannot start waypoint following, nodelet is not initialized."); + return true; + } + + if (waypoints_loaded_) { + + timer_publisher_reference_.start(); + + ROS_INFO("[WaypointFlier]: Starting waypoint following."); + + res.success = true; + res.message = "Starting waypoint following."; + + } else { + + ROS_WARN("[WaypointFlier]: Cannot start waypoint following, waypoints are not set."); + res.success = false; + res.message = "Waypoints not set."; + } + + return true; +} + +//} + +/* //{ callbackStopWaypointFollowing() */ + +bool WaypointFlier::callbackStopWaypointFollowing([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) { + + if (!is_initialized_) { + + res.success = false; + res.message = "Waypoint flier not initialized!"; + ROS_WARN("[WaypointFlier]: Cannot stop waypoint following, nodelet is not initialized."); + return true; + } + + timer_publisher_reference_.stop(); + + ROS_INFO("[WaypointFlier]: Waypoint following stopped."); + + res.success = true; + res.message = "Waypoint following stopped."; + + return true; +} + +//} + +/* //{ callbackFlyToFirstWaypoint() */ + +bool WaypointFlier::callbackFlyToFirstWaypoint([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) { + + if (!is_initialized_) { + + res.success = false; + res.message = "Waypoint flier not initialized!"; + ROS_WARN("[WaypointFlier]: Cannot start waypoint following, nodelet is not initialized."); + + return true; + } + + if (waypoints_loaded_) { + + /* create new waypoint msg */ + mrs_msgs::ReferenceStamped new_waypoint; + + // it is important to set the frame id correctly !! + new_waypoint.header.frame_id = _uav_name_ + "/" + _waypoints_frame_; + new_waypoint.header.stamp = ros::Time::now(); + new_waypoint.reference = waypoints_.at(0); + + mrs_lib::set_mutexed(mutex_current_waypoint_, waypoints_.at(0), current_waypoint_); + + // set the variable under the mutex + + idx_current_waypoint_ = 0; + c_loop_ = 0; + + have_goal_ = true; + + try { + pub_reference_.publish(new_waypoint); + } + catch (...) { + ROS_ERROR("Exception caught during publishing topic %s.", pub_reference_.getTopic().c_str()); + } + + std::stringstream ss; + ss << "Flying to first waypoint: x: " << new_waypoint.reference.position.x << ", y: " << new_waypoint.reference.position.y + << ", z: " << new_waypoint.reference.position.z << ", heading: " << new_waypoint.reference.heading; + + ROS_INFO_STREAM_THROTTLE(1.0, "[WaypointFlier]: " << ss.str()); + + res.success = true; + res.message = ss.str(); + + } else { + + ROS_WARN("[WaypointFlier]: Cannot fly to first waypoint, waypoints not loaded!"); + + res.success = false; + res.message = "Waypoints not loaded"; + } + + return true; +} + +//} + +// | -------------- dynamic reconfigure callback -------------- | + +/* //{ callbackDynamicReconfigure() */ + +void WaypointFlier::callbackDynamicReconfigure([[maybe_unused]] Config& config, [[maybe_unused]] uint32_t level) { + + if (!is_initialized_) + return; + + ROS_INFO( + "[WaypointFlier]:" + "Reconfigure Request: " + "Waypoint idle time: %.2f", + config.waypoint_idle_time); + + { + std::scoped_lock lock(mutex_waypoint_idle_time_); + + _waypoint_idle_time_ = config.waypoint_idle_time; + } +} + +//} + +// | -------------------- support functions ------------------- | + +/* matrixToPoints() //{ */ + +std::vector WaypointFlier::matrixToPoints(const Eigen::MatrixXd& matrix) { + + std::vector points; + + for (int i = 0; i < matrix.rows(); i++) { + + mrs_msgs::Reference point; + point.position.x = matrix.row(i)(0); + point.position.y = matrix.row(i)(1); + point.position.z = matrix.row(i)(2); + point.heading = matrix.row(i)(3); + + points.push_back(point); + } + + return points; +} + +//} + +/* offsetPoints() //{ */ + +void WaypointFlier::offsetPoints(std::vector& points, const Eigen::MatrixXd& offset) { + + for (size_t i = 0; i < points.size(); i++) { + + points.at(i).position.x += offset(0); + points.at(i).position.y += offset(1); + points.at(i).position.z += offset(2); + points.at(i).heading += offset(3); + } +} + +//} + +/* distance() //{ */ + +double WaypointFlier::distance(const mrs_msgs::Reference& waypoint, const geometry_msgs::Pose& pose) { + + return mrs_lib::geometry::dist(vec3_t(waypoint.position.x, waypoint.position.y, waypoint.position.z), + vec3_t(pose.position.x, pose.position.y, pose.position.z)); +} + +//} + +} // namespace waypoint_flier + +/* every nodelet must include macros which export the class as a nodelet plugin */ +#include +PLUGINLIB_EXPORT_CLASS(waypoint_flier::WaypointFlier, nodelet::Nodelet); diff --git a/waypoint_flier/tmux/config/automatic_start.yaml b/waypoint_flier/tmux/config/automatic_start.yaml new file mode 100644 index 0000000..d351868 --- /dev/null +++ b/waypoint_flier/tmux/config/automatic_start.yaml @@ -0,0 +1,5 @@ +# A timeout between the takeoff being triggered and the UAV actually taking off +# while the timeout is counting down, the takeoff can be aborted by switching off +# the offboard mode. +# default = 5 sec +safety_timeout: 1.0 # [s] diff --git a/waypoint_flier/tmux/config/custom_config.yaml b/waypoint_flier/tmux/config/custom_config.yaml new file mode 100644 index 0000000..c1f518c --- /dev/null +++ b/waypoint_flier/tmux/config/custom_config.yaml @@ -0,0 +1,39 @@ +# GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: +## -------------------------------------------------------------- +## | rosrun mrs_uav_core get_public_params.py # +## -------------------------------------------------------------- + +mrs_uav_managers: + + estimation_manager: + + # loaded state estimator plugins + state_estimators: [ + "gps_garmin", + "gps_baro", + ] + + initial_state_estimator: "gps_baro" # will be used as the first state estimator + agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) + + uav_manager: + + takeoff: + + during_takeoff: + controller: "MpcController" + tracker: "LandoffTracker" + + after_takeoff: + controller: "Se3Controller" + tracker: "MpcTracker" + + midair_activation: + + during_activation: + controller: "MidairActivationController" + tracker: "MidairActivationTracker" + + after_activation: + controller: "Se3Controller" + tracker: "MpcTracker" diff --git a/waypoint_flier/tmux/config/hw_api.yaml b/waypoint_flier/tmux/config/hw_api.yaml new file mode 100644 index 0000000..30a6919 --- /dev/null +++ b/waypoint_flier/tmux/config/hw_api.yaml @@ -0,0 +1,28 @@ +input_mode: + actuators: false + control_group: false + attitude_rate: true + attitude: true + acceleration_hdg_rate: false + acceleration_hdg: false + velocity_hdg_rate: false + velocity_hdg: false + position: false + + feedforward: true + +outputs: + distance_sensor: true + gnss: true + rtk: true + imu: true + altitude: true + magnetometer_heading: true + rc_channels: true + battery_state: true + position: true + orientation: true + velocity: true + angular_velocity: true + odometry: true + ground_truth: true diff --git a/waypoint_flier/tmux/config/network_config.yaml b/waypoint_flier/tmux/config/network_config.yaml new file mode 100644 index 0000000..88d3e9f --- /dev/null +++ b/waypoint_flier/tmux/config/network_config.yaml @@ -0,0 +1,20 @@ +# 1. This list is used by NimbroNetwork for mutual communication of the UAVs +# The names of the robots have to match hostnames described in /etc/hosts. +# +# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. +# The names should match the true "UAV_NAMES" (the topic prefixes). +# +# network_config:=~/config/network_config.yaml +# +# to the core.launch and nimbro.launch. + +network: + + robot_names: [ + + uav1, + uav2, + uav3, + uav4, + + ] diff --git a/waypoint_flier/tmux/config/simulator.yaml b/waypoint_flier/tmux/config/simulator.yaml new file mode 100644 index 0000000..7f7346a --- /dev/null +++ b/waypoint_flier/tmux/config/simulator.yaml @@ -0,0 +1,20 @@ +simulation_rate: 250 +clock_rate: 250 +realtime_factor: 1.0 + +iterate_without_input: true + +individual_takeoff_platform: + enabled: false + +uav_names: [ + "uav1", +] + +uav1: + type: "x500" + spawn: + x: 10.0 + y: 15.0 + z: 2.0 + heading: 0 diff --git a/waypoint_flier/tmux/config/world_config.yaml b/waypoint_flier/tmux/config/world_config.yaml new file mode 100644 index 0000000..9b55067 --- /dev/null +++ b/waypoint_flier/tmux/config/world_config.yaml @@ -0,0 +1,34 @@ +world_origin: + + units: "LATLON" # {"UTM, "LATLON"} + + origin_x: 47.397743 + origin_y: 8.545594 + +safety_area: + + enabled: true + + horizontal: + + # the frame of reference in which the points are expressed + frame_name: "world_origin" + + # polygon + # + # x, y [m] for any frame_name except latlon_origin + # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" + points: [ + -50, -50, + 50, -50, + 50, 50, + -50, 50, + ] + + vertical: + + # the frame of reference in which the max&min z is expressed + frame_name: "world_origin" + + max_z: 15.0 + min_z: 0.5 diff --git a/waypoint_flier/tmux/kill.sh b/waypoint_flier/tmux/kill.sh new file mode 100755 index 0000000..28d50b0 --- /dev/null +++ b/waypoint_flier/tmux/kill.sh @@ -0,0 +1,14 @@ +#!/bin/bash + +# Absolute path to this script. /home/user/bin/foo.sh +SCRIPT=$(readlink -f $0) +# Absolute path this script is in. /home/user/bin +SCRIPTPATH=`dirname $SCRIPT` +cd "$SCRIPTPATH" + +export TMUX_SESSION_NAME=simulation +export TMUX_SOCKET_NAME=mrs + +# just attach to the session +tmux -L $TMUX_SOCKET_NAME split-window -t $TMUX_SESSION_NAME +tmux -L $TMUX_SOCKET_NAME send-keys -t $TMUX_SESSION_NAME "sleep 1; tmux list-panes -s -F \"#{pane_pid} #{pane_current_command}\" | grep -v tmux | cut -d\" \" -f1 | while read in; do killp \$in; done; exit" ENTER diff --git a/waypoint_flier/tmux/layout.json b/waypoint_flier/tmux/layout.json new file mode 100644 index 0000000..5664ebd --- /dev/null +++ b/waypoint_flier/tmux/layout.json @@ -0,0 +1,50 @@ +[ +{ + "border": "normal", + "floating": "auto_off", + "fullscreen_mode": 0, + "layout": "splitv", + "percent": 0.285714285714286, + "type": "con", + "nodes": [ + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 1030, + "width": 936, + "x": 4806, + "y": 32 + }, + "name": "rviz.rviz* - RViz", + "percent": 0.5, + "swallows": [ + { + "instance": "^rviz$" + } + ], + "type": "con" + }, + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 460, + "width": 724, + "x": 0, + "y": 0 + }, + "name": "./start.sh", + "percent": 0.5, + "swallows": [ + { + "instance": "^urxvt$" + } + ], + "type": "con" + } + ] +} +] diff --git a/waypoint_flier/tmux/session.yml b/waypoint_flier/tmux/session.yml new file mode 100644 index 0000000..9e433d6 --- /dev/null +++ b/waypoint_flier/tmux/session.yml @@ -0,0 +1,56 @@ +# do not modify these +root: ./ +name: simulation +socket_name: mrs +attach: false +tmux_options: -f /etc/ctu-mrs/tmux.conf +# you can modify these +pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500 +startup_window: waypointflier +windows: + - roscore: + layout: tiled + panes: + - roscore + - simulator: + layout: tiled + panes: + - waitForRos; roslaunch mrs_multirotor_simulator multirotor_simulator.launch custom_config:=./config/simulator.yaml + - hw_api: + layout: tiled + panes: + - waitForTime; roslaunch mrs_multirotor_simulator hw_api.launch custom_config:=./config/hw_api.yaml + - takeoff: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_autostart automatic_start.launch custom_config:=./config/automatic_start.yaml + - waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard + - status: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_status status.launch + - control: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_core core.launch + platform_config:=`rospack find mrs_multirotor_simulator`/config/mrs_uav_system/$UAV_TYPE.yaml + custom_config:=./config/custom_config.yaml + world_config:=./config/world_config.yaml + network_config:=./config/network_config.yaml + - waypointflier: + layout: tiled + panes: + - waitForControl; roslaunch waypoint_flier waypoint_flier.launch + - 'history -s rosservice call /$UAV_NAME/waypoint_flier/fly_to_first_waypoint' + - 'history -s rosservice call /$UAV_NAME/waypoint_flier/start_waypoints_following' + - 'history -s rosservice call /$UAV_NAME/waypoint_flier/stop_waypoints_following' + - rviz: + layout: tiled + panes: + - waitForControl; roslaunch mrs_uav_core rviz.launch + - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch + - waitForControl; waitForControl; roslaunch mrs_rviz_plugins rviz_interface.launch + - layout: + layout: tiled + panes: + - waitForControl; sleep 5; ~/.i3/layout_manager.sh ./layout.json diff --git a/waypoint_flier/tmux/start.sh b/waypoint_flier/tmux/start.sh new file mode 100755 index 0000000..5bb2275 --- /dev/null +++ b/waypoint_flier/tmux/start.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +# Absolute path to this script. /home/user/bin/foo.sh +SCRIPT=$(readlink -f $0) +# Absolute path this script is in. /home/user/bin +SCRIPTPATH=`dirname $SCRIPT` +cd "$SCRIPTPATH" + +export TMUX_SESSION_NAME=simulation +export TMUX_SOCKET_NAME=mrs + +# start tmuxinator +tmuxinator start -p ./session.yml + +# if we are not in tmux +if [ -z $TMUX ]; then + + # just attach to the session + tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME + +# if we are in tmux +else + + # switch to the newly-started session + tmux detach-client -E "tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME" + +fi diff --git a/waypoint_flier_simple/CMakeLists.txt b/waypoint_flier_simple/CMakeLists.txt new file mode 100644 index 0000000..5e51d73 --- /dev/null +++ b/waypoint_flier_simple/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.15.0) +project(waypoint_flier_simple) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +set(CATKIN_DEPENDENCIES + cmake_modules + roscpp + nodelet + nav_msgs + mrs_msgs + geometry_msgs + ) + +set(LIBRARIES + WaypointFlierSimple + ) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_DEPENDENCIES} + ) + +catkin_package( + LIBRARIES ${LIBRARIES} + CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} + ) + +include_directories( + ${catkin_INCLUDE_DIRS} + ) + +# WaypointFlierSimple + +## Declare a C++ library +add_library(WaypointFlierSimple + src/waypoint_flier_simple.cpp + ) + +add_dependencies(WaypointFlierSimple + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + +## Specify libraries to link a library or executable target against +target_link_libraries(WaypointFlierSimple + ${catkin_LIBRARIES} + ) + +## -------------------------------------------------------------- +## | Install | +## -------------------------------------------------------------- + +install(TARGETS ${LIBRARIES} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +install(FILES nodelets.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) diff --git a/waypoint_flier_simple/README.md b/waypoint_flier_simple/README.md new file mode 100644 index 0000000..ccaf3a3 --- /dev/null +++ b/waypoint_flier_simple/README.md @@ -0,0 +1,23 @@ +# MRS ROS simple example + +This package was created as an example of how to write a very simple ROS nodelet. +You can test the program in simulation (see our [simulation tutorial](https://ctu-mrs.github.io/docs/simulation/howto.html)). + +## Functionality + +* Once activated, the nodelet will command an UAV to fly through random waypoints +* Service `start_waypoint_following` will activate the nodelet +* The area in which random waypoints are generated is configurable with a separate config file See [.yaml files](http://wiki.ros.org/rosparam) + +## Package structure + +See [ROS packages](http://wiki.ros.org/Packages) + +* `src` directory contains all source files +* `launch` directory contains `.launch` files which are used to parametrize the nodelet. Command-line arguments, as well as environment variables, can be loaded from the launch files, the nodelet can be put into the correct namespace (each UAV has its namespace to allow multi-robot applications), config files are loaded, and parameters passed to the nodelet. See [.launch files](http://wiki.ros.org/roslaunch/XML) +* `config` directory contains parameters in `.yaml` files. See [.yaml files](http://wiki.ros.org/rosparam) +* `package.xml` defines properties of the package, such as package name and dependencies. See [package.xml](http://wiki.ros.org/catkin/package.xml) + +## More complex example + +* To see a similar node with more functionality and features, see [example_ros_uav](https://github.com/ctu-mrs/example_ros_uav) diff --git a/waypoint_flier_simple/config/waypoint_flier.yaml b/waypoint_flier_simple/config/waypoint_flier.yaml new file mode 100644 index 0000000..fb497ab --- /dev/null +++ b/waypoint_flier_simple/config/waypoint_flier.yaml @@ -0,0 +1,3 @@ +max_x: 30 # [m] +max_y: 50 # [m] +max_z: 10 # [m] diff --git a/waypoint_flier_simple/launch/waypoint_flier_simple.launch b/waypoint_flier_simple/launch/waypoint_flier_simple.launch new file mode 100644 index 0000000..09b443c --- /dev/null +++ b/waypoint_flier_simple/launch/waypoint_flier_simple.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/waypoint_flier_simple/nodelets.xml b/waypoint_flier_simple/nodelets.xml new file mode 100644 index 0000000..3b83172 --- /dev/null +++ b/waypoint_flier_simple/nodelets.xml @@ -0,0 +1,5 @@ + + + WaypointFlierSimple nodelet + + diff --git a/waypoint_flier_simple/package.xml b/waypoint_flier_simple/package.xml new file mode 100644 index 0000000..2a77e11 --- /dev/null +++ b/waypoint_flier_simple/package.xml @@ -0,0 +1,29 @@ + + + + waypoint_flier_simple + 1.0.0 + waypoint_flier_simple + + Daniel Hert + Daniel Hert + + MIT + + catkin + + cmake_modules + roscpp + nodelet + geometry_msgs + nav_msgs + mrs_msgs + + + + + + + + + diff --git a/waypoint_flier_simple/src/waypoint_flier_simple.cpp b/waypoint_flier_simple/src/waypoint_flier_simple.cpp new file mode 100644 index 0000000..3e7201a --- /dev/null +++ b/waypoint_flier_simple/src/waypoint_flier_simple.cpp @@ -0,0 +1,238 @@ +/* includes //{ */ + +/* each ros package must have these */ +#include +#include +#include + +/* for storing information about the state of the uav (position, twist) + covariances */ +#include + +/* custom msgs of MRS group */ +#include + +/* for calling simple ros services */ +#include + +#include +//} + +namespace waypoint_flier_simple +{ + +/* class WaypointFlierSimple //{ */ + +class WaypointFlierSimple : public nodelet::Nodelet { + +public: + /* onInit() is called when nodelet is launched (similar to main() in regular node) */ + virtual void onInit(); + +private: + // | -------------------------- flags ------------------------- | + + /* is set to true when the nodelet is initialized, useful for rejecting callbacks that are called before the node is initialized */ + std::atomic is_initialized_ = false; + + /* by default, the nodelet is deactivated, it only starts publishing goals when activated */ + std::atomic active_ = false; + + /* by default, the nodelet is deactivated, it only starts publishing goals when activated */ + std::atomic have_odom_ = false; + + /* variables to store the coordinates of the current goal */ + double goal_x_ = 0.0; + double goal_y_ = 0.0; + double goal_z_ = 2.0; + + /* variables to store the maximum limit for the random waypoint generator */ + double max_x_; + double max_y_; + double max_z_; + + /* ROS messages which store the current reference and odometry */ + mrs_msgs::ReferenceStamped ref_; + nav_msgs::Odometry current_odom_; + + // | ---------------------- ROS subscribers --------------------- | + ros::Subscriber sub_odom_; + void callbackOdom(const nav_msgs::Odometry& msg); + + // | ---------------------- ROS publishers --------------------- | + ros::Publisher pub_reference_; + + // | ---------------------- ROS timers --------------------- | + + ros::Timer main_timer_; + void callbackMainTimer([[maybe_unused]] const ros::TimerEvent& te); + + // | ---------------------- ROS service servers --------------------- | + + bool callbackStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); + ros::ServiceServer srv_server_start_; + + // | ------------------ Additional functions ------------------ | + + double distance(const mrs_msgs::ReferenceStamped& waypoint, const nav_msgs::Odometry& odom); + + double getRandomDouble(double min, double max); +}; +//} + +/* onInit() //{ */ + +void WaypointFlierSimple::onInit() { + + /* obtain node handle */ + ros::NodeHandle nh = nodelet::Nodelet::getMTPrivateNodeHandle(); + + /* initialize random number generator */ + srand(time(NULL)); + + /* waits for the ROS to publish clock */ + ros::Time::waitForValid(); + + // | ------------------- load ros parameters ------------------ | + nh.param("max_x", max_x_, 10.0); + nh.param("max_y", max_y_, 10.0); + nh.param("max_z", max_z_, 5.0); + + // | -------- initialize a subscriber for UAV odometry -------- | + sub_odom_ = nh.subscribe("odom_in", 10, &WaypointFlierSimple::callbackOdom, this, ros::TransportHints().tcpNoDelay()); + + // | -------- initialize a publisher for UAV reference -------- | + pub_reference_ = nh.advertise("reference_out", 1); + + // | -- initialize the main timer - main loop of the nodelet -- | + main_timer_ = nh.createTimer(ros::Rate(10), &WaypointFlierSimple::callbackMainTimer, this); + + // | ---------------- initialize service server --------------- | + srv_server_start_ = nh.advertiseService("start", &WaypointFlierSimple::callbackStart, this); + is_initialized_ = true; +} + +//} + +// | ---------------------- msg callbacks --------------------- | + +/* callbackControlManagerDiag() //{ */ + + +void WaypointFlierSimple::callbackOdom(const nav_msgs::Odometry& msg) { + + /* do not continue if the nodelet is not initialized */ + if (!is_initialized_) { + return; + } + // | -------------- save the current UAV odometry ------------- | + current_odom_ = msg; + have_odom_ = true; +} + +//} + +// | --------------------- timer callbacks -------------------- | + +/* callbackMainTimer() //{ */ + +void WaypointFlierSimple::callbackMainTimer([[maybe_unused]] const ros::TimerEvent& te) { + + if (!active_) { + + ROS_INFO_THROTTLE(1.0, "[WaypointFlierSimple]: Waiting for activation"); + + } else { + + // calculate the distance to the current reference + double dist_to_ref = distance(ref_, current_odom_); + ROS_INFO_STREAM_THROTTLE(1.0, "[WaypointFlierSimple]: Distance to reference: " << dist_to_ref); + + // if the distance is less than 1 meter, generate a new reference + if (dist_to_ref < 1.0) { + + ROS_INFO_STREAM("[WaypointFlierSimple]: Goal reached!"); + + // generate new goal + goal_x_ = getRandomDouble(-max_x_, max_x_); + goal_y_ = getRandomDouble(-max_y_, max_y_); + goal_z_ = getRandomDouble(2.0, max_z_); + + ROS_INFO_STREAM("[WaypointFlierSimple]: New goal X: " << goal_x_ << " Y: " << goal_y_ << " Z: " << goal_z_); + } + + // fill out the ROS message and publish it + ref_.reference.position.x = goal_x_; + ref_.reference.position.y = goal_y_; + ref_.reference.position.z = goal_z_; + ref_.reference.heading = 0.0; + pub_reference_.publish(ref_); + } +} + +//} + +// | -------------------- service callbacks ------------------- | + +/* //{ callbackStart() */ + +bool WaypointFlierSimple::callbackStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) { + + // | ------------------- activation service ------------------- | + // only activates the main loop when the nodelet is initialized and receiving odometry + + if (!is_initialized_) { + + res.success = false; + res.message = "Waypoint flier not initialized!"; + ROS_WARN("[WaypointFlierSimple]: Cannot start waypoint following, nodelet is not initialized."); + return true; + } + + if (!have_odom_) { + + res.success = false; + res.message = "Waypoint flier is not receiving odometry!"; + ROS_WARN("[WaypointFlierSimple]: Cannot start, nodelet is not receiving odometry!"); + return true; + } + + active_ = true; + + ROS_INFO("[WaypointFlierSimple]: Starting waypoint following."); + ROS_INFO_STREAM("[WaypointFlierSimple]: Goal X: " << goal_x_ << " Y: " << goal_y_ << " Z: " << goal_z_); + + res.success = true; + res.message = "Starting waypoint following."; + + return true; +} + +//} + +// | -------------------- support functions ------------------- | + +/* distance() //{ */ + +double WaypointFlierSimple::distance(const mrs_msgs::ReferenceStamped& waypoint, const nav_msgs::Odometry& odom) { + + // | ------------- distance between two 3D points ------------- | + + return sqrt((pow(waypoint.reference.position.x - odom.pose.pose.position.x, 2)) + (pow(waypoint.reference.position.y - odom.pose.pose.position.y, 2)) + + (pow(waypoint.reference.position.z - odom.pose.pose.position.z, 2))); +} + +double WaypointFlierSimple::getRandomDouble(double min, double max) { + + // | --------- random double in the min and max bounds -------- | + + float r = (float)rand() / (float)RAND_MAX; + return min + r * (max - min); +} + +//} + +} // namespace waypoint_flier_simple + +/* every nodelet must include macros which export the class as a nodelet plugin */ +#include +PLUGINLIB_EXPORT_CLASS(waypoint_flier_simple::WaypointFlierSimple, nodelet::Nodelet); diff --git a/waypoint_flier_simple/tmux/config/automatic_start.yaml b/waypoint_flier_simple/tmux/config/automatic_start.yaml new file mode 100644 index 0000000..d351868 --- /dev/null +++ b/waypoint_flier_simple/tmux/config/automatic_start.yaml @@ -0,0 +1,5 @@ +# A timeout between the takeoff being triggered and the UAV actually taking off +# while the timeout is counting down, the takeoff can be aborted by switching off +# the offboard mode. +# default = 5 sec +safety_timeout: 1.0 # [s] diff --git a/waypoint_flier_simple/tmux/config/custom_config.yaml b/waypoint_flier_simple/tmux/config/custom_config.yaml new file mode 100644 index 0000000..c1f518c --- /dev/null +++ b/waypoint_flier_simple/tmux/config/custom_config.yaml @@ -0,0 +1,39 @@ +# GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: +## -------------------------------------------------------------- +## | rosrun mrs_uav_core get_public_params.py # +## -------------------------------------------------------------- + +mrs_uav_managers: + + estimation_manager: + + # loaded state estimator plugins + state_estimators: [ + "gps_garmin", + "gps_baro", + ] + + initial_state_estimator: "gps_baro" # will be used as the first state estimator + agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) + + uav_manager: + + takeoff: + + during_takeoff: + controller: "MpcController" + tracker: "LandoffTracker" + + after_takeoff: + controller: "Se3Controller" + tracker: "MpcTracker" + + midair_activation: + + during_activation: + controller: "MidairActivationController" + tracker: "MidairActivationTracker" + + after_activation: + controller: "Se3Controller" + tracker: "MpcTracker" diff --git a/waypoint_flier_simple/tmux/config/hw_api.yaml b/waypoint_flier_simple/tmux/config/hw_api.yaml new file mode 100644 index 0000000..30a6919 --- /dev/null +++ b/waypoint_flier_simple/tmux/config/hw_api.yaml @@ -0,0 +1,28 @@ +input_mode: + actuators: false + control_group: false + attitude_rate: true + attitude: true + acceleration_hdg_rate: false + acceleration_hdg: false + velocity_hdg_rate: false + velocity_hdg: false + position: false + + feedforward: true + +outputs: + distance_sensor: true + gnss: true + rtk: true + imu: true + altitude: true + magnetometer_heading: true + rc_channels: true + battery_state: true + position: true + orientation: true + velocity: true + angular_velocity: true + odometry: true + ground_truth: true diff --git a/waypoint_flier_simple/tmux/config/network_config.yaml b/waypoint_flier_simple/tmux/config/network_config.yaml new file mode 100644 index 0000000..88d3e9f --- /dev/null +++ b/waypoint_flier_simple/tmux/config/network_config.yaml @@ -0,0 +1,20 @@ +# 1. This list is used by NimbroNetwork for mutual communication of the UAVs +# The names of the robots have to match hostnames described in /etc/hosts. +# +# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. +# The names should match the true "UAV_NAMES" (the topic prefixes). +# +# network_config:=~/config/network_config.yaml +# +# to the core.launch and nimbro.launch. + +network: + + robot_names: [ + + uav1, + uav2, + uav3, + uav4, + + ] diff --git a/waypoint_flier_simple/tmux/config/simulator.yaml b/waypoint_flier_simple/tmux/config/simulator.yaml new file mode 100644 index 0000000..7f7346a --- /dev/null +++ b/waypoint_flier_simple/tmux/config/simulator.yaml @@ -0,0 +1,20 @@ +simulation_rate: 250 +clock_rate: 250 +realtime_factor: 1.0 + +iterate_without_input: true + +individual_takeoff_platform: + enabled: false + +uav_names: [ + "uav1", +] + +uav1: + type: "x500" + spawn: + x: 10.0 + y: 15.0 + z: 2.0 + heading: 0 diff --git a/waypoint_flier_simple/tmux/config/world_config.yaml b/waypoint_flier_simple/tmux/config/world_config.yaml new file mode 100644 index 0000000..9b55067 --- /dev/null +++ b/waypoint_flier_simple/tmux/config/world_config.yaml @@ -0,0 +1,34 @@ +world_origin: + + units: "LATLON" # {"UTM, "LATLON"} + + origin_x: 47.397743 + origin_y: 8.545594 + +safety_area: + + enabled: true + + horizontal: + + # the frame of reference in which the points are expressed + frame_name: "world_origin" + + # polygon + # + # x, y [m] for any frame_name except latlon_origin + # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" + points: [ + -50, -50, + 50, -50, + 50, 50, + -50, 50, + ] + + vertical: + + # the frame of reference in which the max&min z is expressed + frame_name: "world_origin" + + max_z: 15.0 + min_z: 0.5 diff --git a/waypoint_flier_simple/tmux/kill.sh b/waypoint_flier_simple/tmux/kill.sh new file mode 100755 index 0000000..28d50b0 --- /dev/null +++ b/waypoint_flier_simple/tmux/kill.sh @@ -0,0 +1,14 @@ +#!/bin/bash + +# Absolute path to this script. /home/user/bin/foo.sh +SCRIPT=$(readlink -f $0) +# Absolute path this script is in. /home/user/bin +SCRIPTPATH=`dirname $SCRIPT` +cd "$SCRIPTPATH" + +export TMUX_SESSION_NAME=simulation +export TMUX_SOCKET_NAME=mrs + +# just attach to the session +tmux -L $TMUX_SOCKET_NAME split-window -t $TMUX_SESSION_NAME +tmux -L $TMUX_SOCKET_NAME send-keys -t $TMUX_SESSION_NAME "sleep 1; tmux list-panes -s -F \"#{pane_pid} #{pane_current_command}\" | grep -v tmux | cut -d\" \" -f1 | while read in; do killp \$in; done; exit" ENTER diff --git a/waypoint_flier_simple/tmux/layout.json b/waypoint_flier_simple/tmux/layout.json new file mode 100644 index 0000000..5664ebd --- /dev/null +++ b/waypoint_flier_simple/tmux/layout.json @@ -0,0 +1,50 @@ +[ +{ + "border": "normal", + "floating": "auto_off", + "fullscreen_mode": 0, + "layout": "splitv", + "percent": 0.285714285714286, + "type": "con", + "nodes": [ + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 1030, + "width": 936, + "x": 4806, + "y": 32 + }, + "name": "rviz.rviz* - RViz", + "percent": 0.5, + "swallows": [ + { + "instance": "^rviz$" + } + ], + "type": "con" + }, + { + "border": "pixel", + "current_border_width": 3, + "floating": "auto_off", + "geometry": { + "height": 460, + "width": 724, + "x": 0, + "y": 0 + }, + "name": "./start.sh", + "percent": 0.5, + "swallows": [ + { + "instance": "^urxvt$" + } + ], + "type": "con" + } + ] +} +] diff --git a/waypoint_flier_simple/tmux/session.yml b/waypoint_flier_simple/tmux/session.yml new file mode 100644 index 0000000..f5a36ef --- /dev/null +++ b/waypoint_flier_simple/tmux/session.yml @@ -0,0 +1,54 @@ +# do not modify these +root: ./ +name: simulation +socket_name: mrs +attach: false +tmux_options: -f /etc/ctu-mrs/tmux.conf +# you can modify these +pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500 +startup_window: waypointflier +windows: + - roscore: + layout: tiled + panes: + - roscore + - simulator: + layout: tiled + panes: + - waitForRos; roslaunch mrs_multirotor_simulator multirotor_simulator.launch custom_config:=./config/simulator.yaml + - hw_api: + layout: tiled + panes: + - waitForTime; roslaunch mrs_multirotor_simulator hw_api.launch custom_config:=./config/hw_api.yaml + - takeoff: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_autostart automatic_start.launch custom_config:=./config/automatic_start.yaml + - waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard + - status: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_status status.launch + - control: + layout: tiled + panes: + - waitForHw; roslaunch mrs_uav_core core.launch + platform_config:=`rospack find mrs_multirotor_simulator`/config/mrs_uav_system/$UAV_TYPE.yaml + custom_config:=./config/custom_config.yaml + world_config:=./config/world_config.yaml + network_config:=./config/network_config.yaml + - waypointflier: + layout: tiled + panes: + - waitForControl; roslaunch waypoint_flier_simple waypoint_flier_simple.launch + - 'history -s rosservice call /$UAV_NAME/waypoint_flier_simple/start' + - rviz: + layout: tiled + panes: + - waitForControl; roslaunch mrs_uav_core rviz.launch + - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch + - waitForControl; waitForControl; roslaunch mrs_rviz_plugins rviz_interface.launch + - layout: + layout: tiled + panes: + - waitForControl; sleep 5; ~/.i3/layout_manager.sh ./layout.json diff --git a/waypoint_flier_simple/tmux/start.sh b/waypoint_flier_simple/tmux/start.sh new file mode 100755 index 0000000..5bb2275 --- /dev/null +++ b/waypoint_flier_simple/tmux/start.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +# Absolute path to this script. /home/user/bin/foo.sh +SCRIPT=$(readlink -f $0) +# Absolute path this script is in. /home/user/bin +SCRIPTPATH=`dirname $SCRIPT` +cd "$SCRIPTPATH" + +export TMUX_SESSION_NAME=simulation +export TMUX_SOCKET_NAME=mrs + +# start tmuxinator +tmuxinator start -p ./session.yml + +# if we are not in tmux +if [ -z $TMUX ]; then + + # just attach to the session + tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME + +# if we are in tmux +else + + # switch to the newly-started session + tmux detach-client -E "tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME" + +fi