├── .github └── workflows │ └── ros_build_test.yml ├── .gitignore ├── LICENSE ├── README.md ├── cpp ├── README.md ├── controller_plugin │ ├── CMakeLists.txt │ ├── README.md │ ├── cfg │ │ └── example_controller.cfg │ ├── config │ │ └── example_controller.yaml │ ├── include │ │ └── pid.hpp │ ├── package.xml │ ├── plugins.xml │ ├── src │ │ └── example_controller.cpp │ └── tmux │ │ ├── config │ │ ├── custom_config.yaml │ │ ├── hw_api.yaml │ │ ├── network_config.yaml │ │ ├── simulator.yaml │ │ └── world_config.yaml │ │ ├── layout.json │ │ ├── session.yml │ │ └── start.sh ├── pluginlib_example │ ├── README.md │ ├── example_plugin_manager │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ ├── example_plugin_manager.yaml │ │ │ └── plugins.yaml │ │ ├── include │ │ │ └── example_plugin_manager │ │ │ │ ├── common_handlers.h │ │ │ │ └── plugin_interface.h │ │ ├── launch │ │ │ └── example_plugin_manager.launch │ │ ├── nodelets.xml │ │ ├── package.xml │ │ └── src │ │ │ └── example_plugin_manager.cpp │ └── example_plugins │ │ ├── CMakeLists.txt │ │ ├── config │ │ └── example_plugin.yaml │ │ ├── package.xml │ │ ├── plugins.xml │ │ └── src │ │ └── example_plugin.cpp ├── tracker_plugin │ ├── CMakeLists.txt │ ├── README.md │ ├── cfg │ │ └── example_tracker.cfg │ ├── config │ │ └── example_tracker.yaml │ ├── include │ │ └── .gitkeep │ ├── package.xml │ ├── plugins.xml │ ├── src │ │ └── example_tracker.cpp │ └── tmux │ │ ├── config │ │ ├── custom_config.yaml │ │ ├── hw_api.yaml │ │ ├── network_config.yaml │ │ ├── simulator.yaml │ │ └── world_config.yaml │ │ ├── layout.json │ │ ├── session.yml │ │ └── start.sh ├── waypoint_flier │ ├── CMakeLists.txt │ ├── README.md │ ├── config │ │ ├── dynparam.cfg │ │ └── example_waypoint_flier.yaml │ ├── launch │ │ └── example_waypoint_flier.launch │ ├── nodelets.xml │ ├── package.xml │ ├── src │ │ └── example_waypoint_flier.cpp │ └── tmux │ │ ├── config │ │ ├── automatic_start.yaml │ │ ├── custom_config.yaml │ │ ├── hw_api.yaml │ │ ├── network_config.yaml │ │ ├── simulator.yaml │ │ └── world_config.yaml │ │ ├── kill.sh │ │ ├── layout.json │ │ ├── session.yml │ │ └── start.sh └── waypoint_flier_simple │ ├── CMakeLists.txt │ ├── README.md │ ├── config │ └── waypoint_flier.yaml │ ├── launch │ └── waypoint_flier_simple.launch │ ├── nodelets.xml │ ├── package.xml │ ├── src │ └── waypoint_flier_simple.cpp │ └── tmux │ ├── config │ ├── automatic_start.yaml │ ├── custom_config.yaml │ ├── hw_api.yaml │ ├── network_config.yaml │ ├── simulator.yaml │ └── world_config.yaml │ ├── kill.sh │ ├── layout.json │ ├── session.yml │ └── start.sh ├── python ├── README.md └── sweeping_generator │ ├── CMakeLists.txt │ ├── README.md │ ├── config │ └── sweeping_generator.yaml │ ├── launch │ └── sweeping_generator.launch │ ├── package.xml │ ├── scripts │ └── sweeping_generator.py │ └── tmux │ ├── config │ ├── automatic_start.yaml │ ├── custom_config.yaml │ ├── hw_api.yaml │ ├── network_config.yaml │ ├── simulator.yaml │ └── world_config.yaml │ ├── kill.sh │ ├── layout.json │ ├── session.yml │ └── start.sh └── repurpose_package.sh /.github/workflows/ros_build_test.yml: -------------------------------------------------------------------------------- 1 | name: ros_build_test 2 | 3 | on: 4 | 5 | push: 6 | branches: [ master ] 7 | 8 | paths-ignore: 9 | - '**/README.md' 10 | 11 | schedule: 12 | - cron: '0 8 * * *' # every day at 10am UTC+2 13 | 14 | pull_request: 15 | branches: [ master ] 16 | 17 | workflow_dispatch: 18 | 19 | concurrency: 20 | group: ${{ github.ref }} 21 | cancel-in-progress: true 22 | 23 | jobs: 24 | 25 | build: 26 | uses: ctu-mrs/ci_scripts/.github/workflows/ros_build_test.yml@master 27 | secrets: 28 | PUSH_TOKEN: ${{ secrets.PUSH_TOKEN }} 29 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | *.swo 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Multi-robot Systems (MRS) group at Czech Technical University in Prague 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MRS Core Examples 2 | 3 | This repository includes Core ROS examples for the [MRS UAV System](https://github.com/ctu-mrs/mrs_uav_system). 4 | 5 | ## Packages 6 | 7 | ## C++ 8 | 9 | * [waypoint_flier_simple](./cpp/waypoint_flier_simple) - Minimalistic C++ Nodelet with "_vanilla_" ROS features 10 | * [waypoint_flier](./cpp/waypoint_flier) - Full C++ Nodelet with "_MRS_" libraries and wrappers 11 | * [controller_plugin](./cpp/controller_plugin) - Example of Controller plugin for the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers) 12 | * [tracker_plugin](./cpp/tracker_plugin) - Example of Tracker plugin for the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers) 13 | * [pluginlib_example](./cpp/pluginlib_example) - Example of ROS Pluginlib, similar to how it is used in the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers) 14 | 15 | ## Python 16 | 17 | * [sweeping_generator](./python/sweeping_generator) - Minimalistic Python Example that generates sweeping path for the UAV 18 | 19 | # Disclaimer 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 25 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 26 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 27 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | -------------------------------------------------------------------------------- /cpp/README.md: -------------------------------------------------------------------------------- 1 | # MRS C++ Examples 2 | -------------------------------------------------------------------------------- /cpp/controller_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1.2) 2 | project(example_controller_plugin) 3 | 4 | set(CATKIN_DEPENDENCIES 5 | cmake_modules 6 | roscpp 7 | rospy 8 | pluginlib 9 | geometry_msgs 10 | dynamic_reconfigure 11 | mrs_uav_managers 12 | mrs_msgs 13 | mrs_lib 14 | ) 15 | 16 | find_package(catkin REQUIRED COMPONENTS 17 | ${CATKIN_DEPENDENCIES} 18 | ) 19 | 20 | generate_dynamic_reconfigure_options( 21 | cfg/example_controller.cfg 22 | ) 23 | 24 | set(CMAKE_CXX_STANDARD 17) 25 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 26 | 27 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 28 | 29 | find_package(Eigen3 REQUIRED) 30 | set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) 31 | set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) 32 | 33 | set(LIBRARIES 34 | ExampleController 35 | ) 36 | 37 | catkin_package( 38 | INCLUDE_DIRS include 39 | CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} 40 | LIBRARIES ${LIBRARIES} 41 | DEPENDS Eigen 42 | ) 43 | 44 | include_directories( 45 | include 46 | ${catkin_INCLUDE_DIRS} 47 | ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake 48 | ) 49 | 50 | # Example Controller 51 | 52 | add_library(ExampleController 53 | src/example_controller.cpp 54 | ) 55 | 56 | add_dependencies(ExampleController 57 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 58 | ${catkin_EXPORTED_TARGETS} 59 | ${PROJECT_NAME}_gencfg 60 | ) 61 | 62 | target_link_libraries(ExampleController 63 | ${catkin_LIBRARIES} 64 | ) 65 | 66 | ## -------------------------------------------------------------- 67 | ## | Install | 68 | ## -------------------------------------------------------------- 69 | 70 | install(TARGETS ${LIBRARIES} 71 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 72 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 73 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 74 | ) 75 | 76 | install(DIRECTORY config 77 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 78 | ) 79 | 80 | install(FILES plugins.xml 81 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 82 | ) 83 | -------------------------------------------------------------------------------- /cpp/controller_plugin/README.md: -------------------------------------------------------------------------------- 1 | # Example Controller Plugin 2 | 3 | Example controller plugin for the Control Manager. 4 | 5 | ## How to start 6 | 7 | ```bash 8 | ./tmux/start.sh 9 | ``` 10 | -------------------------------------------------------------------------------- /cpp/controller_plugin/cfg/example_controller.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "example_controller_plugin" 3 | 4 | import roslib; 5 | roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | orientation = gen.add_group("Desired orientation"); 12 | 13 | orientation.add("roll", double_t, 0, "Desired roll", 0.0, -3.14, 3.14) 14 | orientation.add("pitch", double_t, 0, "Desired pitch", 0.0, -3.14, 3.14) 15 | orientation.add("yaw", double_t, 0, "Desired pitch", 0.0, -3.14, 3.14) 16 | 17 | force = gen.add_group("Desired force relative to hover"); 18 | 19 | force.add("force", double_t, 0, "Desired force", 0.0, -10.0, 10.0) 20 | 21 | exit(gen.generate(PACKAGE, "ExampleController", "example_controller")) 22 | -------------------------------------------------------------------------------- /cpp/controller_plugin/config/example_controller.yaml: -------------------------------------------------------------------------------- 1 | desired_roll: 0.03 # [rad] 2 | desired_pitch: 0.02 # [rad] 3 | desired_yaw: 0.1 # [rad] 4 | desired_thrust_force: 0.2 # [N], additional to gravity compensation 5 | -------------------------------------------------------------------------------- /cpp/controller_plugin/include/pid.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PID_H 2 | #define PID_H 3 | 4 | #include 5 | 6 | namespace example_controller_plugin 7 | { 8 | 9 | class PIDController { 10 | 11 | private: 12 | // | ----------------------- parameters ----------------------- | 13 | 14 | // gains 15 | double _kp_ = 0; // proportional gain 16 | double _kd_ = 0; // derivative gain 17 | double _ki_ = 0; // integral gain 18 | 19 | // we remember the last control error, to calculate the difference 20 | double last_error_ = 0; 21 | double integral_ = 0; 22 | 23 | double saturation = -1; 24 | double antiwindup = -1; 25 | 26 | public: 27 | PIDController(); 28 | 29 | void setParams(const double &kp, const double &kd, const double &ki, const double &saturation, const double &antiwindup); 30 | 31 | void setSaturation(const double saturation = -1); 32 | 33 | void reset(void); 34 | 35 | double update(const double &error, const double &dt); 36 | }; 37 | 38 | // -------------------------------------------------------------- 39 | // | implementation | 40 | // -------------------------------------------------------------- 41 | 42 | PIDController::PIDController() { 43 | 44 | this->reset(); 45 | } 46 | 47 | void PIDController::setParams(const double &kp, const double &kd, const double &ki, const double &saturation, const double &antiwindup) { 48 | 49 | this->_kp_ = kp; 50 | this->_kd_ = kd; 51 | this->_ki_ = ki; 52 | this->saturation = saturation; 53 | this->antiwindup = antiwindup; 54 | } 55 | 56 | void PIDController::setSaturation(const double saturation) { 57 | 58 | this->saturation = saturation; 59 | } 60 | 61 | void PIDController::reset(void) { 62 | 63 | this->last_error_ = 0; 64 | this->integral_ = 0; 65 | } 66 | 67 | double PIDController::update(const double &error, const double &dt) { 68 | 69 | // calculate the control error difference 70 | double difference = (error - last_error_) / dt; 71 | last_error_ = error; 72 | 73 | double p_component = _kp_ * error; // proportional feedback 74 | double d_component = _kd_ * difference; // derivative feedback 75 | double i_component = _ki_ * integral_; // derivative feedback 76 | 77 | double sum = p_component + d_component + i_component; 78 | 79 | if (saturation > 0) { 80 | if (sum >= saturation) { 81 | sum = saturation; 82 | } else if (sum <= -saturation) { 83 | sum = -saturation; 84 | } 85 | } 86 | 87 | if (antiwindup > 0) { 88 | if (std::abs(sum) < antiwindup) { 89 | // add to the integral 90 | integral_ += error * dt; 91 | } 92 | } 93 | 94 | // return the summ of the components 95 | return sum; 96 | } 97 | 98 | } // namespace example_controller_plugin 99 | 100 | #endif // PID_H 101 | -------------------------------------------------------------------------------- /cpp/controller_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | example_controller_plugin 5 | 1.0.0 6 | The example_controller_plugin package 7 | 8 | Tomas Baca 9 | Tomas Baca 10 | 11 | BSD 3-Clause 12 | 13 | catkin 14 | 15 | cmake_modules 16 | roscpp 17 | rospy 18 | pluginlib 19 | geometry_msgs 20 | dynamic_reconfigure 21 | mrs_uav_managers 22 | mrs_msgs 23 | mrs_lib 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /cpp/controller_plugin/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is the Example Controller. 4 | 5 | 6 | -------------------------------------------------------------------------------- /cpp/controller_plugin/src/example_controller.cpp: -------------------------------------------------------------------------------- 1 | /* includes //{ */ 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | //} 18 | 19 | namespace example_controller_plugin 20 | { 21 | 22 | namespace example_controller 23 | { 24 | 25 | /* //{ class ExampleController */ 26 | 27 | class ExampleController : public mrs_uav_managers::Controller { 28 | 29 | public: 30 | bool initialize(const ros::NodeHandle& nh, std::shared_ptr common_handlers, 31 | std::shared_ptr private_handlers); 32 | 33 | bool activate(const ControlOutput& last_control_output); 34 | 35 | void deactivate(void); 36 | 37 | void updateInactive(const mrs_msgs::UavState& uav_state, const std::optional& tracker_command); 38 | 39 | ControlOutput updateActive(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command); 40 | 41 | const mrs_msgs::ControllerStatus getStatus(); 42 | 43 | void switchOdometrySource(const mrs_msgs::UavState& new_uav_state); 44 | 45 | void resetDisturbanceEstimators(void); 46 | 47 | const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& cmd); 48 | 49 | private: 50 | ros::NodeHandle nh_; 51 | 52 | bool is_initialized_ = false; 53 | bool is_active_ = false; 54 | 55 | std::shared_ptr common_handlers_; 56 | std::shared_ptr private_handlers_; 57 | 58 | // | ------------------------ uav state ----------------------- | 59 | 60 | mrs_msgs::UavState uav_state_; 61 | std::mutex mutex_uav_state_; 62 | 63 | // | --------------- dynamic reconfigure server --------------- | 64 | 65 | boost::recursive_mutex mutex_drs_; 66 | typedef example_controller_plugin::example_controllerConfig DrsConfig_t; 67 | typedef dynamic_reconfigure::Server Drs_t; 68 | boost::shared_ptr drs_; 69 | void callbackDrs(example_controller_plugin::example_controllerConfig& config, uint32_t level); 70 | DrsConfig_t drs_params_; 71 | std::mutex mutex_drs_params_; 72 | 73 | // | ----------------------- constraints ---------------------- | 74 | 75 | mrs_msgs::DynamicsConstraints constraints_; 76 | std::mutex mutex_constraints_; 77 | 78 | // | --------- throttle generation and mass estimation -------- | 79 | 80 | double _uav_mass_; 81 | 82 | // | ------------------ activation and output ----------------- | 83 | 84 | ControlOutput last_control_output_; 85 | ControlOutput activation_control_output_; 86 | 87 | ros::Time last_update_time_; 88 | std::atomic first_iteration_ = true; 89 | }; 90 | 91 | //} 92 | 93 | // -------------------------------------------------------------- 94 | // | controller's interface | 95 | // -------------------------------------------------------------- 96 | 97 | /* //{ initialize() */ 98 | 99 | bool ExampleController::initialize(const ros::NodeHandle& nh, std::shared_ptr common_handlers, 100 | std::shared_ptr private_handlers) { 101 | 102 | nh_ = nh; 103 | 104 | common_handlers_ = common_handlers; 105 | private_handlers_ = private_handlers; 106 | 107 | _uav_mass_ = common_handlers->getMass(); 108 | 109 | last_update_time_ = ros::Time(0); 110 | 111 | ros::Time::waitForValid(); 112 | 113 | // | ------------------- loading parameters ------------------- | 114 | 115 | bool success = true; 116 | 117 | // FYI 118 | // This method will load the file using `rosparam get` 119 | // Pros: you can the full power of the official param loading 120 | // Cons: it is slower 121 | // 122 | // Alternatives: 123 | // You can load the file directly into the ParamLoader as shown below. 124 | 125 | success *= private_handlers->loadConfigFile(ros::package::getPath("example_controller_plugin") + "/config/example_controller.yaml"); 126 | 127 | if (!success) { 128 | return false; 129 | } 130 | 131 | mrs_lib::ParamLoader param_loader(nh_, "ExampleController"); 132 | 133 | // This is the alternaive way of loading the config file. 134 | // 135 | // Files loaded using this method are prioritized over ROS params. 136 | // 137 | // param_loader.addYamlFile(ros::package::getPath("example_tracker_plugin") + "/config/example_tracker.yaml"); 138 | 139 | param_loader.loadParam("desired_roll", drs_params_.roll); 140 | param_loader.loadParam("desired_pitch", drs_params_.pitch); 141 | param_loader.loadParam("desired_yaw", drs_params_.yaw); 142 | param_loader.loadParam("desired_thrust_force", drs_params_.force); 143 | 144 | // | ------------------ finish loading params ----------------- | 145 | 146 | if (!param_loader.loadedSuccessfully()) { 147 | ROS_ERROR("[ExampleController]: could not load all parameters!"); 148 | return false; 149 | } 150 | 151 | // | --------------- dynamic reconfigure server --------------- | 152 | 153 | drs_.reset(new Drs_t(mutex_drs_, nh_)); 154 | drs_->updateConfig(drs_params_); 155 | Drs_t::CallbackType f = boost::bind(&ExampleController::callbackDrs, this, _1, _2); 156 | drs_->setCallback(f); 157 | 158 | // | ----------------------- finish init ---------------------- | 159 | 160 | ROS_INFO("[ExampleController]: initialized"); 161 | 162 | is_initialized_ = true; 163 | 164 | return true; 165 | } 166 | 167 | //} 168 | 169 | /* //{ activate() */ 170 | 171 | bool ExampleController::activate(const ControlOutput& last_control_output) { 172 | 173 | activation_control_output_ = last_control_output; 174 | 175 | first_iteration_ = true; 176 | 177 | is_active_ = true; 178 | 179 | ROS_INFO("[ExampleController]: activated"); 180 | 181 | return true; 182 | } 183 | 184 | //} 185 | 186 | /* //{ deactivate() */ 187 | 188 | void ExampleController::deactivate(void) { 189 | 190 | is_active_ = false; 191 | first_iteration_ = false; 192 | 193 | ROS_INFO("[ExampleController]: deactivated"); 194 | } 195 | 196 | //} 197 | 198 | /* updateInactive() //{ */ 199 | 200 | void ExampleController::updateInactive(const mrs_msgs::UavState& uav_state, [[maybe_unused]] const std::optional& tracker_command) { 201 | 202 | mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_); 203 | 204 | last_update_time_ = uav_state.header.stamp; 205 | 206 | first_iteration_ = false; 207 | } 208 | 209 | //} 210 | 211 | /* //{ updateActive() */ 212 | 213 | ExampleController::ControlOutput ExampleController::updateActive(const mrs_msgs::UavState& uav_state, const mrs_msgs::TrackerCommand& tracker_command) { 214 | 215 | auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_); 216 | 217 | mrs_lib::set_mutexed(mutex_uav_state_, uav_state, uav_state_); 218 | 219 | // clear all the optional parts of the result 220 | last_control_output_.desired_heading_rate = {}; 221 | last_control_output_.desired_orientation = {}; 222 | last_control_output_.desired_unbiased_acceleration = {}; 223 | last_control_output_.control_output = {}; 224 | 225 | if (!is_active_) { 226 | return last_control_output_; 227 | } 228 | 229 | // | ---------- calculate dt from the last iteration ---------- | 230 | 231 | double dt; 232 | 233 | if (first_iteration_) { 234 | dt = 0.01; 235 | first_iteration_ = false; 236 | } else { 237 | dt = (uav_state.header.stamp - last_update_time_).toSec(); 238 | } 239 | 240 | last_update_time_ = uav_state.header.stamp; 241 | 242 | if (fabs(dt) < 0.001) { 243 | 244 | ROS_DEBUG("[ExampleController]: the last odometry message came too close (%.2f s)!", dt); 245 | dt = 0.01; 246 | } 247 | 248 | // | -------- check for the available output modalities ------- | 249 | 250 | // you can decide what to return, but it needs to be available 251 | if (common_handlers_->control_output_modalities.attitude) { 252 | ROS_INFO_THROTTLE(1.0, "[ExampleController]: desired attitude output modality is available"); 253 | } 254 | 255 | // | ---------- extract the detailed model parameters --------- | 256 | 257 | if (common_handlers_->detailed_model_params) { 258 | 259 | mrs_uav_managers::control_manager::DetailedModelParams_t detailed_model_params = common_handlers_->detailed_model_params.value(); 260 | 261 | ROS_INFO_STREAM_THROTTLE(1.0, "[ExampleController]: UAV inertia is: " << detailed_model_params.inertia); 262 | } 263 | 264 | // | -------------- prepare the control reference ------------- | 265 | 266 | geometry_msgs::PoseStamped position_reference; 267 | 268 | position_reference.header = tracker_command.header; 269 | position_reference.pose.position = tracker_command.position; 270 | position_reference.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0).setHeading(tracker_command.heading); 271 | 272 | 273 | // | ---------------- prepare the control output --------------- | 274 | 275 | mrs_msgs::HwApiAttitudeCmd attitude_cmd; 276 | 277 | attitude_cmd.orientation = mrs_lib::AttitudeConverter(drs_params.roll, drs_params.pitch, drs_params.yaw); 278 | attitude_cmd.throttle = mrs_lib::quadratic_throttle_model::forceToThrottle(common_handlers_->throttle_model, 279 | common_handlers_->getMass() * common_handlers_->g + drs_params.force); 280 | 281 | 282 | // | ----------------- set the control output ----------------- | 283 | 284 | last_control_output_.control_output = attitude_cmd; 285 | 286 | // | --------------- fill in the optional parts --------------- | 287 | 288 | //// it is recommended to fill the optinal parts if you know them 289 | 290 | /// this is used for: 291 | // * plotting the orientation in the control_refence topic (optional) 292 | // * checking for attitude control error 293 | last_control_output_.desired_orientation = mrs_lib::AttitudeConverter(drs_params.roll, drs_params.pitch, drs_params.yaw); 294 | 295 | /// IMPORANT 296 | // 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. 297 | last_control_output_.desired_unbiased_acceleration = Eigen::Vector3d(0, 0, 0); 298 | last_control_output_.desired_heading_rate = 0; 299 | 300 | // | ----------------- fill in the diagnostics ---------------- | 301 | 302 | last_control_output_.diagnostics.controller = "ExampleController"; 303 | 304 | return last_control_output_; 305 | } 306 | 307 | //} 308 | 309 | /* //{ getStatus() */ 310 | 311 | const mrs_msgs::ControllerStatus ExampleController::getStatus() { 312 | 313 | mrs_msgs::ControllerStatus controller_status; 314 | 315 | controller_status.active = is_active_; 316 | 317 | return controller_status; 318 | } 319 | 320 | //} 321 | 322 | /* switchOdometrySource() //{ */ 323 | 324 | void ExampleController::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState& new_uav_state) { 325 | } 326 | 327 | //} 328 | 329 | /* resetDisturbanceEstimators() //{ */ 330 | 331 | void ExampleController::resetDisturbanceEstimators(void) { 332 | } 333 | 334 | //} 335 | 336 | /* setConstraints() //{ */ 337 | 338 | const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr ExampleController::setConstraints([ 339 | [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr& constraints) { 340 | 341 | if (!is_initialized_) { 342 | return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse()); 343 | } 344 | 345 | mrs_lib::set_mutexed(mutex_constraints_, constraints->constraints, constraints_); 346 | 347 | ROS_INFO("[ExampleController]: updating constraints"); 348 | 349 | mrs_msgs::DynamicsConstraintsSrvResponse res; 350 | res.success = true; 351 | res.message = "constraints updated"; 352 | 353 | return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res)); 354 | } 355 | 356 | //} 357 | 358 | // -------------------------------------------------------------- 359 | // | callbacks | 360 | // -------------------------------------------------------------- 361 | 362 | /* //{ callbackDrs() */ 363 | 364 | void ExampleController::callbackDrs(example_controller_plugin::example_controllerConfig& config, [[maybe_unused]] uint32_t level) { 365 | 366 | mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_); 367 | 368 | ROS_INFO("[ExampleController]: dynamic reconfigure params updated"); 369 | } 370 | 371 | //} 372 | 373 | } // namespace example_controller 374 | 375 | } // namespace example_controller_plugin 376 | 377 | #include 378 | PLUGINLIB_EXPORT_CLASS(example_controller_plugin::example_controller::ExampleController, mrs_uav_managers::Controller) 379 | -------------------------------------------------------------------------------- /cpp/controller_plugin/tmux/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | estimation_manager: 4 | 5 | # loaded state estimator plugins 6 | state_estimators: [ 7 | "gps_baro", 8 | "ground_truth", 9 | ] 10 | 11 | initial_state_estimator: "gps_baro" # will be used as the first state estimator 12 | agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) 13 | 14 | control_manager: 15 | 16 | ExampleController: 17 | address: "example_controller_plugin/ExampleController" 18 | namespace: "example_controller" 19 | eland_threshold: 20.0 # [m], position error triggering eland 20 | failsafe_threshold: 30.0 # [m], position error triggering failsafe land 21 | odometry_innovation_threshold: 1.5 # [m], position odometry innovation threshold 22 | human_switchable: true 23 | 24 | # which outputs the controller can provide 25 | outputs: 26 | actuators: false 27 | control_group: false 28 | attitude_rate: false 29 | attitude: true 30 | acceleration_hdg_rate: false 31 | acceleration_hdg: false 32 | velocity_hdg_rate: false 33 | velocity_hdg: false 34 | position: false 35 | 36 | # list of names of dynamically loaded controllers 37 | controllers : [ 38 | "ExampleController", 39 | ] 40 | 41 | uav_manager: 42 | 43 | takeoff: 44 | after_takeoff: 45 | controller: "ExampleController" 46 | 47 | midair_activation: 48 | after_activation: 49 | controller: "ExampleController" 50 | 51 | min_height_checking: 52 | enabled: false 53 | 54 | max_height_checking: 55 | enabled: false 56 | -------------------------------------------------------------------------------- /cpp/controller_plugin/tmux/config/hw_api.yaml: -------------------------------------------------------------------------------- 1 | input_mode: 2 | actuators: false 3 | control_group: false 4 | attitude_rate: true 5 | attitude: true 6 | acceleration_hdg_rate: false 7 | acceleration_hdg: false 8 | velocity_hdg_rate: false 9 | velocity_hdg: false 10 | position: false 11 | 12 | feedforward: true 13 | 14 | outputs: 15 | distance_sensor: true 16 | gnss: true 17 | rtk: false 18 | imu: false 19 | altitude: true 20 | magnetometer_heading: true 21 | rc_channels: false 22 | battery_state: false 23 | position: true 24 | orientation: true 25 | velocity: true 26 | angular_velocity: true 27 | odometry: false 28 | ground_truth: true 29 | -------------------------------------------------------------------------------- /cpp/controller_plugin/tmux/config/network_config.yaml: -------------------------------------------------------------------------------- 1 | # 1. This list is used by NimbroNetwork for mutual communication of the UAVs 2 | # The names of the robots have to match hostnames described in /etc/hosts. 3 | # 4 | # 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. 5 | # The names should match the true "UAV_NAMES" (the topic prefixes). 6 | # 7 | # network_config:=~/config/network_config.yaml 8 | # 9 | # to the core.launch and nimbro.launch. 10 | 11 | network: 12 | 13 | robot_names: [ 14 | uav1 15 | ] 16 | -------------------------------------------------------------------------------- /cpp/controller_plugin/tmux/config/simulator.yaml: -------------------------------------------------------------------------------- 1 | simulation_rate: 250 2 | clock_rate: 250 3 | realtime_factor: 1.0 4 | 5 | uav_names: [ 6 | "uav1", 7 | ] 8 | 9 | individual_takeoff_platform: 10 | enabled: false 11 | 12 | uav1: 13 | type: "x500" 14 | spawn: 15 | x: 10.0 16 | y: 15.0 17 | z: 2.0 18 | heading: 0 19 | -------------------------------------------------------------------------------- /cpp/controller_plugin/tmux/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: true 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /cpp/controller_plugin/tmux/layout.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "border": "normal", 4 | "floating": "auto_off", 5 | "fullscreen_mode": 0, 6 | "layout": "splitv", 7 | "percent": 0.285714285714286, 8 | "type": "con", 9 | "nodes": [ 10 | { 11 | "border": "pixel", 12 | "current_border_width": 3, 13 | "floating": "auto_off", 14 | "geometry": { 15 | "height": 1030, 16 | "width": 936, 17 | "x": 4806, 18 | "y": 32 19 | }, 20 | "name": "rviz.rviz* - RViz", 21 | "percent": 0.5, 22 | "swallows": [ 23 | { 24 | "instance": "^rviz$" 25 | } 26 | ], 27 | "type": "con" 28 | }, 29 | { 30 | "border": "pixel", 31 | "current_border_width": 3, 32 | "floating": "auto_off", 33 | "geometry": { 34 | "height": 460, 35 | "width": 724, 36 | "x": 0, 37 | "y": 0 38 | }, 39 | "name": "./start.sh", 40 | "percent": 0.5, 41 | "swallows": [ 42 | { 43 | "instance": "^urxvt$" 44 | } 45 | ], 46 | "type": "con" 47 | } 48 | ] 49 | } 50 | ] 51 | -------------------------------------------------------------------------------- /cpp/controller_plugin/tmux/session.yml: -------------------------------------------------------------------------------- 1 | # do not modify these 2 | root: ./ 3 | name: simulation 4 | socket_name: mrs 5 | attach: false 6 | tmux_options: -f /etc/ctu-mrs/tmux.conf 7 | # you can modify these 8 | pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500 9 | startup_window: control 10 | windows: 11 | - roscore: 12 | layout: tiled 13 | panes: 14 | - roscore 15 | - simulator: 16 | layout: tiled 17 | panes: 18 | - waitForRos; roslaunch mrs_multirotor_simulator multirotor_simulator.launch custom_config:=./config/simulator.yaml 19 | - hw_api: 20 | layout: tiled 21 | panes: 22 | - waitForTime; roslaunch mrs_multirotor_simulator hw_api.launch custom_config:=./config/hw_api.yaml 23 | - takeoff: 24 | layout: tiled 25 | panes: 26 | - waitForHw; roslaunch mrs_uav_autostart automatic_start.launch 27 | - waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard 28 | # - midair_activation: 29 | # # disabled the "takeoff" section and turn on the "individual_takeoff_platform" in the simulator config 30 | # layout: tiled 31 | # panes: 32 | # - waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; rosservice call /$UAV_NAME/uav_manager/midair_activation 33 | - status: 34 | layout: tiled 35 | panes: 36 | - waitForHw; roslaunch mrs_uav_status status.launch 37 | - control: 38 | layout: tiled 39 | panes: 40 | - waitForHw; roslaunch mrs_uav_core core.launch 41 | platform_config:=`rospack find mrs_multirotor_simulator`/config/mrs_uav_system/$UAV_TYPE.yaml 42 | custom_config:=./config/custom_config.yaml 43 | world_config:=./config/world_config.yaml 44 | network_config:=./config/network_config.yaml 45 | - rviz: 46 | layout: tiled 47 | panes: 48 | - waitForControl; roslaunch mrs_uav_core rviz.launch 49 | - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch 50 | - waitForControl; waitForControl; roslaunch mrs_rviz_plugins rviz_interface.launch 51 | - layout: 52 | layout: tiled 53 | panes: 54 | - waitForControl; sleep 3; ~/.i3/layout_manager.sh ./layout.json 55 | -------------------------------------------------------------------------------- /cpp/controller_plugin/tmux/start.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Absolute path to this script. /home/user/bin/foo.sh 4 | SCRIPT=$(readlink -f $0) 5 | # Absolute path this script is in. /home/user/bin 6 | SCRIPTPATH=`dirname $SCRIPT` 7 | cd "$SCRIPTPATH" 8 | 9 | export TMUX_SESSION_NAME=simulation 10 | export TMUX_SOCKET_NAME=mrs 11 | 12 | # start tmuxinator 13 | tmuxinator start -p ./session.yml 14 | 15 | # if we are not in tmux 16 | if [ -z $TMUX ]; then 17 | 18 | # just attach to the session 19 | tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME 20 | 21 | # if we are in tmux 22 | else 23 | 24 | # switch to the newly-started session 25 | tmux detach-client -E "tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME" 26 | 27 | fi 28 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/README.md: -------------------------------------------------------------------------------- 1 | # Pluginlib Example 2 | 3 | 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). 4 | 5 | ## example_plugin_manager package 6 | 7 | * 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 8 | * 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 9 | * 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) 10 | * 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) 11 | * regularly updates the active plugin and queries a result 12 | 13 | ## example_plugins package 14 | 15 | * 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) 16 | * the plugin loads its params and prepares itself for activation 17 | * it calculates results in its `update()` method and returns them to the manager 18 | 19 | # How to start it? 20 | 21 | ```bash 22 | roslaunch example_plugin_manager example_plugin_manager.launch 23 | ``` 24 | 25 | # Dependencies 26 | 27 | * [mrs_lib](https://github.com/ctu-mrs/mrs_lib) for param loading and mutexing 28 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugin_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15.0) 2 | project(example_plugin_manager) 3 | 4 | set(CATKIN_DEPENDENCIES 5 | roscpp 6 | cmake_modules 7 | mrs_lib 8 | nodelet 9 | ) 10 | 11 | set(LIBRARIES 12 | ExamplePluginManager 13 | ) 14 | 15 | find_package(catkin REQUIRED COMPONENTS 16 | ${CATKIN_DEPENDENCIES} 17 | ) 18 | 19 | find_package(Eigen3 REQUIRED) 20 | set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) 21 | set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) 22 | 23 | set(CMAKE_CXX_STANDARD 17) 24 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 25 | 26 | catkin_package( 27 | INCLUDE_DIRS include 28 | LIBRARIES ${LIBRARIES} 29 | CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} 30 | ) 31 | 32 | include_directories( 33 | include 34 | ${catkin_INCLUDE_DIRS} 35 | ) 36 | 37 | # ExamplePluginManager 38 | 39 | add_library(ExamplePluginManager src/example_plugin_manager.cpp) 40 | 41 | add_dependencies(ExamplePluginManager 42 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 43 | ${catkin_EXPORTED_TARGETS} 44 | ) 45 | 46 | target_link_libraries(ExamplePluginManager 47 | ${catkin_LIBRARIES} 48 | ) 49 | 50 | ## -------------------------------------------------------------- 51 | ## | Install | 52 | ## -------------------------------------------------------------- 53 | 54 | install(TARGETS ${LIBRARIES} 55 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 58 | ) 59 | 60 | install(DIRECTORY launch config 61 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 62 | ) 63 | 64 | install(DIRECTORY include/example_plugin_manager/ 65 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 66 | ) 67 | 68 | install(FILES nodelets.xml 69 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 70 | ) 71 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugin_manager/config/example_plugin_manager.yaml: -------------------------------------------------------------------------------- 1 | # the list of "names" of the loaded plugins 2 | # each plugin's properties are defined in 'plugins.yaml' 3 | plugins : [ 4 | "ExamplePlugin", 5 | "ExamplePlugin2", 6 | ] 7 | 8 | # the first loaded plugin 9 | # should be within "plugins" 10 | initial_plugin: "ExamplePlugin2" 11 | 12 | update_timer_rate: 1 # [Hz] 13 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugin_manager/config/plugins.yaml: -------------------------------------------------------------------------------- 1 | # define "running instances" of plugins, there can be more than one for each 2 | 3 | ExamplePlugin: 4 | address: "example_plugins/ExamplePlugin" # this is taken from the plugin's package.xml 5 | name_space: "example_plugin" # useful for loading parameters into difference instances of the same plugin 6 | some_property: 6.28 # this can be anything specific the manager needs to hold for each instance of each plugin 7 | 8 | # We can run another instance of the same plugin 9 | ExamplePlugin2: 10 | address: "example_plugins/ExamplePlugin" # this is taken from the plugin's package.xml 11 | name_space: "example_plugin_2" 12 | some_property: 10.1 13 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugin_manager/include/example_plugin_manager/common_handlers.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_HANDLERS_H 2 | #define COMMON_HANDLERS_H 3 | 4 | #include 5 | #include 6 | 7 | namespace example_plugin_manager 8 | { 9 | 10 | //} 11 | 12 | // | ---- logical units of supplied variables and functions --- | 13 | 14 | typedef boost::function vectorNorm_t; 15 | 16 | struct VectorCalculator_t 17 | { 18 | bool enabled; 19 | example_plugin_manager::vectorNorm_t vectorNorm; 20 | }; 21 | 22 | // | ---------------------------------------------------------- | 23 | 24 | struct CommonHandlers_t 25 | { 26 | std::shared_ptr some_shared_object; 27 | VectorCalculator_t vector_calculator; 28 | }; 29 | 30 | 31 | } // namespace example_plugin_manager 32 | 33 | #endif // COMMON_HANDLERS_H 34 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugin_manager/include/example_plugin_manager/plugin_interface.h: -------------------------------------------------------------------------------- 1 | #ifndef PLUGIN_INTERFACE_H 2 | #define PLUGIN_INTERFACE_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace example_plugin_manager 9 | { 10 | 11 | class Plugin { 12 | public: 13 | virtual void initialize(const ros::NodeHandle& parent_nh, const std::string& name, const std::string& name_space, 14 | std::shared_ptr common_handlers) = 0; 15 | 16 | virtual bool activate(const int& some_number) = 0; 17 | 18 | virtual void deactivate(void) = 0; 19 | 20 | virtual const std::optional update(const Eigen::Vector3d& input) = 0; 21 | 22 | virtual ~Plugin() = default; 23 | }; 24 | 25 | } // namespace example_plugin_manager 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugin_manager/launch/example_plugin_manager.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugin_manager/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ExamplePluginManager nodelet 4 | 5 | 6 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugin_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | example_plugin_manager 5 | 1.0.0 6 | The example_plugin_manager package 7 | 8 | Tomas Baca 9 | Tomas Baca 10 | 11 | MIT 12 | 13 | catkin 14 | 15 | cmake_modules 16 | roscpp 17 | nodelet 18 | mrs_lib 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugin_manager/src/example_plugin_manager.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace example_plugin_manager 12 | { 13 | 14 | /* //{ class ExamplePluginManager */ 15 | 16 | /* class PluginParams() //{ */ 17 | 18 | class PluginParams { 19 | 20 | public: 21 | PluginParams(const std::string& address, const std::string& name_space, const double& some_property); 22 | 23 | public: 24 | std::string address; 25 | std::string name_space; 26 | double some_property; 27 | }; 28 | 29 | PluginParams::PluginParams(const std::string& address, const std::string& name_space, const double& some_property) { 30 | 31 | this->address = address; 32 | this->name_space = name_space; 33 | this->some_property = some_property; 34 | } 35 | 36 | //} 37 | 38 | class ExamplePluginManager : public nodelet::Nodelet { 39 | 40 | public: 41 | virtual void onInit(); 42 | 43 | private: 44 | ros::NodeHandle nh_; 45 | bool is_initialized_ = false; 46 | 47 | // | ---------------------- update timer ---------------------- | 48 | 49 | ros::Timer timer_update_; 50 | double _rate_timer_update_; 51 | 52 | // | -------- an object we want to share to our plugins ------- | 53 | 54 | std::string example_of_a_shared_object_; 55 | 56 | // | --------------------- common handlers -------------------- | 57 | 58 | std::shared_ptr common_handlers_; 59 | 60 | // | --------------- dynamic loading of plugins --------------- | 61 | 62 | std::unique_ptr> plugin_loader_; // pluginlib loader 63 | std::vector _plugin_names_; 64 | std::map plugins_; // map between plugin names and plugin params 65 | std::vector> plugin_list_; // list of plugins, routines are callable from this 66 | std::mutex mutex_plugins_; 67 | 68 | std::string _initial_plugin_name_; 69 | int _initial_plugin_idx_ = 0; 70 | 71 | int active_plugin_idx_ = 0; 72 | 73 | // | ------------------------ routines ------------------------ | 74 | 75 | double vectorNorm(const Eigen::Vector3d& input); 76 | 77 | // | ------------------------- timers ------------------------- | 78 | 79 | void timerUpdate(const ros::TimerEvent& event); 80 | }; 81 | 82 | //} 83 | 84 | /* onInit() //{ */ 85 | 86 | void ExamplePluginManager::onInit() { 87 | 88 | ros::NodeHandle nh_ = nodelet::Nodelet::getMTPrivateNodeHandle(); 89 | 90 | ros::Time::waitForValid(); 91 | 92 | ROS_INFO("[ExamplePluginManager]: initializing"); 93 | 94 | // -------------------------------------------------------------- 95 | // | params | 96 | // -------------------------------------------------------------- 97 | 98 | mrs_lib::ParamLoader param_loader(nh_, "ExamplePluginManager"); 99 | 100 | param_loader.loadParam("update_timer_rate", _rate_timer_update_); 101 | param_loader.loadParam("initial_plugin", _initial_plugin_name_); 102 | 103 | // | --------------- example of a shared object --------------- | 104 | 105 | example_of_a_shared_object_ = "Hello, this is a shared object"; 106 | 107 | // -------------------------------------------------------------- 108 | // | common handlers | 109 | // -------------------------------------------------------------- 110 | 111 | common_handlers_ = std::make_shared(); 112 | 113 | common_handlers_->some_shared_object = std::make_shared(example_of_a_shared_object_); 114 | 115 | common_handlers_->vector_calculator.vectorNorm = boost::bind(&ExamplePluginManager::vectorNorm, this, _1); 116 | common_handlers_->vector_calculator.enabled = true; 117 | 118 | // -------------------------------------------------------------- 119 | // | load the plugins | 120 | // -------------------------------------------------------------- 121 | 122 | param_loader.loadParam("plugins", _plugin_names_); 123 | 124 | plugin_loader_ = std::make_unique>("example_plugin_manager", "example_plugin_manager::Plugin"); 125 | 126 | // for each plugin in the list 127 | for (int i = 0; i < int(_plugin_names_.size()); i++) { 128 | std::string plugin_name = _plugin_names_[i]; 129 | 130 | // load the plugin parameters 131 | std::string address; 132 | std::string name_space; 133 | double some_property; 134 | 135 | param_loader.loadParam(plugin_name + "/address", address); 136 | param_loader.loadParam(plugin_name + "/name_space", name_space); 137 | param_loader.loadParam(plugin_name + "/some_property", some_property); 138 | 139 | PluginParams new_plugin(address, name_space, some_property); 140 | plugins_.insert(std::pair(plugin_name, new_plugin)); 141 | 142 | try { 143 | ROS_INFO("[ExamplePluginManager]: loading the plugin '%s'", new_plugin.address.c_str()); 144 | plugin_list_.push_back(plugin_loader_->createInstance(new_plugin.address.c_str())); 145 | } 146 | catch (pluginlib::CreateClassException& ex1) { 147 | ROS_ERROR("[ExamplePluginManager]: CreateClassException for the plugin '%s'", new_plugin.address.c_str()); 148 | ROS_ERROR("[ExamplePluginManager]: Error: %s", ex1.what()); 149 | ros::shutdown(); 150 | } 151 | catch (pluginlib::PluginlibException& ex) { 152 | ROS_ERROR("[ExamplePluginManager]: PluginlibException for the plugin '%s'", new_plugin.address.c_str()); 153 | ROS_ERROR("[ExamplePluginManager]: Error: %s", ex.what()); 154 | ros::shutdown(); 155 | } 156 | } 157 | 158 | ROS_INFO("[ExamplePluginManager]: plugins were loaded"); 159 | 160 | for (int i = 0; i < int(plugin_list_.size()); i++) { 161 | try { 162 | std::map::iterator it; 163 | it = plugins_.find(_plugin_names_[i]); 164 | 165 | ROS_INFO("[ExamplePluginManager]: initializing the plugin '%s'", it->second.address.c_str()); 166 | plugin_list_[i]->initialize(nh_, _plugin_names_[i], it->second.name_space, common_handlers_); 167 | } 168 | catch (std::runtime_error& ex) { 169 | ROS_ERROR("[ExamplePluginManager]: exception caught during plugin initialization: '%s'", ex.what()); 170 | } 171 | } 172 | 173 | ROS_INFO("[ExamplePluginManager]: plugins were initialized"); 174 | 175 | // -------------------------------------------------------------- 176 | // | check for existance of the initial plugin | 177 | // -------------------------------------------------------------- 178 | 179 | { 180 | bool check = false; 181 | 182 | for (int i = 0; i < int(_plugin_names_.size()); i++) { 183 | 184 | std::string plugin_name = _plugin_names_[i]; 185 | 186 | if (plugin_name == _initial_plugin_name_) { 187 | check = true; 188 | _initial_plugin_idx_ = i; 189 | break; 190 | } 191 | } 192 | if (!check) { 193 | ROS_ERROR("[ExamplePluginManager]: the initial plugin (%s) is not within the loaded plugins", _initial_plugin_name_.c_str()); 194 | ros::shutdown(); 195 | } 196 | } 197 | 198 | // | ---------- activate the first plugin on the list --------- | 199 | 200 | ROS_INFO("[ExamplePluginManager]: activating plugin with idx %d on the list (named: %s)", _initial_plugin_idx_, _plugin_names_[_initial_plugin_idx_].c_str()); 201 | 202 | int some_activation_input_to_plugin = 1234; 203 | 204 | plugin_list_[_initial_plugin_idx_]->activate(some_activation_input_to_plugin); 205 | active_plugin_idx_ = _initial_plugin_idx_; 206 | 207 | // | ------------------------- timers ------------------------- | 208 | 209 | timer_update_ = nh_.createTimer(ros::Rate(_rate_timer_update_), &ExamplePluginManager::timerUpdate, this); 210 | 211 | // | ----------------------- finish init ---------------------- | 212 | 213 | if (!param_loader.loadedSuccessfully()) { 214 | ROS_ERROR("[ExamplePluginManager]: could not load all parameters!"); 215 | ros::shutdown(); 216 | } 217 | 218 | is_initialized_ = true; 219 | 220 | ROS_INFO("[ExamplePluginManager]: initialized"); 221 | } 222 | 223 | //} 224 | 225 | // | ------------------------- timers ------------------------- | 226 | 227 | /* timerUpdate() //{ */ 228 | 229 | void ExamplePluginManager::timerUpdate([[maybe_unused]] const ros::TimerEvent& event) { 230 | 231 | if (!is_initialized_) 232 | return; 233 | 234 | auto active_plugin_idx = mrs_lib::get_mutexed(mutex_plugins_, active_plugin_idx_); 235 | 236 | // plugin input 237 | Eigen::Vector3d input; 238 | input << 0, 1, 2; 239 | 240 | // call the plugin's update routine 241 | auto result = plugin_list_[active_plugin_idx]->update(input); 242 | 243 | if (result) { 244 | 245 | // print the result 246 | ROS_INFO("[ExamplePluginManager]: plugin update() returned: %.2f", result.value()); 247 | 248 | } else { 249 | 250 | ROS_ERROR("[ExamplePluginManager]: plugin update failed!"); 251 | } 252 | } 253 | 254 | //} 255 | 256 | // | ------------------------ routines ------------------------ | 257 | 258 | /* vectorNorm() //{ */ 259 | 260 | double ExamplePluginManager::vectorNorm(const Eigen::Vector3d& input) { 261 | 262 | ROS_INFO("[ExamplePluginManager]: somebody called my vectorNorm() function, probably some plugin"); 263 | 264 | return input.norm(); 265 | } 266 | 267 | //} 268 | 269 | } // namespace example_plugin_manager 270 | 271 | #include 272 | PLUGINLIB_EXPORT_CLASS(example_plugin_manager::ExamplePluginManager, nodelet::Nodelet) 273 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15.0) 2 | project(example_plugins) 3 | 4 | set(CATKIN_DEPENDENCIES 5 | roscpp 6 | cmake_modules 7 | mrs_lib 8 | example_plugin_manager 9 | pluginlib 10 | ) 11 | 12 | set(LIBRARIES 13 | ExamplePlugin 14 | ) 15 | 16 | find_package(catkin REQUIRED COMPONENTS 17 | ${CATKIN_DEPENDENCIES} 18 | ) 19 | 20 | find_package(Eigen3 REQUIRED) 21 | set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) 22 | set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) 23 | 24 | set(CMAKE_CXX_STANDARD 17) 25 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 26 | 27 | add_definitions(-Wall) 28 | add_definitions(-Wextra) 29 | add_definitions(-Wpedantic) 30 | 31 | catkin_package( 32 | LIBRARIES ${LIBRARIES} 33 | CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} 34 | DEPENDS Eigen 35 | ) 36 | 37 | include_directories( 38 | ${catkin_INCLUDE_DIRS} 39 | ) 40 | 41 | # ExamplePlugin 42 | 43 | add_library(ExamplePlugin 44 | src/example_plugin.cpp 45 | ) 46 | 47 | add_dependencies(ExamplePlugin 48 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 49 | ${catkin_EXPORTED_TARGETS} 50 | ) 51 | 52 | target_link_libraries(ExamplePlugin 53 | ${catkin_LIBRARIES} 54 | ${Eigen_LIBRARIES} 55 | ) 56 | 57 | ## -------------------------------------------------------------- 58 | ## | Install | 59 | ## -------------------------------------------------------------- 60 | 61 | install(TARGETS ${LIBRARIES} 62 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 63 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 64 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 65 | ) 66 | 67 | install(DIRECTORY config 68 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 69 | ) 70 | 71 | install(FILES plugins.xml 72 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 73 | ) 74 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugins/config/example_plugin.yaml: -------------------------------------------------------------------------------- 1 | pi: 3.141592653 2 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | example_plugins 5 | 1.0.0 6 | The example_plugins package 7 | 8 | Tomas Baca 9 | Tomas Baca 10 | 11 | MIT 12 | 13 | catkin 14 | 15 | pluginlib 16 | roscpp 17 | cmake_modules 18 | mrs_lib 19 | example_plugin_manager 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugins/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is ExamplePlugin. 4 | 5 | 6 | -------------------------------------------------------------------------------- /cpp/pluginlib_example/example_plugins/src/example_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace example_plugins 9 | { 10 | 11 | namespace example_plugin 12 | { 13 | 14 | /* class ExamplePlugin //{ */ 15 | 16 | class ExamplePlugin : public example_plugin_manager::Plugin { 17 | 18 | public: 19 | void initialize(const ros::NodeHandle& parent_nh, const std::string& name, const std::string& name_space, 20 | std::shared_ptr common_handlers); 21 | 22 | bool activate(const int& some_number); 23 | void deactivate(void); 24 | 25 | const std::optional update(const Eigen::Vector3d& input); 26 | 27 | // parameter from a config file 28 | double _pi_; 29 | 30 | std::string _name_; 31 | 32 | private: 33 | bool is_initialized_ = false; 34 | bool is_active_ = false; 35 | 36 | std::shared_ptr common_handlers_; 37 | }; 38 | 39 | //} 40 | 41 | // | -------------------- plugin interface -------------------- | 42 | 43 | /* initialize() //{ */ 44 | 45 | void ExamplePlugin::initialize(const ros::NodeHandle& parent_nh, const std::string& name, const std::string& name_space, 46 | std::shared_ptr common_handlers) { 47 | 48 | // nh_ will behave just like normal NodeHandle 49 | ros::NodeHandle nh_(parent_nh, name_space); 50 | 51 | _name_ = name; 52 | 53 | // I can use this to get stuff from the manager interactively 54 | common_handlers_ = common_handlers; 55 | 56 | ros::Time::waitForValid(); 57 | 58 | // | ------------------- loading parameters ------------------- | 59 | 60 | mrs_lib::ParamLoader param_loader(nh_, "ExamplePlugin"); 61 | 62 | param_loader.addYamlFile(ros::package::getPath("example_plugins") + "/config/example_plugin.yaml"); 63 | 64 | // can load params like in a ROS node 65 | param_loader.loadParam("pi", _pi_); 66 | 67 | if (!param_loader.loadedSuccessfully()) { 68 | ROS_ERROR("[%s]: could not load all parameters!", _name_.c_str()); 69 | ros::shutdown(); 70 | } 71 | 72 | ROS_INFO("[%s]: loaded custom parameter: pi=%f", _name_.c_str(), _pi_); 73 | 74 | // | ----------------------- finish init ---------------------- | 75 | 76 | ROS_INFO("[%s]: initialized under the name '%s', and namespace '%s'", _name_.c_str(), name.c_str(), name_space.c_str()); 77 | 78 | is_initialized_ = true; 79 | } 80 | 81 | //} 82 | 83 | /* activate() //{ */ 84 | 85 | bool ExamplePlugin::activate(const int& some_number) { 86 | 87 | ROS_INFO("[%s]: activated with some_number=%d", _name_.c_str(), some_number); 88 | 89 | is_active_ = true; 90 | 91 | return true; 92 | } 93 | 94 | //} 95 | 96 | /* deactivate() //{ */ 97 | 98 | void ExamplePlugin::deactivate(void) { 99 | 100 | is_active_ = false; 101 | 102 | ROS_INFO("[%s]: deactivated", _name_.c_str()); 103 | } 104 | 105 | //} 106 | 107 | /* update() //{ */ 108 | 109 | const std::optional ExamplePlugin::update(const Eigen::Vector3d& input) { 110 | 111 | if (!is_active_) { 112 | return false; 113 | } 114 | 115 | ROS_INFO_STREAM("[" << _name_ << "]: update() was called, let's find out the size of the vector [" << input.transpose() << "]"); 116 | 117 | // check some property from the "manager" 118 | if (common_handlers_->vector_calculator.enabled) { 119 | 120 | // use a function from the common_handlers 121 | double vector_norm = common_handlers_->vector_calculator.vectorNorm(input); 122 | 123 | // we calculated our result, just return it to the manager 124 | return vector_norm; 125 | 126 | } else { 127 | 128 | return false; 129 | } 130 | } 131 | 132 | //} 133 | 134 | } // namespace example_plugin 135 | 136 | } // namespace example_plugins 137 | 138 | #include 139 | PLUGINLIB_EXPORT_CLASS(example_plugins::example_plugin::ExamplePlugin, example_plugin_manager::Plugin) 140 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1.2) 2 | project(example_tracker_plugin) 3 | 4 | set(CATKIN_DEPENDENCIES 5 | cmake_modules 6 | roscpp 7 | rospy 8 | dynamic_reconfigure 9 | mrs_lib 10 | mrs_msgs 11 | mrs_uav_managers 12 | ) 13 | 14 | find_package(catkin REQUIRED COMPONENTS 15 | ${CATKIN_DEPENDENCIES} 16 | ) 17 | 18 | generate_dynamic_reconfigure_options( 19 | cfg/example_tracker.cfg 20 | ) 21 | 22 | set(CMAKE_CXX_STANDARD 17) 23 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 24 | 25 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 26 | 27 | # include Eigen3 28 | find_package(Eigen3 REQUIRED) 29 | set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) 30 | set(Eigen_LIBRARIES ${EIGEN3_LIBRARIES}) 31 | 32 | set(LIBRARIES 33 | ExampleTracker 34 | ) 35 | 36 | catkin_package( 37 | INCLUDE_DIRS include 38 | CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} 39 | LIBRARIES ${LIBRARIES} 40 | DEPENDS Eigen 41 | ) 42 | 43 | ## -------------------------------------------------------------- 44 | ## | Build | 45 | ## -------------------------------------------------------------- 46 | 47 | include_directories( 48 | include 49 | ${EIGEN_INCLUDE_DIR} 50 | ${catkin_INCLUDE_DIRS} 51 | ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake 52 | ) 53 | 54 | # MPC Tracker 55 | 56 | add_library(ExampleTracker 57 | src/example_tracker.cpp 58 | ) 59 | 60 | add_dependencies(ExampleTracker 61 | ${catkin_EXPORTED_TARGETS} 62 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 63 | ) 64 | 65 | target_link_libraries(ExampleTracker 66 | ${catkin_LIBRARIES} 67 | ) 68 | 69 | ## -------------------------------------------------------------- 70 | ## | Install | 71 | ## -------------------------------------------------------------- 72 | 73 | install(TARGETS ${LIBRARIES} 74 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 75 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 76 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 77 | ) 78 | 79 | install(DIRECTORY config 80 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 81 | ) 82 | 83 | install(FILES plugins.xml 84 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 85 | ) 86 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/README.md: -------------------------------------------------------------------------------- 1 | # Example Tracker Plugin 2 | 3 | Example tracker plugin for the MRS Control Manager. 4 | 5 | ## How to start 6 | 7 | ```bash 8 | ./tmux/start.sh 9 | ``` 10 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/cfg/example_tracker.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "example_tracker_plugin" 3 | 4 | import roslib; 5 | roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | dynamics = gen.add_group("Dynamics"); 12 | 13 | dynamics.add("some_parameter", double_t, 0, "Some parameter", 0.0, -10.0, 10.0) 14 | 15 | exit(gen.generate(PACKAGE, "ExampleTracker", "example_tracker")) 16 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/config/example_tracker.yaml: -------------------------------------------------------------------------------- 1 | some_parameter: 1.0 # [-] 2 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/include/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ctu-mrs/mrs_core_examples/22d9a4d2ed3e5937a0712a305e576b25dfa741da/cpp/tracker_plugin/include/.gitkeep -------------------------------------------------------------------------------- /cpp/tracker_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | example_tracker_plugin 5 | 1.0.0 6 | The example_controller_plugin package 7 | 8 | Tomas Baca 9 | Tomas Baca 10 | 11 | BSD 3-Clause 12 | 13 | catkin 14 | 15 | cmake_modules 16 | rospy 17 | pluginlib 18 | roscpp 19 | mrs_msgs 20 | mrs_lib 21 | mrs_uav_managers 22 | dynamic_reconfigure 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is the example tracker. 4 | 5 | 6 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/src/example_tracker.cpp: -------------------------------------------------------------------------------- 1 | /* includes //{ */ 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | //} 18 | 19 | namespace example_tracker_plugin 20 | { 21 | 22 | namespace example_tracker 23 | { 24 | 25 | /* //{ class ExampleTracker */ 26 | 27 | class ExampleTracker : public mrs_uav_managers::Tracker { 28 | public: 29 | bool initialize(const ros::NodeHandle &nh, std::shared_ptr common_handlers, 30 | std::shared_ptr private_handlers); 31 | 32 | std::tuple activate(const std::optional &last_tracker_cmd); 33 | void deactivate(void); 34 | bool resetStatic(void); 35 | 36 | std::optional update(const mrs_msgs::UavState &uav_state, const mrs_uav_managers::Controller::ControlOutput &last_control_output); 37 | const mrs_msgs::TrackerStatus getStatus(); 38 | const std_srvs::SetBoolResponse::ConstPtr enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd); 39 | const std_srvs::TriggerResponse::ConstPtr switchOdometrySource(const mrs_msgs::UavState &new_uav_state); 40 | 41 | const mrs_msgs::ReferenceSrvResponse::ConstPtr setReference(const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd); 42 | const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr setVelocityReference(const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd); 43 | const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr setTrajectoryReference(const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd); 44 | 45 | const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd); 46 | 47 | const std_srvs::TriggerResponse::ConstPtr hover(const std_srvs::TriggerRequest::ConstPtr &cmd); 48 | const std_srvs::TriggerResponse::ConstPtr startTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd); 49 | const std_srvs::TriggerResponse::ConstPtr stopTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd); 50 | const std_srvs::TriggerResponse::ConstPtr resumeTrajectoryTracking(const std_srvs::TriggerRequest::ConstPtr &cmd); 51 | const std_srvs::TriggerResponse::ConstPtr gotoTrajectoryStart(const std_srvs::TriggerRequest::ConstPtr &cmd); 52 | 53 | private: 54 | ros::NodeHandle nh_; 55 | 56 | bool callbacks_enabled_ = true; 57 | 58 | std::string _uav_name_; 59 | 60 | std::shared_ptr common_handlers_; 61 | std::shared_ptr private_handlers_; 62 | 63 | // | ------------------------ uav state ----------------------- | 64 | 65 | mrs_msgs::UavState uav_state_; 66 | bool got_uav_state_ = false; 67 | std::mutex mutex_uav_state_; 68 | 69 | // | ------------------ dynamics constriants ------------------ | 70 | 71 | mrs_msgs::DynamicsConstraints constraints_; 72 | std::mutex mutex_constraints_; 73 | 74 | // | ----------------------- goal state ----------------------- | 75 | 76 | double goal_x_ = 0; 77 | double goal_y_ = 0; 78 | double goal_z_ = 0; 79 | double goal_heading_ = 0; 80 | 81 | // | ---------------- the tracker's inner state --------------- | 82 | 83 | std::atomic is_initialized_ = false; 84 | std::atomic is_active_ = false; 85 | 86 | double pos_x_ = 0; 87 | double pos_y_ = 0; 88 | double pos_z_ = 0; 89 | double heading_ = 0; 90 | 91 | // | ------------------- for calculating dt ------------------- | 92 | 93 | ros::Time last_update_time_; 94 | std::atomic first_iteration_ = true; 95 | 96 | // | --------------- dynamic reconfigure server --------------- | 97 | 98 | boost::recursive_mutex mutex_drs_; 99 | typedef example_tracker_plugin::example_trackerConfig DrsConfig_t; 100 | typedef dynamic_reconfigure::Server Drs_t; 101 | boost::shared_ptr drs_; 102 | void callbackDrs(example_tracker_plugin::example_trackerConfig &config, uint32_t level); 103 | DrsConfig_t drs_params_; 104 | std::mutex mutex_drs_params_; 105 | }; 106 | 107 | //} 108 | 109 | // | -------------- tracker's interface routines -------------- | 110 | 111 | /* //{ initialize() */ 112 | 113 | bool ExampleTracker::initialize(const ros::NodeHandle &nh, std::shared_ptr common_handlers, 114 | std::shared_ptr private_handlers) { 115 | 116 | this->common_handlers_ = common_handlers; 117 | this->private_handlers_ = private_handlers; 118 | 119 | _uav_name_ = common_handlers->uav_name; 120 | 121 | nh_ = nh; 122 | 123 | ros::Time::waitForValid(); 124 | 125 | last_update_time_ = ros::Time(0); 126 | 127 | // -------------------------------------------------------------- 128 | // | loading parameters | 129 | // -------------------------------------------------------------- 130 | 131 | // | -------------------- load param files -------------------- | 132 | 133 | bool success = true; 134 | 135 | // FYI 136 | // This method will load the file using `rosparam get` 137 | // Pros: you can the full power of the official param loading 138 | // Cons: it is slower 139 | // 140 | // Alternatives: 141 | // You can load the file directly into the ParamLoader as shown below. 142 | 143 | success *= private_handlers->loadConfigFile(ros::package::getPath("example_tracker_plugin") + "/config/example_tracker.yaml"); 144 | 145 | if (!success) { 146 | return false; 147 | } 148 | 149 | // | ---------------- load plugin's parameters ---------------- | 150 | 151 | mrs_lib::ParamLoader param_loader(nh_, "ExampleTracker"); 152 | 153 | // This is the alternaive way of loading the config file. 154 | // 155 | // Files loaded using this method are prioritized over ROS params. 156 | // 157 | // param_loader.addYamlFile(ros::package::getPath("example_tracker_plugin") + "/config/example_tracker.yaml"); 158 | 159 | param_loader.loadParam("some_parameter", drs_params_.some_parameter); 160 | 161 | 162 | if (!param_loader.loadedSuccessfully()) { 163 | ROS_ERROR("[ExampleTracker]: could not load all parameters!"); 164 | return false; 165 | } 166 | 167 | // | --------------- dynamic reconfigure server --------------- | 168 | 169 | drs_.reset(new Drs_t(mutex_drs_, nh_)); 170 | drs_->updateConfig(drs_params_); 171 | Drs_t::CallbackType f = boost::bind(&ExampleTracker::callbackDrs, this, _1, _2); 172 | drs_->setCallback(f); 173 | 174 | // | --------------------- finish the init -------------------- | 175 | 176 | is_initialized_ = true; 177 | 178 | ROS_INFO("[ExampleTracker]: initialized"); 179 | 180 | return true; 181 | } 182 | 183 | //} 184 | 185 | /* //{ activate() */ 186 | 187 | std::tuple ExampleTracker::activate([[maybe_unused]] const std::optional &last_tracker_cmd) { 188 | 189 | if (last_tracker_cmd) { 190 | 191 | // actually, you should actually check if these entries are filled in 192 | pos_x_ = last_tracker_cmd->position.x; 193 | pos_y_ = last_tracker_cmd->position.y; 194 | pos_z_ = last_tracker_cmd->position.z; 195 | heading_ = last_tracker_cmd->heading; 196 | 197 | goal_x_ = pos_x_; 198 | goal_y_ = pos_y_; 199 | goal_z_ = pos_z_; 200 | goal_heading_ = heading_; 201 | 202 | } else { 203 | 204 | auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_); 205 | 206 | pos_x_ = uav_state.pose.position.x; 207 | pos_y_ = uav_state.pose.position.y; 208 | pos_z_ = uav_state.pose.position.z; 209 | 210 | try { 211 | heading_ = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading(); 212 | } 213 | catch (...) { 214 | heading_ = 0; 215 | } 216 | } 217 | 218 | std::stringstream ss; 219 | ss << "activated"; 220 | ROS_INFO_STREAM("[ExampleTracker]: " << ss.str()); 221 | 222 | is_active_ = true; 223 | 224 | return std::tuple(true, ss.str()); 225 | } 226 | 227 | //} 228 | 229 | /* //{ deactivate() */ 230 | 231 | void ExampleTracker::deactivate(void) { 232 | 233 | is_active_ = false; 234 | 235 | ROS_INFO("[ExampleTracker]: deactivated"); 236 | } 237 | 238 | //} 239 | 240 | /* //{ update() */ 241 | 242 | std::optional ExampleTracker::update(const mrs_msgs::UavState & uav_state, 243 | [[maybe_unused]] const mrs_uav_managers::Controller::ControlOutput &last_control_output) { 244 | 245 | { 246 | std::scoped_lock lock(mutex_uav_state_); 247 | 248 | uav_state_ = uav_state; 249 | 250 | got_uav_state_ = true; 251 | } 252 | 253 | // | ---------- calculate dt from the last iteration ---------- | 254 | 255 | double dt; 256 | 257 | if (first_iteration_) { 258 | dt = 0.01; 259 | first_iteration_ = false; 260 | } else { 261 | dt = (uav_state.header.stamp - last_update_time_).toSec(); 262 | } 263 | 264 | last_update_time_ = uav_state.header.stamp; 265 | 266 | if (fabs(dt) < 0.001) { 267 | 268 | ROS_DEBUG("[ExampleTracker]: the last odometry message came too close (%.2f s)!", dt); 269 | dt = 0.01; 270 | } 271 | 272 | // up to this part the update() method is evaluated even when the tracker is not active 273 | if (!is_active_) { 274 | return {}; 275 | } 276 | 277 | // | ------------------ move the inner state ------------------ | 278 | 279 | auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_); 280 | 281 | Eigen::Vector2d vec_to_goal_horizontal(goal_x_ - pos_x_, goal_y_ - pos_y_); 282 | double to_goal_vertical = goal_z_ - pos_z_; 283 | double to_goal_heading = mrs_lib::geometry::sradians::diff(goal_heading_, heading_); 284 | 285 | Eigen::Vector2d step_horizontal = vec_to_goal_horizontal.normalized() * constraints.horizontal_speed * dt; 286 | 287 | double step_vertical; 288 | if (to_goal_vertical >= 0) { 289 | step_vertical = constraints.vertical_ascending_speed * dt; 290 | } else { 291 | step_vertical = -constraints.vertical_descending_speed * dt; 292 | } 293 | 294 | double step_heading = mrs_lib::signum(to_goal_heading) * constraints.heading_speed * dt; 295 | 296 | pos_x_ += step_horizontal(0); 297 | pos_y_ += step_horizontal(1); 298 | pos_z_ += step_vertical; 299 | heading_ += step_heading; 300 | 301 | // | --------------- check for reaching the goal -------------- | 302 | 303 | if (vec_to_goal_horizontal.norm() <= constraints.horizontal_speed * dt) { 304 | pos_x_ = goal_x_; 305 | pos_y_ = goal_y_; 306 | } 307 | 308 | if (to_goal_vertical > 0 && to_goal_vertical <= constraints.vertical_ascending_speed * dt) { 309 | pos_z_ = goal_z_; 310 | } 311 | 312 | if (to_goal_vertical < 0 && -to_goal_vertical <= constraints.vertical_descending_speed * dt) { 313 | pos_z_ = goal_z_; 314 | } 315 | 316 | if (std::abs(to_goal_heading) <= constraints.heading_speed * dt) { 317 | heading_ = goal_heading_; 318 | } 319 | 320 | // | ------------------- fill in the result ------------------- | 321 | 322 | mrs_msgs::TrackerCommand tracker_cmd; 323 | 324 | tracker_cmd.header.stamp = ros::Time::now(); 325 | tracker_cmd.header.frame_id = uav_state.header.frame_id; 326 | 327 | tracker_cmd.position.x = pos_x_; 328 | tracker_cmd.position.y = pos_y_; 329 | tracker_cmd.position.z = pos_z_; 330 | tracker_cmd.heading = heading_; 331 | 332 | tracker_cmd.use_position_vertical = 1; 333 | tracker_cmd.use_position_horizontal = 1; 334 | tracker_cmd.use_heading = 1; 335 | 336 | return {tracker_cmd}; 337 | } 338 | 339 | //} 340 | 341 | /* //{ resetStatic() */ 342 | 343 | bool ExampleTracker::resetStatic(void) { 344 | 345 | return false; 346 | } 347 | 348 | //} 349 | 350 | /* //{ getStatus() */ 351 | 352 | const mrs_msgs::TrackerStatus ExampleTracker::getStatus() { 353 | 354 | mrs_msgs::TrackerStatus tracker_status; 355 | 356 | tracker_status.active = is_active_; 357 | tracker_status.callbacks_enabled = callbacks_enabled_; 358 | 359 | return tracker_status; 360 | } 361 | 362 | //} 363 | 364 | /* //{ enableCallbacks() */ 365 | 366 | const std_srvs::SetBoolResponse::ConstPtr ExampleTracker::enableCallbacks(const std_srvs::SetBoolRequest::ConstPtr &cmd) { 367 | 368 | std_srvs::SetBoolResponse res; 369 | std::stringstream ss; 370 | 371 | if (cmd->data != callbacks_enabled_) { 372 | 373 | callbacks_enabled_ = cmd->data; 374 | 375 | ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled"); 376 | ROS_INFO_STREAM_THROTTLE(1.0, "[ExampleTracker]: " << ss.str()); 377 | 378 | } else { 379 | 380 | ss << "callbacks were already " << (callbacks_enabled_ ? "enabled" : "disabled"); 381 | ROS_WARN_STREAM_THROTTLE(1.0, "[ExampleTracker]: " << ss.str()); 382 | } 383 | 384 | res.message = ss.str(); 385 | res.success = true; 386 | 387 | return std_srvs::SetBoolResponse::ConstPtr(new std_srvs::SetBoolResponse(res)); 388 | } 389 | 390 | //} 391 | 392 | /* switchOdometrySource() //{ */ 393 | 394 | const std_srvs::TriggerResponse::ConstPtr ExampleTracker::switchOdometrySource([[maybe_unused]] const mrs_msgs::UavState &new_uav_state) { 395 | 396 | return std_srvs::TriggerResponse::Ptr(); 397 | } 398 | 399 | //} 400 | 401 | /* //{ hover() */ 402 | 403 | const std_srvs::TriggerResponse::ConstPtr ExampleTracker::hover([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) { 404 | 405 | return std_srvs::TriggerResponse::Ptr(); 406 | } 407 | 408 | //} 409 | 410 | /* //{ startTrajectoryTracking() */ 411 | 412 | const std_srvs::TriggerResponse::ConstPtr ExampleTracker::startTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) { 413 | return std_srvs::TriggerResponse::Ptr(); 414 | } 415 | 416 | //} 417 | 418 | /* //{ stopTrajectoryTracking() */ 419 | 420 | const std_srvs::TriggerResponse::ConstPtr ExampleTracker::stopTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) { 421 | return std_srvs::TriggerResponse::Ptr(); 422 | } 423 | 424 | //} 425 | 426 | /* //{ resumeTrajectoryTracking() */ 427 | 428 | const std_srvs::TriggerResponse::ConstPtr ExampleTracker::resumeTrajectoryTracking([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) { 429 | return std_srvs::TriggerResponse::Ptr(); 430 | } 431 | 432 | //} 433 | 434 | /* //{ gotoTrajectoryStart() */ 435 | 436 | const std_srvs::TriggerResponse::ConstPtr ExampleTracker::gotoTrajectoryStart([[maybe_unused]] const std_srvs::TriggerRequest::ConstPtr &cmd) { 437 | return std_srvs::TriggerResponse::Ptr(); 438 | } 439 | 440 | //} 441 | 442 | /* //{ setConstraints() */ 443 | 444 | const mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr ExampleTracker::setConstraints([ 445 | [maybe_unused]] const mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr &cmd) { 446 | 447 | { 448 | std::scoped_lock lock(mutex_constraints_); 449 | 450 | constraints_ = cmd->constraints; 451 | } 452 | 453 | mrs_msgs::DynamicsConstraintsSrvResponse res; 454 | 455 | res.success = true; 456 | res.message = "constraints updated"; 457 | 458 | return mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr(new mrs_msgs::DynamicsConstraintsSrvResponse(res)); 459 | } 460 | 461 | //} 462 | 463 | /* //{ setReference() */ 464 | 465 | const mrs_msgs::ReferenceSrvResponse::ConstPtr ExampleTracker::setReference([[maybe_unused]] const mrs_msgs::ReferenceSrvRequest::ConstPtr &cmd) { 466 | 467 | goal_x_ = cmd->reference.position.x; 468 | goal_y_ = cmd->reference.position.y; 469 | goal_z_ = cmd->reference.position.z; 470 | goal_heading_ = cmd->reference.heading; 471 | 472 | mrs_msgs::ReferenceSrvResponse response; 473 | 474 | response.message = "reference set"; 475 | response.success = true; 476 | 477 | return mrs_msgs::ReferenceSrvResponse::ConstPtr(new mrs_msgs::ReferenceSrvResponse(response)); 478 | } 479 | 480 | //} 481 | 482 | /* //{ setVelocityReference() */ 483 | 484 | const mrs_msgs::VelocityReferenceSrvResponse::ConstPtr ExampleTracker::setVelocityReference([ 485 | [maybe_unused]] const mrs_msgs::VelocityReferenceSrvRequest::ConstPtr &cmd) { 486 | return mrs_msgs::VelocityReferenceSrvResponse::Ptr(); 487 | } 488 | 489 | //} 490 | 491 | /* //{ setTrajectoryReference() */ 492 | 493 | const mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr ExampleTracker::setTrajectoryReference([ 494 | [maybe_unused]] const mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr &cmd) { 495 | return mrs_msgs::TrajectoryReferenceSrvResponse::Ptr(); 496 | } 497 | 498 | //} 499 | 500 | // -------------------------------------------------------------- 501 | // | callbacks | 502 | // -------------------------------------------------------------- 503 | 504 | /* //{ callbackDrs() */ 505 | 506 | void ExampleTracker::callbackDrs(example_tracker_plugin::example_trackerConfig &config, [[maybe_unused]] uint32_t level) { 507 | 508 | mrs_lib::set_mutexed(mutex_drs_params_, config, drs_params_); 509 | 510 | ROS_INFO("[Exampletracker]: dynamic reconfigure params updated"); 511 | } 512 | 513 | //} 514 | 515 | } // namespace example_tracker 516 | 517 | } // namespace example_tracker_plugin 518 | 519 | #include 520 | PLUGINLIB_EXPORT_CLASS(example_tracker_plugin::example_tracker::ExampleTracker, mrs_uav_managers::Tracker) 521 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/tmux/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | mrs_uav_managers: 2 | 3 | control_manager: 4 | 5 | ExampleTracker: 6 | address: "example_tracker_plugin/ExampleTracker" 7 | namespace: "example_tracker" 8 | human_switchable: true 9 | 10 | # list of names of dynamically loaded trackers 11 | trackers : [ 12 | "ExampleTracker", 13 | ] 14 | 15 | uav_manager: 16 | 17 | takeoff: 18 | after_takeoff: 19 | tracker: "ExampleTracker" 20 | 21 | midair_activation: 22 | after_activation: 23 | tracker: "ExampleTracker" 24 | 25 | min_height_checking: 26 | enabled: false 27 | 28 | max_height_checking: 29 | enabled: false 30 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/tmux/config/hw_api.yaml: -------------------------------------------------------------------------------- 1 | input_mode: 2 | actuators: false 3 | control_group: false 4 | attitude_rate: true 5 | attitude: true 6 | acceleration_hdg_rate: false 7 | acceleration_hdg: false 8 | velocity_hdg_rate: false 9 | velocity_hdg: false 10 | position: false 11 | 12 | feedforward: true 13 | 14 | outputs: 15 | distance_sensor: true 16 | gnss: true 17 | rtk: false 18 | imu: false 19 | altitude: true 20 | magnetometer_heading: true 21 | rc_channels: false 22 | battery_state: false 23 | position: true 24 | orientation: true 25 | velocity: true 26 | angular_velocity: true 27 | odometry: false 28 | ground_truth: true 29 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/tmux/config/network_config.yaml: -------------------------------------------------------------------------------- 1 | # 1. This list is used by NimbroNetwork for mutual communication of the UAVs 2 | # The names of the robots have to match hostnames described in /etc/hosts. 3 | # 4 | # 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. 5 | # The names should match the true "UAV_NAMES" (the topic prefixes). 6 | # 7 | # network_config:=~/config/network_config.yaml 8 | # 9 | # to the core.launch and nimbro.launch. 10 | 11 | network: 12 | 13 | robot_names: [ 14 | 15 | uav1, 16 | 17 | ] 18 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/tmux/config/simulator.yaml: -------------------------------------------------------------------------------- 1 | simulation_rate: 250.0 2 | clock_rate: 250.0 3 | realtime_factor: 1.0 4 | 5 | individual_takeoff_platform: 6 | enabled: false 7 | 8 | uav_names: [ 9 | "uav1", 10 | ] 11 | 12 | uav1: 13 | type: "x500" 14 | spawn: 15 | x: 10.0 16 | y: 15.0 17 | z: 2.0 18 | heading: 0 19 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/tmux/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: true 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/tmux/layout.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "border": "normal", 4 | "floating": "auto_off", 5 | "fullscreen_mode": 0, 6 | "layout": "splitv", 7 | "percent": 0.285714285714286, 8 | "type": "con", 9 | "nodes": [ 10 | { 11 | "border": "pixel", 12 | "current_border_width": 3, 13 | "floating": "auto_off", 14 | "geometry": { 15 | "height": 1030, 16 | "width": 936, 17 | "x": 4806, 18 | "y": 32 19 | }, 20 | "name": "rviz.rviz* - RViz", 21 | "percent": 0.5, 22 | "swallows": [ 23 | { 24 | "instance": "^rviz$" 25 | } 26 | ], 27 | "type": "con" 28 | }, 29 | { 30 | "border": "pixel", 31 | "current_border_width": 3, 32 | "floating": "auto_off", 33 | "geometry": { 34 | "height": 460, 35 | "width": 724, 36 | "x": 0, 37 | "y": 0 38 | }, 39 | "name": "./start.sh", 40 | "percent": 0.5, 41 | "swallows": [ 42 | { 43 | "instance": "^urxvt$" 44 | } 45 | ], 46 | "type": "con" 47 | } 48 | ] 49 | } 50 | ] 51 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/tmux/session.yml: -------------------------------------------------------------------------------- 1 | # do not modify these 2 | root: ./ 3 | name: simulation 4 | socket_name: mrs 5 | attach: false 6 | tmux_options: -f /etc/ctu-mrs/tmux.conf 7 | # you can modify these 8 | pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500 9 | startup_window: control 10 | windows: 11 | - roscore: 12 | layout: tiled 13 | panes: 14 | - roscore 15 | - simulator: 16 | layout: tiled 17 | panes: 18 | - waitForRos; roslaunch mrs_multirotor_simulator multirotor_simulator.launch custom_config:=./config/simulator.yaml 19 | - hw_api: 20 | layout: tiled 21 | panes: 22 | - waitForTime; roslaunch mrs_multirotor_simulator hw_api.launch custom_config:=./config/hw_api.yaml 23 | - takeoff: 24 | layout: tiled 25 | panes: 26 | - waitForHw; roslaunch mrs_uav_autostart automatic_start.launch 27 | - waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard 28 | # - midair_activation: 29 | # # disabled the "takeoff" section and turn on the "individual_takeoff_platform" in the simulator config 30 | # layout: tiled 31 | # panes: 32 | # - waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; rosservice call /$UAV_NAME/uav_manager/midair_activation 33 | - status: 34 | layout: tiled 35 | panes: 36 | - waitForHw; roslaunch mrs_uav_status status.launch 37 | - control: 38 | layout: tiled 39 | panes: 40 | - waitForHw; roslaunch mrs_uav_core core.launch 41 | platform_config:=`rospack find mrs_multirotor_simulator`/config/mrs_uav_system/$UAV_TYPE.yaml 42 | custom_config:=./config/custom_config.yaml 43 | world_config:=./config/world_config.yaml 44 | network_config:=./config/network_config.yaml 45 | - rviz: 46 | layout: tiled 47 | panes: 48 | - waitForControl; roslaunch mrs_uav_core rviz.launch 49 | - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch 50 | - waitForControl; waitForControl; roslaunch mrs_rviz_plugins rviz_interface.launch 51 | - layout: 52 | layout: tiled 53 | panes: 54 | - waitForControl; sleep 3; ~/.i3/layout_manager.sh ./layout.json 55 | -------------------------------------------------------------------------------- /cpp/tracker_plugin/tmux/start.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Absolute path to this script. /home/user/bin/foo.sh 4 | SCRIPT=$(readlink -f $0) 5 | # Absolute path this script is in. /home/user/bin 6 | SCRIPTPATH=`dirname $SCRIPT` 7 | cd "$SCRIPTPATH" 8 | 9 | export TMUX_SESSION_NAME=simulation 10 | export TMUX_SOCKET_NAME=mrs 11 | 12 | # start tmuxinator 13 | tmuxinator start -p ./session.yml 14 | 15 | # if we are not in tmux 16 | if [ -z $TMUX ]; then 17 | 18 | # just attach to the session 19 | tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME 20 | 21 | # if we are in tmux 22 | else 23 | 24 | # switch to the newly-started session 25 | tmux detach-client -E "tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME" 26 | 27 | fi 28 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15.0) 2 | project(example_waypoint_flier) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | # Uncomment these two lines to enable debugging symbols and disable optimization for debugging purposes. 7 | # add_definitions(-g) 8 | # add_definitions(-O0) 9 | 10 | SET(CATKIN_DEPENDENCIES 11 | roscpp 12 | nodelet 13 | dynamic_reconfigure 14 | geometry_msgs 15 | nav_msgs 16 | mrs_msgs 17 | mrs_lib 18 | ) 19 | 20 | SET(LIBRARIES 21 | ExampleWaypointFlier 22 | ) 23 | 24 | find_package(catkin REQUIRED COMPONENTS 25 | ${CATKIN_DEPENDENCIES} 26 | ) 27 | 28 | find_package(Eigen3 REQUIRED) 29 | set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) 30 | set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) 31 | 32 | ## Generate dynamic parameters which are loaded by the dynamic reconfigure server 33 | generate_dynamic_reconfigure_options( 34 | config/dynparam.cfg 35 | ) 36 | 37 | catkin_package( 38 | LIBRARIES ${LIBRARIES} 39 | CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} 40 | DEPENDS Eigen 41 | ) 42 | 43 | include_directories( 44 | ${catkin_INCLUDE_DIRS} 45 | ${Eigen_INCLUDE_DIRS} 46 | ) 47 | 48 | ## Declare a C++ library 49 | add_library(ExampleWaypointFlier 50 | src/example_waypoint_flier.cpp 51 | ) 52 | 53 | ## Add configure headers for dynamic reconfigure 54 | add_dependencies(ExampleWaypointFlier 55 | ${PROJECT_NAME}_gencfg 56 | ) 57 | 58 | ## Specify libraries to link a library or executable target against 59 | target_link_libraries(ExampleWaypointFlier 60 | ${catkin_LIBRARIES} 61 | ${Eigen_LIBRARIES} 62 | ) 63 | 64 | ## -------------------------------------------------------------- 65 | ## | Install | 66 | ## -------------------------------------------------------------- 67 | 68 | install(TARGETS ${LIBRARIES} 69 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 70 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 71 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 72 | ) 73 | 74 | install(DIRECTORY launch config 75 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 76 | ) 77 | 78 | install(FILES nodelets.xml 79 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 80 | ) 81 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/README.md: -------------------------------------------------------------------------------- 1 | # ExampleWaypointFlier ROS example 2 | 3 | This package was created as an example of how to write ROS nodelets. 4 | The package is written in C++ and features custom MRS libraries and msgs. 5 | 6 | ## Functionality 7 | 8 | * Desired waypoints are loaded as a matrix from config file 9 | * Service `fly_to_first_waypoint` prepares the UAV by flying to the first waypoint 10 | * Service `start_waypoint_following` causes the UAV to start tracking the waypoints 11 | * Service `stop_waypoint_following` stops adding new waypoints. Flight to the current waypoint is not interrupted. 12 | 13 | ## How to start 14 | 15 | ```bash 16 | ./tmux/start.sh 17 | ``` 18 | 19 | Then, call the services prepared in the terminal window either by: 20 | 21 | 1. Pressing tmux binding (`Ctrl + b` or `Ctrl + a`) 22 | 2. Pressing the down arrow to change to the terminal below 23 | 3. Pressing the up arrow to bring up the prepared terminal command 24 | 25 | Or typing the following command into a terminal connected to the ROS server: 26 | ``` 27 | rosservice call /uav1/waypoint_flier_simple/start 28 | ``` 29 | 30 | ## Package structure 31 | 32 | See [ROS packages](http://wiki.ros.org/Packages) 33 | 34 | * `src` directory contains all source files 35 | * `include` directory contains all header files. It is good practice to separate them from source files. 36 | * `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) 37 | * `config` directory contains parameters in `.yaml` files. See [.yaml files](http://wiki.ros.org/rosparam) 38 | * `package.xml` defines properties of the package, such as package name and dependencies. See [package.xml](http://wiki.ros.org/catkin/package.xml) 39 | 40 | ## Example features 41 | 42 | * [Nodelet](http://wiki.ros.org/nodelet) initialization 43 | * [Subscriber, publisher](http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29), and [timer](http://wiki.ros.org/roscpp/Overview/Timers) initialization 44 | * [Service servers and clients](http://wiki.ros.org/roscpp/Overview/Services) initialization 45 | * Loading [parameters](http://wiki.ros.org/Parameter%20Server) with `mrs_lib::ParamLoader` class 46 | * Loading [Eigen matrices](https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html) with `mrs_lib::ParamLoader` class 47 | * Checking nodelet initialization status in every callback 48 | * Checking whether subscribed messages are coming 49 | * Throttling [text output](http://wiki.ros.org/roscpp/Overview/Logging) to a terminal 50 | * [Thread-safe access](https://en.cppreference.com/w/cpp/thread/mutex) to variables using `std::lock_scope()` 51 | * Using `ConstPtr` when subscribing to a topic to avoid copying large messages 52 | * Storing and accessing matrices in `Eigen` classes 53 | * [Remapping topics](http://wiki.ros.org/roslaunch/XML/remap) in the launch file 54 | 55 | ## Coding style 56 | 57 | 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. 58 | Also check out our general [C++ good/bad coding practices tutorial](https://ctu-mrs.github.io/docs/introduction/c_to_cpp.html). 59 | 60 | ### Naming variables 61 | 62 | * Member variables are distinguished from local variables by underscore at the end: 63 | - `position_x` - local variable 64 | - `position_x_` - member variable 65 | * Also, we distinguish parameters which are loaded as parameters by underscore at the beginning 66 | * Descriptive variable names are used. The purpose of the variable should be obvious from the name. 67 | - `sub_odom_uav_` - member subscriber to uav odometry msg type 68 | - `pub_reference_` - member publisher of reference msg type 69 | - `srv_server_start_waypoints_following_` - member service server for starting following of waypoints 70 | - `ExampleWaypointFlier::callbackTimerCheckSubscribers()` - callback of timer which checks subscribers 71 | - `mutex_odom_uav_` - mutex locking access to variable containing odometry of the UAV 72 | 73 | ### Good practices 74 | 75 | * [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. 76 | * Do not use raw pointers! Smart pointers from `` free resources automatically, thus preventing memory leaks. 77 | * Lock access to member variables! Nodelets are multi-thread processes, so it is our responsibility to make our code thread-safe. 78 | - Use `c++17` `scoped_lock` which unlocks the mutex after leaving the scope. This way, you can't forget to unlock the mutex. 79 | ```cpp 80 | { 81 | std::scoped_lock lock(mutex_odom_uav_); 82 | odom_uav_ = *msg; 83 | } 84 | ``` 85 | * Use `ros::Time::waitForValid()` after creating node handle `ros::NodeHandle nh("~")` 86 | * 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`. 87 | * 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. 88 | * 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. 89 | * 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. 90 | * 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. 91 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/config/dynparam.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE='example_waypoint_flier' 4 | import roslib; 5 | roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator(); 10 | params = gen.add_group("example_waypoint_flier parameters"); 11 | 12 | params.add("waypoint_idle_time", double_t, 1, "The time to wait before flying to the next waypoint", 1, 0, 60); 13 | 14 | exit(gen.generate(PACKAGE, "example_waypoint_flier", "dynparam")) 15 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/config/example_waypoint_flier.yaml: -------------------------------------------------------------------------------- 1 | # rates of timers in Hz 2 | rate: 3 | publish_dist_to_waypoint: 1 # [Hz] 4 | publish_reference: 10 # [Hz] 5 | check_subscribers: 1 # [Hz] 6 | 7 | # number of times the waypoint sequence should be repeated 8 | n_loops: 1 # [-] 9 | 10 | # should the UAV land after flying through all the waypoints? 11 | land_at_the_end: false 12 | 13 | # how long should the UAV idle at a waypoint? 14 | waypoint_idle_time: 1.0 15 | 16 | # how close should the uav get to the waypoint before considering it reached? 17 | waypoint_desired_distance: 0.3 18 | 19 | # matrix of waypoints which will be flown through by the UAV 20 | # x [m], y [m], z [m], heading [rad] 21 | waypoints: [-10, -10, 3, 0, 22 | 10, -10, 3, 1.57, 23 | 10, 10, 3, 3.14, 24 | -10, 10, 3, 4.61] 25 | 26 | # in which frame of reference are the points specified? 27 | waypoints_frame: "world_origin" 28 | 29 | # offset will be added to all waypoints 30 | # x [m], y [m], z [m], yaw [rad] 31 | offset: [0, 0, 0, 0] 32 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/launch/example_waypoint_flier.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ExampleWaypointFlier nodelet 4 | 5 | 6 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | example_waypoint_flier 5 | 1.0.0 6 | The Example Waypoint Flier 7 | 8 | Matej Petrlik 9 | Matej Petrlik 10 | 11 | MIT 12 | 13 | catkin 14 | 15 | roscpp 16 | nodelet 17 | geometry_msgs 18 | nav_msgs 19 | mrs_msgs 20 | mrs_lib 21 | dynamic_reconfigure 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/src/example_waypoint_flier.cpp: -------------------------------------------------------------------------------- 1 | /* includes //{ */ 2 | 3 | /* each ros package must have these */ 4 | #include 5 | #include 6 | #include 7 | 8 | /* for loading dynamic parameters while the nodelet is running */ 9 | #include 10 | 11 | /* this header file is created during compilation from python script dynparam.cfg */ 12 | #include 13 | 14 | /* for smart pointers (do not use raw pointers) */ 15 | #include 16 | 17 | /* for protecting variables from simultaneous by from multiple threads */ 18 | #include 19 | 20 | /* for writing and reading from streams */ 21 | #include 22 | #include 23 | 24 | /* for storing information about the state of the uav (position) */ 25 | #include 26 | #include 27 | 28 | /* for storing information about the state of the uav (position, twist) + covariances */ 29 | #include 30 | 31 | /* custom msgs of MRS group */ 32 | #include 33 | #include 34 | #include 35 | 36 | /* custom helper functions from our library */ 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | /* for calling simple ros services */ 45 | #include 46 | 47 | /* for operations with matrices */ 48 | #include 49 | 50 | //} 51 | 52 | using vec2_t = mrs_lib::geometry::vec_t<2>; 53 | using vec3_t = mrs_lib::geometry::vec_t<3>; 54 | 55 | namespace example_waypoint_flier 56 | { 57 | 58 | /* class ExampleWaypointFlier //{ */ 59 | 60 | class ExampleWaypointFlier : public nodelet::Nodelet { 61 | 62 | public: 63 | /* onInit() is called when nodelet is launched (similar to main() in regular node) */ 64 | virtual void onInit(); 65 | 66 | private: 67 | /* flags */ 68 | std::atomic is_initialized_ = false; 69 | 70 | /* ros parameters */ 71 | std::string _uav_name_; 72 | 73 | // | ---------------------- msg callbacks --------------------- | 74 | 75 | mrs_lib::SubscribeHandler sh_odometry_; 76 | mrs_lib::SubscribeHandler sh_control_manager_diag_; 77 | 78 | void callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnostics::ConstPtr msg); 79 | std::atomic have_goal_ = false; 80 | std::atomic waypoint_reached_ = false; 81 | 82 | // | --------------------- timer callbacks -------------------- | 83 | 84 | void timerPublishDistToWaypoint(const ros::TimerEvent& te); 85 | ros::Publisher pub_dist_to_waypoint_; 86 | ros::Timer timer_publish_dist_to_waypoint_; 87 | int _rate_timer_publish_dist_to_waypoint_; 88 | 89 | void timerPublishSetReference(const ros::TimerEvent& te); 90 | ros::Publisher pub_reference_; 91 | ros::Timer timer_publisher_reference_; 92 | int _rate_timer_publisher_reference_; 93 | 94 | void timerCheckSubscribers(const ros::TimerEvent& te); 95 | ros::Timer timer_check_subscribers_; 96 | int _rate_timer_check_subscribers_; 97 | 98 | // | ---------------- service server callbacks ---------------- | 99 | 100 | bool callbackStartWaypointFollowing(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); 101 | ros::ServiceServer srv_server_start_waypoints_following_; 102 | 103 | bool callbackStopWaypointFollowing(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); 104 | ros::ServiceServer srv_server_stop_waypoints_following_; 105 | 106 | bool callbackFlyToFirstWaypoint(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); 107 | ros::ServiceServer srv_server_fly_to_first_waypoint_; 108 | 109 | // | --------------------- service clients -------------------- | 110 | 111 | ros::ServiceClient srv_client_land_; 112 | bool _land_end_; 113 | 114 | // | -------------------- loading waypoints ------------------- | 115 | 116 | std::vector waypoints_; 117 | std::string _waypoints_frame_; 118 | bool waypoints_loaded_ = false; 119 | mrs_msgs::Reference current_waypoint_; 120 | std::mutex mutex_current_waypoint_; 121 | int idx_current_waypoint_; 122 | int n_waypoints_; 123 | int _n_loops_; 124 | int c_loop_; 125 | std::mutex mutex_waypoint_idle_time_; 126 | Eigen::MatrixXd _offset_; 127 | 128 | // | ------------------- dynamic reconfigure ------------------ | 129 | 130 | typedef example_waypoint_flier::dynparamConfig Config; 131 | typedef dynamic_reconfigure::Server ReconfigureServer; 132 | boost::recursive_mutex mutex_dynamic_reconfigure_; 133 | boost::shared_ptr reconfigure_server_; 134 | void callbackDynamicReconfigure(Config& config, uint32_t level); 135 | example_waypoint_flier::dynparamConfig last_drs_config_; 136 | 137 | // | --------------------- waypoint idling -------------------- | 138 | 139 | bool is_idling_ = false; 140 | ros::Timer timer_idling_; 141 | double _waypoint_idle_time_; 142 | double _waypoint_desired_dist_; 143 | void timerIdling(const ros::TimerEvent& te); 144 | 145 | // | -------------------- support functions ------------------- | 146 | 147 | std::vector matrixToPoints(const Eigen::MatrixXd& matrix); 148 | 149 | void offsetPoints(std::vector& points, const Eigen::MatrixXd& offset); 150 | 151 | double distance(const mrs_msgs::Reference& waypoint, const geometry_msgs::Pose& pose); 152 | }; 153 | 154 | //} 155 | 156 | /* onInit() //{ */ 157 | 158 | void ExampleWaypointFlier::onInit() { 159 | 160 | // | ---------------- set my booleans to false ---------------- | 161 | // but remember, always set them to their default value in the header file 162 | // because, when you add new one later, you might forger to come back here 163 | 164 | have_goal_ = false; 165 | is_idling_ = false; 166 | waypoints_loaded_ = false; 167 | 168 | /* obtain node handle */ 169 | ros::NodeHandle nh = nodelet::Nodelet::getMTPrivateNodeHandle(); 170 | 171 | /* waits for the ROS to publish clock */ 172 | ros::Time::waitForValid(); 173 | 174 | // | ------------------- load ros parameters ------------------ | 175 | /* (mrs_lib implementation checks whether the parameter was loaded or not) */ 176 | mrs_lib::ParamLoader param_loader(nh, "ExampleWaypointFlier"); 177 | 178 | param_loader.loadParam("uav_name", _uav_name_); 179 | param_loader.loadParam("land_at_the_end", _land_end_); 180 | param_loader.loadParam("n_loops", _n_loops_); 181 | param_loader.loadParam("waypoint_desired_distance", _waypoint_desired_dist_); 182 | param_loader.loadParam("waypoint_idle_time", _waypoint_idle_time_); 183 | param_loader.loadParam("waypoints_frame", _waypoints_frame_); 184 | param_loader.loadParam("rate/publish_dist_to_waypoint", _rate_timer_publish_dist_to_waypoint_); 185 | param_loader.loadParam("rate/publish_reference", _rate_timer_publisher_reference_); 186 | param_loader.loadParam("rate/check_subscribers", _rate_timer_check_subscribers_); 187 | 188 | /* load waypoints as a half-dynamic matrix from config file */ 189 | Eigen::MatrixXd waypoint_matrix; 190 | param_loader.loadMatrixDynamic("waypoints", waypoint_matrix, -1, 4); // -1 indicates the dynamic dimension 191 | waypoints_ = matrixToPoints(waypoint_matrix); 192 | n_waypoints_ = waypoints_.size(); 193 | waypoints_loaded_ = true; 194 | idx_current_waypoint_ = 0; 195 | c_loop_ = 0; 196 | ROS_INFO_STREAM_ONCE("[ExampleWaypointFlier]: " << n_waypoints_ << " waypoints loaded"); 197 | ROS_INFO_STREAM_ONCE("[ExampleWaypointFlier]: " << _n_loops_ << " loops requested"); 198 | 199 | /* load offset of all waypoints as a static matrix from config file */ 200 | param_loader.loadMatrixKnown("offset", _offset_, 1, 4); 201 | offsetPoints(waypoints_, _offset_); 202 | 203 | if (!param_loader.loadedSuccessfully()) { 204 | ROS_ERROR("[ExampleWaypointFlier]: failed to load non-optional parameters!"); 205 | ros::shutdown(); 206 | } 207 | 208 | // | ------------------ initialize subscribers ----------------- | 209 | 210 | mrs_lib::SubscribeHandlerOptions shopts; 211 | shopts.nh = nh; 212 | shopts.node_name = "ExampleWaypointFlier"; 213 | shopts.no_message_timeout = ros::Duration(1.0); 214 | shopts.threadsafe = true; 215 | shopts.autostart = true; 216 | shopts.queue_size = 10; 217 | shopts.transport_hints = ros::TransportHints().tcpNoDelay(); 218 | 219 | sh_odometry_ = mrs_lib::SubscribeHandler(shopts, "odom_uav_in"); 220 | sh_control_manager_diag_ = mrs_lib::SubscribeHandler(shopts, "control_manager_diagnostics_in", 221 | &ExampleWaypointFlier::callbackControlManagerDiag, this); 222 | 223 | // | ------------------ initialize publishers ----------------- | 224 | 225 | pub_dist_to_waypoint_ = nh.advertise("dist_to_waypoint_out", 1); 226 | pub_reference_ = nh.advertise("reference_out", 1); 227 | 228 | // | -------------------- initialize timers ------------------- | 229 | 230 | timer_publish_dist_to_waypoint_ = nh.createTimer(ros::Rate(_rate_timer_publish_dist_to_waypoint_), &ExampleWaypointFlier::timerPublishDistToWaypoint, this); 231 | 232 | timer_check_subscribers_ = nh.createTimer(ros::Rate(_rate_timer_check_subscribers_), &ExampleWaypointFlier::timerCheckSubscribers, this); 233 | 234 | // you can disable autostarting of the timer by the last argument 235 | timer_publisher_reference_ = nh.createTimer(ros::Rate(_rate_timer_publisher_reference_), &ExampleWaypointFlier::timerPublishSetReference, this, false, false); 236 | 237 | // | --------------- initialize service servers --------------- | 238 | 239 | srv_server_start_waypoints_following_ = nh.advertiseService("start_waypoints_following_in", &ExampleWaypointFlier::callbackStartWaypointFollowing, this); 240 | srv_server_stop_waypoints_following_ = nh.advertiseService("stop_waypoints_following_in", &ExampleWaypointFlier::callbackStopWaypointFollowing, this); 241 | srv_server_fly_to_first_waypoint_ = nh.advertiseService("fly_to_first_waypoint_in", &ExampleWaypointFlier::callbackFlyToFirstWaypoint, this); 242 | 243 | // | --------------- initialize service clients --------------- | 244 | 245 | srv_client_land_ = nh.serviceClient("land_out"); 246 | 247 | // | ---------- initialize dynamic reconfigure server --------- | 248 | 249 | reconfigure_server_.reset(new ReconfigureServer(mutex_dynamic_reconfigure_, nh)); 250 | ReconfigureServer::CallbackType f = boost::bind(&ExampleWaypointFlier::callbackDynamicReconfigure, this, _1, _2); 251 | reconfigure_server_->setCallback(f); 252 | 253 | /* set the default value of dynamic reconfigure server to the value of parameter with the same name */ 254 | { 255 | std::scoped_lock lock(mutex_waypoint_idle_time_); 256 | last_drs_config_.waypoint_idle_time = _waypoint_idle_time_; 257 | } 258 | reconfigure_server_->updateConfig(last_drs_config_); 259 | 260 | ROS_INFO_ONCE("[ExampleWaypointFlier]: initialized"); 261 | 262 | is_initialized_ = true; 263 | } 264 | 265 | //} 266 | 267 | // | ---------------------- msg callbacks --------------------- | 268 | 269 | /* callbackControlManagerDiag() //{ */ 270 | 271 | void ExampleWaypointFlier::callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnostics::ConstPtr diagnostics) { 272 | 273 | /* do not continue if the nodelet is not initialized */ 274 | if (!is_initialized_) { 275 | return; 276 | } 277 | 278 | ROS_INFO_ONCE("[ExampleWaypointFlier]: Received first control manager diagnostics msg"); 279 | 280 | // get the variable under the mutex 281 | mrs_msgs::Reference current_waypoint = mrs_lib::get_mutexed(mutex_current_waypoint_, current_waypoint_); 282 | 283 | // extract the pose part of the odometry 284 | geometry_msgs::Pose current_pose = mrs_lib::getPose(sh_odometry_.getMsg()); 285 | 286 | double dist = distance(current_waypoint, current_pose); 287 | ROS_INFO("[ExampleWaypointFlier]: Distance to waypoint: %.2f", dist); 288 | 289 | if (have_goal_ && !diagnostics->tracker_status.have_goal) { 290 | have_goal_ = false; 291 | 292 | if (dist < _waypoint_desired_dist_) { 293 | waypoint_reached_ = true; 294 | ROS_INFO("[ExampleWaypointFlier]: Waypoint reached."); 295 | 296 | /* start idling at the reached waypoint */ 297 | is_idling_ = true; 298 | 299 | ros::NodeHandle nh("~"); 300 | timer_idling_ = nh.createTimer(ros::Duration(_waypoint_idle_time_), &ExampleWaypointFlier::timerIdling, this, 301 | true); // the last boolean argument makes the timer run only once 302 | 303 | ROS_INFO("[ExampleWaypointFlier]: Idling for %.2f seconds.", _waypoint_idle_time_); 304 | } 305 | } 306 | } 307 | 308 | //} 309 | 310 | // | --------------------- timer callbacks -------------------- | 311 | 312 | /* timerPublishSetReference() //{ */ 313 | 314 | void ExampleWaypointFlier::timerPublishSetReference([[maybe_unused]] const ros::TimerEvent& te) { 315 | 316 | if (!is_initialized_) { 317 | return; 318 | } 319 | 320 | /* return if the uav is still flying to the previous waypoints */ 321 | if (have_goal_) { 322 | return; 323 | } 324 | 325 | /* return if the UAV is idling at a waypoint */ 326 | if (is_idling_) { 327 | return; 328 | } 329 | 330 | /* shutdown node after flying through all the waypoints (call land service before) */ 331 | if (idx_current_waypoint_ >= n_waypoints_) { 332 | 333 | c_loop_++; 334 | 335 | ROS_INFO("[ExampleWaypointFlier]: Finished loop %d/%d", c_loop_, _n_loops_); 336 | 337 | if (c_loop_ >= _n_loops_) { 338 | 339 | ROS_INFO("[ExampleWaypointFlier]: Finished %d loops of %d waypoints.", _n_loops_, n_waypoints_); 340 | 341 | if (_land_end_) { 342 | ROS_INFO("[ExampleWaypointFlier]: Calling land service."); 343 | std_srvs::Trigger srv_land_call; 344 | srv_client_land_.call(srv_land_call); 345 | } 346 | 347 | ROS_INFO("[ExampleWaypointFlier]: Shutting down."); 348 | ros::shutdown(); 349 | return; 350 | 351 | } else { 352 | ROS_INFO("[ExampleWaypointFlier]: Starting loop %d/%d", c_loop_ + 1, _n_loops_); 353 | idx_current_waypoint_ = 0; 354 | } 355 | } 356 | 357 | /* create new waypoint msg */ 358 | mrs_msgs::ReferenceStamped new_waypoint; 359 | 360 | // set the frame id in which the reference is expressed 361 | new_waypoint.header.frame_id = _uav_name_ + "/" + _waypoints_frame_; 362 | new_waypoint.header.stamp = ros::Time::now(); 363 | 364 | new_waypoint.reference = waypoints_.at(idx_current_waypoint_); 365 | 366 | // set the variable under the mutex 367 | mrs_lib::set_mutexed(mutex_current_waypoint_, waypoints_.at(idx_current_waypoint_), current_waypoint_); 368 | 369 | ROS_INFO("[ExampleWaypointFlier]: Flying to waypoint %d: x: %.2f y: %.2f z: %.2f heading: %.2f", idx_current_waypoint_ + 1, new_waypoint.reference.position.x, 370 | new_waypoint.reference.position.y, new_waypoint.reference.position.z, new_waypoint.reference.heading); 371 | 372 | try { 373 | pub_reference_.publish(new_waypoint); 374 | } 375 | catch (...) { 376 | ROS_ERROR("Exception caught during publishing topic %s.", pub_reference_.getTopic().c_str()); 377 | } 378 | 379 | if (waypoint_reached_) { 380 | idx_current_waypoint_++; 381 | waypoint_reached_ = false; 382 | } 383 | 384 | have_goal_ = true; 385 | } 386 | 387 | //} 388 | 389 | /* timerPublishDistToWaypoint() //{ */ 390 | 391 | void ExampleWaypointFlier::timerPublishDistToWaypoint([[maybe_unused]] const ros::TimerEvent& te) { 392 | 393 | if (!is_initialized_) { 394 | return; 395 | } 396 | 397 | /* do not publish distance to waypoint when the uav is not flying towards a waypoint */ 398 | if (!have_goal_) { 399 | return; 400 | } 401 | 402 | // this routine can not work without the odometry 403 | if (!sh_odometry_.hasMsg()) { 404 | return; 405 | } 406 | 407 | // get the variable under the mutex 408 | mrs_msgs::Reference current_waypoint = mrs_lib::get_mutexed(mutex_current_waypoint_, current_waypoint_); 409 | 410 | // extract the pose part of the odometry 411 | geometry_msgs::Pose current_pose = mrs_lib::getPose(sh_odometry_.getMsg()); 412 | 413 | double dist = distance(current_waypoint, current_pose); 414 | ROS_INFO("[ExampleWaypointFlier]: Distance to waypoint: %.2f", dist); 415 | 416 | mrs_msgs::Float64Stamped dist_msg; 417 | // it is important to set the frame id correctly !! 418 | dist_msg.header.frame_id = _uav_name_ + "/" + _waypoints_frame_; 419 | dist_msg.header.stamp = ros::Time::now(); 420 | dist_msg.value = dist; 421 | 422 | try { 423 | pub_dist_to_waypoint_.publish(dist_msg); 424 | } 425 | catch (...) { 426 | ROS_ERROR("Exception caught during publishing topic %s.", pub_dist_to_waypoint_.getTopic().c_str()); 427 | } 428 | } 429 | 430 | //} 431 | 432 | /* timerCheckSubscribers() //{ */ 433 | 434 | void ExampleWaypointFlier::timerCheckSubscribers([[maybe_unused]] const ros::TimerEvent& te) { 435 | 436 | if (!is_initialized_) { 437 | return; 438 | } 439 | 440 | if (!sh_odometry_.hasMsg()) { 441 | ROS_WARN_THROTTLE(1.0, "[ExampleWaypointFlier]: Not received uav odom msg since node launch."); 442 | } 443 | 444 | if (!sh_control_manager_diag_.hasMsg()) { 445 | ROS_WARN_THROTTLE(1.0, "[ExampleWaypointFlier]: Not received tracker diagnostics msg since node launch."); 446 | } 447 | } 448 | 449 | //} 450 | 451 | /* timerIdling() //{ */ 452 | 453 | void ExampleWaypointFlier::timerIdling([[maybe_unused]] const ros::TimerEvent& te) { 454 | 455 | ROS_INFO("[ExampleWaypointFlier]: Idling finished"); 456 | is_idling_ = false; 457 | } 458 | 459 | //} 460 | 461 | // | -------------------- service callbacks ------------------- | 462 | 463 | /* //{ callbackStartWaypointFollowing() */ 464 | 465 | bool ExampleWaypointFlier::callbackStartWaypointFollowing([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) { 466 | 467 | if (!is_initialized_) { 468 | 469 | res.success = false; 470 | res.message = "Waypoint flier not initialized!"; 471 | ROS_WARN("[ExampleWaypointFlier]: Cannot start waypoint following, nodelet is not initialized."); 472 | return true; 473 | } 474 | 475 | if (waypoints_loaded_) { 476 | 477 | timer_publisher_reference_.start(); 478 | 479 | ROS_INFO("[ExampleWaypointFlier]: Starting waypoint following."); 480 | 481 | res.success = true; 482 | res.message = "Starting waypoint following."; 483 | 484 | } else { 485 | 486 | ROS_WARN("[ExampleWaypointFlier]: Cannot start waypoint following, waypoints are not set."); 487 | res.success = false; 488 | res.message = "Waypoints not set."; 489 | } 490 | 491 | return true; 492 | } 493 | 494 | //} 495 | 496 | /* //{ callbackStopWaypointFollowing() */ 497 | 498 | bool ExampleWaypointFlier::callbackStopWaypointFollowing([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) { 499 | 500 | if (!is_initialized_) { 501 | 502 | res.success = false; 503 | res.message = "Waypoint flier not initialized!"; 504 | ROS_WARN("[ExampleWaypointFlier]: Cannot stop waypoint following, nodelet is not initialized."); 505 | return true; 506 | } 507 | 508 | timer_publisher_reference_.stop(); 509 | 510 | ROS_INFO("[ExampleWaypointFlier]: Waypoint following stopped."); 511 | 512 | res.success = true; 513 | res.message = "Waypoint following stopped."; 514 | 515 | return true; 516 | } 517 | 518 | //} 519 | 520 | /* //{ callbackFlyToFirstWaypoint() */ 521 | 522 | bool ExampleWaypointFlier::callbackFlyToFirstWaypoint([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) { 523 | 524 | if (!is_initialized_) { 525 | 526 | res.success = false; 527 | res.message = "Waypoint flier not initialized!"; 528 | ROS_WARN("[ExampleWaypointFlier]: Cannot start waypoint following, nodelet is not initialized."); 529 | 530 | return true; 531 | } 532 | 533 | if (waypoints_loaded_) { 534 | 535 | /* create new waypoint msg */ 536 | mrs_msgs::ReferenceStamped new_waypoint; 537 | 538 | // it is important to set the frame id correctly !! 539 | new_waypoint.header.frame_id = _uav_name_ + "/" + _waypoints_frame_; 540 | new_waypoint.header.stamp = ros::Time::now(); 541 | new_waypoint.reference = waypoints_.at(0); 542 | 543 | mrs_lib::set_mutexed(mutex_current_waypoint_, waypoints_.at(0), current_waypoint_); 544 | 545 | // set the variable under the mutex 546 | 547 | idx_current_waypoint_ = 0; 548 | c_loop_ = 0; 549 | 550 | have_goal_ = true; 551 | 552 | try { 553 | pub_reference_.publish(new_waypoint); 554 | } 555 | catch (...) { 556 | ROS_ERROR("Exception caught during publishing topic %s.", pub_reference_.getTopic().c_str()); 557 | } 558 | 559 | std::stringstream ss; 560 | ss << "Flying to first waypoint: x: " << new_waypoint.reference.position.x << ", y: " << new_waypoint.reference.position.y 561 | << ", z: " << new_waypoint.reference.position.z << ", heading: " << new_waypoint.reference.heading; 562 | 563 | ROS_INFO_STREAM_THROTTLE(1.0, "[ExampleWaypointFlier]: " << ss.str()); 564 | 565 | res.success = true; 566 | res.message = ss.str(); 567 | 568 | } else { 569 | 570 | ROS_WARN("[ExampleWaypointFlier]: Cannot fly to first waypoint, waypoints not loaded!"); 571 | 572 | res.success = false; 573 | res.message = "Waypoints not loaded"; 574 | } 575 | 576 | return true; 577 | } 578 | 579 | //} 580 | 581 | // | -------------- dynamic reconfigure callback -------------- | 582 | 583 | /* //{ callbackDynamicReconfigure() */ 584 | 585 | void ExampleWaypointFlier::callbackDynamicReconfigure([[maybe_unused]] Config& config, [[maybe_unused]] uint32_t level) { 586 | 587 | if (!is_initialized_) 588 | return; 589 | 590 | ROS_INFO( 591 | "[ExampleWaypointFlier]:" 592 | "Reconfigure Request: " 593 | "Waypoint idle time: %.2f", 594 | config.waypoint_idle_time); 595 | 596 | { 597 | std::scoped_lock lock(mutex_waypoint_idle_time_); 598 | 599 | _waypoint_idle_time_ = config.waypoint_idle_time; 600 | } 601 | } 602 | 603 | //} 604 | 605 | // | -------------------- support functions ------------------- | 606 | 607 | /* matrixToPoints() //{ */ 608 | 609 | std::vector ExampleWaypointFlier::matrixToPoints(const Eigen::MatrixXd& matrix) { 610 | 611 | std::vector points; 612 | 613 | for (int i = 0; i < matrix.rows(); i++) { 614 | 615 | mrs_msgs::Reference point; 616 | point.position.x = matrix.row(i)(0); 617 | point.position.y = matrix.row(i)(1); 618 | point.position.z = matrix.row(i)(2); 619 | point.heading = matrix.row(i)(3); 620 | 621 | points.push_back(point); 622 | } 623 | 624 | return points; 625 | } 626 | 627 | //} 628 | 629 | /* offsetPoints() //{ */ 630 | 631 | void ExampleWaypointFlier::offsetPoints(std::vector& points, const Eigen::MatrixXd& offset) { 632 | 633 | for (size_t i = 0; i < points.size(); i++) { 634 | 635 | points.at(i).position.x += offset(0); 636 | points.at(i).position.y += offset(1); 637 | points.at(i).position.z += offset(2); 638 | points.at(i).heading += offset(3); 639 | } 640 | } 641 | 642 | //} 643 | 644 | /* distance() //{ */ 645 | 646 | double ExampleWaypointFlier::distance(const mrs_msgs::Reference& waypoint, const geometry_msgs::Pose& pose) { 647 | 648 | return mrs_lib::geometry::dist(vec3_t(waypoint.position.x, waypoint.position.y, waypoint.position.z), 649 | vec3_t(pose.position.x, pose.position.y, pose.position.z)); 650 | } 651 | 652 | //} 653 | 654 | } // namespace example_waypoint_flier 655 | 656 | /* every nodelet must include macros which export the class as a nodelet plugin */ 657 | #include 658 | PLUGINLIB_EXPORT_CLASS(example_waypoint_flier::ExampleWaypointFlier, nodelet::Nodelet); 659 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/tmux/config/automatic_start.yaml: -------------------------------------------------------------------------------- 1 | # A timeout between the takeoff being triggered and the UAV actually taking off 2 | # while the timeout is counting down, the takeoff can be aborted by switching off 3 | # the offboard mode. 4 | # default = 5 sec 5 | safety_timeout: 1.0 # [s] 6 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/tmux/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | # GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: 2 | ## -------------------------------------------------------------- 3 | ## | rosrun mrs_uav_core get_public_params.py # 4 | ## -------------------------------------------------------------- 5 | 6 | mrs_uav_managers: 7 | 8 | estimation_manager: 9 | 10 | # loaded state estimator plugins 11 | state_estimators: [ 12 | "gps_garmin", 13 | "gps_baro", 14 | ] 15 | 16 | initial_state_estimator: "gps_baro" # will be used as the first state estimator 17 | agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) 18 | 19 | uav_manager: 20 | 21 | takeoff: 22 | 23 | during_takeoff: 24 | controller: "MpcController" 25 | tracker: "LandoffTracker" 26 | 27 | after_takeoff: 28 | controller: "Se3Controller" 29 | tracker: "MpcTracker" 30 | 31 | midair_activation: 32 | 33 | during_activation: 34 | controller: "MidairActivationController" 35 | tracker: "MidairActivationTracker" 36 | 37 | after_activation: 38 | controller: "Se3Controller" 39 | tracker: "MpcTracker" 40 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/tmux/config/hw_api.yaml: -------------------------------------------------------------------------------- 1 | input_mode: 2 | actuators: false 3 | control_group: false 4 | attitude_rate: true 5 | attitude: true 6 | acceleration_hdg_rate: false 7 | acceleration_hdg: false 8 | velocity_hdg_rate: false 9 | velocity_hdg: false 10 | position: false 11 | 12 | feedforward: true 13 | 14 | outputs: 15 | distance_sensor: true 16 | gnss: true 17 | rtk: true 18 | imu: true 19 | altitude: true 20 | magnetometer_heading: true 21 | rc_channels: true 22 | battery_state: true 23 | position: true 24 | orientation: true 25 | velocity: true 26 | angular_velocity: true 27 | odometry: true 28 | ground_truth: true 29 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/tmux/config/network_config.yaml: -------------------------------------------------------------------------------- 1 | # 1. This list is used by NimbroNetwork for mutual communication of the UAVs 2 | # The names of the robots have to match hostnames described in /etc/hosts. 3 | # 4 | # 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. 5 | # The names should match the true "UAV_NAMES" (the topic prefixes). 6 | # 7 | # network_config:=~/config/network_config.yaml 8 | # 9 | # to the core.launch and nimbro.launch. 10 | 11 | network: 12 | 13 | robot_names: [ 14 | 15 | uav1, 16 | uav2, 17 | uav3, 18 | uav4, 19 | 20 | ] 21 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/tmux/config/simulator.yaml: -------------------------------------------------------------------------------- 1 | simulation_rate: 250 2 | clock_rate: 250 3 | realtime_factor: 1.0 4 | 5 | iterate_without_input: true 6 | 7 | individual_takeoff_platform: 8 | enabled: false 9 | 10 | uav_names: [ 11 | "uav1", 12 | ] 13 | 14 | uav1: 15 | type: "x500" 16 | spawn: 17 | x: 10.0 18 | y: 15.0 19 | z: 2.0 20 | heading: 0 21 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/tmux/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: true 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/tmux/kill.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Absolute path to this script. /home/user/bin/foo.sh 4 | SCRIPT=$(readlink -f $0) 5 | # Absolute path this script is in. /home/user/bin 6 | SCRIPTPATH=`dirname $SCRIPT` 7 | cd "$SCRIPTPATH" 8 | 9 | export TMUX_SESSION_NAME=simulation 10 | export TMUX_SOCKET_NAME=mrs 11 | 12 | # just attach to the session 13 | tmux -L $TMUX_SOCKET_NAME split-window -t $TMUX_SESSION_NAME 14 | 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 15 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/tmux/layout.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "border": "normal", 4 | "floating": "auto_off", 5 | "fullscreen_mode": 0, 6 | "layout": "splitv", 7 | "percent": 0.285714285714286, 8 | "type": "con", 9 | "nodes": [ 10 | { 11 | "border": "pixel", 12 | "current_border_width": 3, 13 | "floating": "auto_off", 14 | "geometry": { 15 | "height": 1030, 16 | "width": 936, 17 | "x": 4806, 18 | "y": 32 19 | }, 20 | "name": "rviz.rviz* - RViz", 21 | "percent": 0.5, 22 | "swallows": [ 23 | { 24 | "instance": "^rviz$" 25 | } 26 | ], 27 | "type": "con" 28 | }, 29 | { 30 | "border": "pixel", 31 | "current_border_width": 3, 32 | "floating": "auto_off", 33 | "geometry": { 34 | "height": 460, 35 | "width": 724, 36 | "x": 0, 37 | "y": 0 38 | }, 39 | "name": "./start.sh", 40 | "percent": 0.5, 41 | "swallows": [ 42 | { 43 | "instance": "^urxvt$" 44 | } 45 | ], 46 | "type": "con" 47 | } 48 | ] 49 | } 50 | ] 51 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/tmux/session.yml: -------------------------------------------------------------------------------- 1 | # do not modify these 2 | root: ./ 3 | name: simulation 4 | socket_name: mrs 5 | attach: false 6 | tmux_options: -f /etc/ctu-mrs/tmux.conf 7 | # you can modify these 8 | pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500 9 | startup_window: example_waypoint_flier 10 | windows: 11 | - roscore: 12 | layout: tiled 13 | panes: 14 | - roscore 15 | - simulator: 16 | layout: tiled 17 | panes: 18 | - waitForRos; roslaunch mrs_multirotor_simulator multirotor_simulator.launch custom_config:=./config/simulator.yaml 19 | - hw_api: 20 | layout: tiled 21 | panes: 22 | - waitForTime; roslaunch mrs_multirotor_simulator hw_api.launch custom_config:=./config/hw_api.yaml 23 | - takeoff: 24 | layout: tiled 25 | panes: 26 | - waitForHw; roslaunch mrs_uav_autostart automatic_start.launch custom_config:=./config/automatic_start.yaml 27 | - waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard 28 | - status: 29 | layout: tiled 30 | panes: 31 | - waitForHw; roslaunch mrs_uav_status status.launch 32 | - control: 33 | layout: tiled 34 | panes: 35 | - waitForHw; roslaunch mrs_uav_core core.launch 36 | platform_config:=`rospack find mrs_multirotor_simulator`/config/mrs_uav_system/$UAV_TYPE.yaml 37 | custom_config:=./config/custom_config.yaml 38 | world_config:=./config/world_config.yaml 39 | network_config:=./config/network_config.yaml 40 | - example_waypoint_flier: 41 | layout: tiled 42 | panes: 43 | - waitForControl; roslaunch example_waypoint_flier example_waypoint_flier.launch 44 | - 'history -s rosservice call /$UAV_NAME/example_waypoint_flier/fly_to_first_waypoint' 45 | - 'history -s rosservice call /$UAV_NAME/example_waypoint_flier/start_waypoints_following' 46 | - 'history -s rosservice call /$UAV_NAME/example_waypoint_flier/stop_waypoints_following' 47 | - rviz: 48 | layout: tiled 49 | panes: 50 | - waitForControl; roslaunch mrs_uav_core rviz.launch 51 | - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch 52 | - waitForControl; waitForControl; roslaunch mrs_rviz_plugins rviz_interface.launch 53 | - layout: 54 | layout: tiled 55 | panes: 56 | - waitForControl; sleep 5; ~/.i3/layout_manager.sh ./layout.json 57 | -------------------------------------------------------------------------------- /cpp/waypoint_flier/tmux/start.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Absolute path to this script. /home/user/bin/foo.sh 4 | SCRIPT=$(readlink -f $0) 5 | # Absolute path this script is in. /home/user/bin 6 | SCRIPTPATH=`dirname $SCRIPT` 7 | cd "$SCRIPTPATH" 8 | 9 | export TMUX_SESSION_NAME=simulation 10 | export TMUX_SOCKET_NAME=mrs 11 | 12 | # start tmuxinator 13 | tmuxinator start -p ./session.yml 14 | 15 | # if we are not in tmux 16 | if [ -z $TMUX ]; then 17 | 18 | # just attach to the session 19 | tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME 20 | 21 | # if we are in tmux 22 | else 23 | 24 | # switch to the newly-started session 25 | tmux detach-client -E "tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME" 26 | 27 | fi 28 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15.0) 2 | project(example_waypoint_flier_simple) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | 7 | set(CATKIN_DEPENDENCIES 8 | cmake_modules 9 | roscpp 10 | nodelet 11 | nav_msgs 12 | mrs_msgs 13 | geometry_msgs 14 | ) 15 | 16 | set(LIBRARIES 17 | WaypointFlierSimple 18 | ) 19 | 20 | find_package(catkin REQUIRED COMPONENTS 21 | ${CATKIN_DEPENDENCIES} 22 | ) 23 | 24 | catkin_package( 25 | LIBRARIES ${LIBRARIES} 26 | CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} 27 | ) 28 | 29 | include_directories( 30 | ${catkin_INCLUDE_DIRS} 31 | ) 32 | 33 | # WaypointFlierSimple 34 | 35 | ## Declare a C++ library 36 | add_library(WaypointFlierSimple 37 | src/waypoint_flier_simple.cpp 38 | ) 39 | 40 | add_dependencies(WaypointFlierSimple 41 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 42 | ${catkin_EXPORTED_TARGETS} 43 | ) 44 | 45 | ## Specify libraries to link a library or executable target against 46 | target_link_libraries(WaypointFlierSimple 47 | ${catkin_LIBRARIES} 48 | ) 49 | 50 | ## -------------------------------------------------------------- 51 | ## | Install | 52 | ## -------------------------------------------------------------- 53 | 54 | install(TARGETS ${LIBRARIES} 55 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 58 | ) 59 | 60 | install(DIRECTORY launch config 61 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 62 | ) 63 | 64 | install(FILES nodelets.xml 65 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 66 | ) 67 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/README.md: -------------------------------------------------------------------------------- 1 | # WaypointFlier Simple - ROS simple example 2 | 3 | This package was created as an example of how to write a very simple ROS nodelet. 4 | You can test the program in simulation (see our [simulation tutorial](https://ctu-mrs.github.io/docs/simulation/howto.html)). 5 | 6 | ## Functionality 7 | 8 | * Once activated, the nodelet will command an UAV to fly through random waypoints 9 | * Service `start_waypoint_following` will activate the nodelet 10 | * The area in which random waypoints are generated is configurable with a separate config file See [.yaml files](http://wiki.ros.org/rosparam) 11 | 12 | ## How to start 13 | 14 | ```bash 15 | ./tmux/start.sh 16 | ``` 17 | 18 | Then, call the services prepared in the terminal window either by: 19 | 20 | 1. Pressing tmux binding (`Ctrl + b` or `Ctrl + a`) 21 | 2. Pressing the down arrow to change to the terminal below 22 | 3. Pressing the up arrow to bring up the prepared terminal command 23 | 24 | Or typing the following command into a terminal connected to the ROS server: 25 | ``` 26 | rosservice call /uav1/waypoint_flier_simple/start 27 | ``` 28 | 29 | ## Package structure 30 | 31 | See [ROS packages](http://wiki.ros.org/Packages) 32 | 33 | * `src` directory contains all source files 34 | * `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) 35 | * `config` directory contains parameters in `.yaml` files. See [.yaml files](http://wiki.ros.org/rosparam) 36 | * `package.xml` defines properties of the package, such as package name and dependencies. See [package.xml](http://wiki.ros.org/catkin/package.xml) 37 | 38 | ## More complex example 39 | 40 | * To see a similar node with more functionality and features, see [example_ros_uav](https://github.com/ctu-mrs/example_ros_uav) 41 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/config/waypoint_flier.yaml: -------------------------------------------------------------------------------- 1 | max_x: 30 # [m] 2 | max_y: 50 # [m] 3 | max_z: 10 # [m] 4 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/launch/waypoint_flier_simple.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | WaypointFlierSimple nodelet 4 | 5 | 6 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | example_waypoint_flier_simple 5 | 1.0.0 6 | Example for the Simple Waypoint Flier 7 | 8 | Daniel Hert 9 | Daniel Hert 10 | 11 | MIT 12 | 13 | catkin 14 | 15 | cmake_modules 16 | roscpp 17 | nodelet 18 | geometry_msgs 19 | nav_msgs 20 | mrs_msgs 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/src/waypoint_flier_simple.cpp: -------------------------------------------------------------------------------- 1 | /* includes //{ */ 2 | 3 | /* each ros package must have these */ 4 | #include 5 | #include 6 | #include 7 | 8 | /* for storing information about the state of the uav (position, twist) + covariances */ 9 | #include 10 | 11 | /* custom msgs of MRS group */ 12 | #include 13 | 14 | /* for calling simple ros services */ 15 | #include 16 | 17 | #include 18 | //} 19 | 20 | namespace example_waypoint_flier_simple 21 | { 22 | 23 | /* class WaypointFlierSimple //{ */ 24 | 25 | class WaypointFlierSimple : public nodelet::Nodelet { 26 | 27 | public: 28 | /* onInit() is called when nodelet is launched (similar to main() in regular node) */ 29 | virtual void onInit(); 30 | 31 | private: 32 | // | -------------------------- flags ------------------------- | 33 | 34 | /* is set to true when the nodelet is initialized, useful for rejecting callbacks that are called before the node is initialized */ 35 | std::atomic is_initialized_ = false; 36 | 37 | /* by default, the nodelet is deactivated, it only starts publishing goals when activated */ 38 | std::atomic active_ = false; 39 | 40 | /* by default, the nodelet is deactivated, it only starts publishing goals when activated */ 41 | std::atomic have_odom_ = false; 42 | 43 | /* variables to store the coordinates of the current goal */ 44 | double goal_x_ = 0.0; 45 | double goal_y_ = 0.0; 46 | double goal_z_ = 2.0; 47 | 48 | /* variables to store the maximum limit for the random waypoint generator */ 49 | double max_x_; 50 | double max_y_; 51 | double max_z_; 52 | 53 | /* ROS messages which store the current reference and odometry */ 54 | mrs_msgs::ReferenceStamped ref_; 55 | nav_msgs::Odometry current_odom_; 56 | 57 | // | ---------------------- ROS subscribers --------------------- | 58 | ros::Subscriber sub_odom_; 59 | void callbackOdom(const nav_msgs::Odometry& msg); 60 | 61 | // | ---------------------- ROS publishers --------------------- | 62 | ros::Publisher pub_reference_; 63 | 64 | // | ---------------------- ROS timers --------------------- | 65 | 66 | ros::Timer main_timer_; 67 | void callbackMainTimer([[maybe_unused]] const ros::TimerEvent& te); 68 | 69 | // | ---------------------- ROS service servers --------------------- | 70 | 71 | bool callbackStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res); 72 | ros::ServiceServer srv_server_start_; 73 | 74 | // | ------------------ Additional functions ------------------ | 75 | 76 | double distance(const mrs_msgs::ReferenceStamped& waypoint, const nav_msgs::Odometry& odom); 77 | 78 | double getRandomDouble(double min, double max); 79 | }; 80 | //} 81 | 82 | /* onInit() //{ */ 83 | 84 | void WaypointFlierSimple::onInit() { 85 | 86 | /* obtain node handle */ 87 | ros::NodeHandle nh = nodelet::Nodelet::getMTPrivateNodeHandle(); 88 | 89 | /* initialize random number generator */ 90 | srand(time(NULL)); 91 | 92 | /* waits for the ROS to publish clock */ 93 | ros::Time::waitForValid(); 94 | 95 | // | ------------------- load ros parameters ------------------ | 96 | nh.param("max_x", max_x_, 10.0); 97 | nh.param("max_y", max_y_, 10.0); 98 | nh.param("max_z", max_z_, 5.0); 99 | 100 | // | -------- initialize a subscriber for UAV odometry -------- | 101 | sub_odom_ = nh.subscribe("odom_in", 10, &WaypointFlierSimple::callbackOdom, this, ros::TransportHints().tcpNoDelay()); 102 | 103 | // | -------- initialize a publisher for UAV reference -------- | 104 | pub_reference_ = nh.advertise("reference_out", 1); 105 | 106 | // | -- initialize the main timer - main loop of the nodelet -- | 107 | main_timer_ = nh.createTimer(ros::Rate(10), &WaypointFlierSimple::callbackMainTimer, this); 108 | 109 | // | ---------------- initialize service server --------------- | 110 | srv_server_start_ = nh.advertiseService("start", &WaypointFlierSimple::callbackStart, this); 111 | is_initialized_ = true; 112 | } 113 | 114 | //} 115 | 116 | // | ---------------------- msg callbacks --------------------- | 117 | 118 | /* callbackOdom() //{ */ 119 | 120 | 121 | void WaypointFlierSimple::callbackOdom(const nav_msgs::Odometry& msg) { 122 | 123 | /* do not continue if the nodelet is not initialized */ 124 | if (!is_initialized_) { 125 | return; 126 | } 127 | // | -------------- save the current UAV odometry ------------- | 128 | current_odom_ = msg; 129 | have_odom_ = true; 130 | } 131 | 132 | //} 133 | 134 | // | --------------------- timer callbacks -------------------- | 135 | 136 | /* callbackMainTimer() //{ */ 137 | 138 | void WaypointFlierSimple::callbackMainTimer([[maybe_unused]] const ros::TimerEvent& te) { 139 | 140 | if (!active_) { 141 | 142 | ROS_INFO_THROTTLE(1.0, "[WaypointFlierSimple]: Waiting for activation"); 143 | 144 | } else { 145 | 146 | // calculate the distance to the current reference 147 | double dist_to_ref = distance(ref_, current_odom_); 148 | ROS_INFO_STREAM_THROTTLE(1.0, "[WaypointFlierSimple]: Distance to reference: " << dist_to_ref); 149 | 150 | // if the distance is less than 1 meter, generate a new reference 151 | if (dist_to_ref < 1.0) { 152 | 153 | ROS_INFO_STREAM("[WaypointFlierSimple]: Goal reached!"); 154 | 155 | // generate new goal 156 | goal_x_ = getRandomDouble(-max_x_, max_x_); 157 | goal_y_ = getRandomDouble(-max_y_, max_y_); 158 | goal_z_ = getRandomDouble(2.0, max_z_); 159 | 160 | ROS_INFO_STREAM("[WaypointFlierSimple]: New goal X: " << goal_x_ << " Y: " << goal_y_ << " Z: " << goal_z_); 161 | } 162 | 163 | // fill out the ROS message and publish it 164 | ref_.reference.position.x = goal_x_; 165 | ref_.reference.position.y = goal_y_; 166 | ref_.reference.position.z = goal_z_; 167 | ref_.reference.heading = 0.0; 168 | pub_reference_.publish(ref_); 169 | } 170 | } 171 | 172 | //} 173 | 174 | // | -------------------- service callbacks ------------------- | 175 | 176 | /* //{ callbackStart() */ 177 | 178 | bool WaypointFlierSimple::callbackStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) { 179 | 180 | // | ------------------- activation service ------------------- | 181 | // only activates the main loop when the nodelet is initialized and receiving odometry 182 | 183 | if (!is_initialized_) { 184 | 185 | res.success = false; 186 | res.message = "Waypoint flier not initialized!"; 187 | ROS_WARN("[WaypointFlierSimple]: Cannot start waypoint following, nodelet is not initialized."); 188 | return true; 189 | } 190 | 191 | if (!have_odom_) { 192 | 193 | res.success = false; 194 | res.message = "Waypoint flier is not receiving odometry!"; 195 | ROS_WARN("[WaypointFlierSimple]: Cannot start, nodelet is not receiving odometry!"); 196 | return true; 197 | } 198 | 199 | active_ = true; 200 | 201 | ROS_INFO("[WaypointFlierSimple]: Starting waypoint following."); 202 | ROS_INFO_STREAM("[WaypointFlierSimple]: Goal X: " << goal_x_ << " Y: " << goal_y_ << " Z: " << goal_z_); 203 | 204 | res.success = true; 205 | res.message = "Starting waypoint following."; 206 | 207 | return true; 208 | } 209 | 210 | //} 211 | 212 | // | -------------------- support functions ------------------- | 213 | 214 | /* distance() //{ */ 215 | 216 | double WaypointFlierSimple::distance(const mrs_msgs::ReferenceStamped& waypoint, const nav_msgs::Odometry& odom) { 217 | 218 | // | ------------- distance between two 3D points ------------- | 219 | 220 | return sqrt((pow(waypoint.reference.position.x - odom.pose.pose.position.x, 2)) + (pow(waypoint.reference.position.y - odom.pose.pose.position.y, 2)) + 221 | (pow(waypoint.reference.position.z - odom.pose.pose.position.z, 2))); 222 | } 223 | 224 | double WaypointFlierSimple::getRandomDouble(double min, double max) { 225 | 226 | // | --------- random double in the min and max bounds -------- | 227 | 228 | float r = (float)rand() / (float)RAND_MAX; 229 | return min + r * (max - min); 230 | } 231 | 232 | //} 233 | 234 | } // namespace example_waypoint_flier_simple 235 | 236 | /* every nodelet must include macros which export the class as a nodelet plugin */ 237 | #include 238 | PLUGINLIB_EXPORT_CLASS(example_waypoint_flier_simple::WaypointFlierSimple, nodelet::Nodelet); 239 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/tmux/config/automatic_start.yaml: -------------------------------------------------------------------------------- 1 | # A timeout between the takeoff being triggered and the UAV actually taking off 2 | # while the timeout is counting down, the takeoff can be aborted by switching off 3 | # the offboard mode. 4 | # default = 5 sec 5 | safety_timeout: 1.0 # [s] 6 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/tmux/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | # GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: 2 | ## -------------------------------------------------------------- 3 | ## | rosrun mrs_uav_core get_public_params.py # 4 | ## -------------------------------------------------------------- 5 | 6 | mrs_uav_managers: 7 | 8 | estimation_manager: 9 | 10 | # loaded state estimator plugins 11 | state_estimators: [ 12 | "gps_garmin", 13 | "gps_baro", 14 | ] 15 | 16 | initial_state_estimator: "gps_baro" # will be used as the first state estimator 17 | agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) 18 | 19 | uav_manager: 20 | 21 | takeoff: 22 | 23 | during_takeoff: 24 | controller: "MpcController" 25 | tracker: "LandoffTracker" 26 | 27 | after_takeoff: 28 | controller: "Se3Controller" 29 | tracker: "MpcTracker" 30 | 31 | midair_activation: 32 | 33 | during_activation: 34 | controller: "MidairActivationController" 35 | tracker: "MidairActivationTracker" 36 | 37 | after_activation: 38 | controller: "Se3Controller" 39 | tracker: "MpcTracker" 40 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/tmux/config/hw_api.yaml: -------------------------------------------------------------------------------- 1 | input_mode: 2 | actuators: false 3 | control_group: false 4 | attitude_rate: true 5 | attitude: true 6 | acceleration_hdg_rate: false 7 | acceleration_hdg: false 8 | velocity_hdg_rate: false 9 | velocity_hdg: false 10 | position: false 11 | 12 | feedforward: true 13 | 14 | outputs: 15 | distance_sensor: true 16 | gnss: true 17 | rtk: true 18 | imu: true 19 | altitude: true 20 | magnetometer_heading: true 21 | rc_channels: true 22 | battery_state: true 23 | position: true 24 | orientation: true 25 | velocity: true 26 | angular_velocity: true 27 | odometry: true 28 | ground_truth: true 29 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/tmux/config/network_config.yaml: -------------------------------------------------------------------------------- 1 | # 1. This list is used by NimbroNetwork for mutual communication of the UAVs 2 | # The names of the robots have to match hostnames described in /etc/hosts. 3 | # 4 | # 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. 5 | # The names should match the true "UAV_NAMES" (the topic prefixes). 6 | # 7 | # network_config:=~/config/network_config.yaml 8 | # 9 | # to the core.launch and nimbro.launch. 10 | 11 | network: 12 | 13 | robot_names: [ 14 | 15 | uav1, 16 | uav2, 17 | uav3, 18 | uav4, 19 | 20 | ] 21 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/tmux/config/simulator.yaml: -------------------------------------------------------------------------------- 1 | simulation_rate: 250 2 | clock_rate: 250 3 | realtime_factor: 1.0 4 | 5 | iterate_without_input: true 6 | 7 | individual_takeoff_platform: 8 | enabled: false 9 | 10 | uav_names: [ 11 | "uav1", 12 | ] 13 | 14 | uav1: 15 | type: "x500" 16 | spawn: 17 | x: 10.0 18 | y: 15.0 19 | z: 2.0 20 | heading: 0 21 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/tmux/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: true 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/tmux/kill.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Absolute path to this script. /home/user/bin/foo.sh 4 | SCRIPT=$(readlink -f $0) 5 | # Absolute path this script is in. /home/user/bin 6 | SCRIPTPATH=`dirname $SCRIPT` 7 | cd "$SCRIPTPATH" 8 | 9 | export TMUX_SESSION_NAME=simulation 10 | export TMUX_SOCKET_NAME=mrs 11 | 12 | # just attach to the session 13 | tmux -L $TMUX_SOCKET_NAME split-window -t $TMUX_SESSION_NAME 14 | 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 15 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/tmux/layout.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "border": "normal", 4 | "floating": "auto_off", 5 | "fullscreen_mode": 0, 6 | "layout": "splitv", 7 | "percent": 0.285714285714286, 8 | "type": "con", 9 | "nodes": [ 10 | { 11 | "border": "pixel", 12 | "current_border_width": 3, 13 | "floating": "auto_off", 14 | "geometry": { 15 | "height": 1030, 16 | "width": 936, 17 | "x": 4806, 18 | "y": 32 19 | }, 20 | "name": "rviz.rviz* - RViz", 21 | "percent": 0.5, 22 | "swallows": [ 23 | { 24 | "instance": "^rviz$" 25 | } 26 | ], 27 | "type": "con" 28 | }, 29 | { 30 | "border": "pixel", 31 | "current_border_width": 3, 32 | "floating": "auto_off", 33 | "geometry": { 34 | "height": 460, 35 | "width": 724, 36 | "x": 0, 37 | "y": 0 38 | }, 39 | "name": "./start.sh", 40 | "percent": 0.5, 41 | "swallows": [ 42 | { 43 | "instance": "^urxvt$" 44 | } 45 | ], 46 | "type": "con" 47 | } 48 | ] 49 | } 50 | ] 51 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/tmux/session.yml: -------------------------------------------------------------------------------- 1 | # do not modify these 2 | root: ./ 3 | name: simulation 4 | socket_name: mrs 5 | attach: false 6 | tmux_options: -f /etc/ctu-mrs/tmux.conf 7 | # you can modify these 8 | pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500 9 | startup_window: waypointflier 10 | windows: 11 | - roscore: 12 | layout: tiled 13 | panes: 14 | - roscore 15 | - simulator: 16 | layout: tiled 17 | panes: 18 | - waitForRos; roslaunch mrs_multirotor_simulator multirotor_simulator.launch custom_config:=./config/simulator.yaml 19 | - hw_api: 20 | layout: tiled 21 | panes: 22 | - waitForTime; roslaunch mrs_multirotor_simulator hw_api.launch custom_config:=./config/hw_api.yaml 23 | - takeoff: 24 | layout: tiled 25 | panes: 26 | - waitForHw; roslaunch mrs_uav_autostart automatic_start.launch custom_config:=./config/automatic_start.yaml 27 | - waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard 28 | - status: 29 | layout: tiled 30 | panes: 31 | - waitForHw; roslaunch mrs_uav_status status.launch 32 | - control: 33 | layout: tiled 34 | panes: 35 | - waitForHw; roslaunch mrs_uav_core core.launch 36 | platform_config:=`rospack find mrs_multirotor_simulator`/config/mrs_uav_system/$UAV_TYPE.yaml 37 | custom_config:=./config/custom_config.yaml 38 | world_config:=./config/world_config.yaml 39 | network_config:=./config/network_config.yaml 40 | - waypointflier: 41 | layout: tiled 42 | panes: 43 | - waitForControl; roslaunch example_waypoint_flier_simple waypoint_flier_simple.launch 44 | - 'history -s rosservice call /$UAV_NAME/waypoint_flier_simple/start' 45 | - rviz: 46 | layout: tiled 47 | panes: 48 | - waitForControl; roslaunch mrs_uav_core rviz.launch 49 | - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch 50 | - waitForControl; waitForControl; roslaunch mrs_rviz_plugins rviz_interface.launch 51 | - layout: 52 | layout: tiled 53 | panes: 54 | - waitForControl; sleep 5; ~/.i3/layout_manager.sh ./layout.json 55 | -------------------------------------------------------------------------------- /cpp/waypoint_flier_simple/tmux/start.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Absolute path to this script. /home/user/bin/foo.sh 4 | SCRIPT=$(readlink -f $0) 5 | # Absolute path this script is in. /home/user/bin 6 | SCRIPTPATH=`dirname $SCRIPT` 7 | cd "$SCRIPTPATH" 8 | 9 | export TMUX_SESSION_NAME=simulation 10 | export TMUX_SOCKET_NAME=mrs 11 | 12 | # start tmuxinator 13 | tmuxinator start -p ./session.yml 14 | 15 | # if we are not in tmux 16 | if [ -z $TMUX ]; then 17 | 18 | # just attach to the session 19 | tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME 20 | 21 | # if we are in tmux 22 | else 23 | 24 | # switch to the newly-started session 25 | tmux detach-client -E "tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME" 26 | 27 | fi 28 | -------------------------------------------------------------------------------- /python/README.md: -------------------------------------------------------------------------------- 1 | # MRS Python Examples 2 | -------------------------------------------------------------------------------- /python/sweeping_generator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15.0) 2 | project(example_sweeping_generator) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | 7 | set(CATKIN_DEPENDENCIES 8 | rospy 9 | mrs_msgs 10 | ) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | ${CATKIN_DEPENDENCIES} 14 | ) 15 | 16 | catkin_package( 17 | CATKIN_DEPENDS ${CATKIN_DEPENDENCIES} 18 | ) 19 | 20 | ## -------------------------------------------------------------- 21 | ## | Install | 22 | ## -------------------------------------------------------------- 23 | 24 | install(DIRECTORY launch config 25 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 26 | ) 27 | 28 | install(DIRECTORY scripts/ 29 | USE_SOURCE_PERMISSIONS 30 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 31 | ) 32 | 33 | install(DIRECTORY scripts 34 | USE_SOURCE_PERMISSIONS 35 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 36 | ) 37 | -------------------------------------------------------------------------------- /python/sweeping_generator/README.md: -------------------------------------------------------------------------------- 1 | # Sweeping Generator 2 | 3 | ## How to start 4 | 5 | ```bash 6 | ./tmux/start.sh 7 | ``` 8 | 9 | When ready, call the service prepared in the bottom terminal window: 10 | ```bash 11 | rosservice call /$UAV_NAME/sweeping_generator/start 3.0 12 | ``` 13 | -------------------------------------------------------------------------------- /python/sweeping_generator/config/sweeping_generator.yaml: -------------------------------------------------------------------------------- 1 | frame_id: "world_origin" 2 | 3 | timer_main: 4 | rate: 1.0 # [Hz] 5 | 6 | center: 7 | x: 0.0 # [m] 8 | y: 0.0 # [m] 9 | z: 2.0 # [m] 10 | 11 | dimensions: 12 | x: 20.0 # [m] 13 | y: 20.0 # [m] 14 | -------------------------------------------------------------------------------- /python/sweeping_generator/launch/sweeping_generator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /python/sweeping_generator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | example_sweeping_generator 5 | 1.0.0 6 | The Python Example for Sweeping Generator 7 | 8 | Tomas Baca 9 | Tomas Baca 10 | 11 | MIT 12 | 13 | catkin 14 | 15 | rospy 16 | mrs_msgs 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /python/sweeping_generator/scripts/sweeping_generator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import rospy 4 | import numpy 5 | 6 | from mrs_msgs.msg import ControlManagerDiagnostics,Reference 7 | from mrs_msgs.srv import PathSrv,PathSrvRequest 8 | from mrs_msgs.srv import Vec1,Vec1Response 9 | 10 | class Node: 11 | 12 | # #{ __init__(self) 13 | 14 | def __init__(self): 15 | 16 | rospy.init_node("sweeping_generator", anonymous=True) 17 | 18 | ## | --------------------- load parameters -------------------- | 19 | 20 | self.frame_id = rospy.get_param("~frame_id") 21 | 22 | self.center_x = rospy.get_param("~center/x") 23 | self.center_y = rospy.get_param("~center/y") 24 | self.center_z = rospy.get_param("~center/z") 25 | 26 | self.dimensions_x = rospy.get_param("~dimensions/x") 27 | self.dimensions_y = rospy.get_param("~dimensions/y") 28 | 29 | self.timer_main_rate = rospy.get_param("~timer_main/rate") 30 | 31 | rospy.loginfo('[SweepingGenerator]: initialized') 32 | 33 | ## | ----------------------- subscribers ---------------------- | 34 | 35 | self.sub_control_manager_diag = rospy.Subscriber("~control_manager_diag_in", ControlManagerDiagnostics, self.callbackControlManagerDiagnostics) 36 | 37 | ## | --------------------- service servers -------------------- | 38 | 39 | self.ss_start = rospy.Service('~start_in', Vec1, self.callbackStart) 40 | 41 | ## | --------------------- service clients -------------------- | 42 | 43 | self.sc_path = rospy.ServiceProxy('~path_out', PathSrv) 44 | 45 | ## | ------------------------- timers ------------------------- | 46 | 47 | self.timer_main = rospy.Timer(rospy.Duration(1.0/self.timer_main_rate), self.timerMain) 48 | 49 | ## | -------------------- spin till the end ------------------- | 50 | 51 | self.is_initialized = True 52 | 53 | rospy.spin() 54 | 55 | # #} end of __init__() 56 | 57 | ## | ------------------------- methods ------------------------ | 58 | 59 | # #{ planPath() 60 | 61 | def planPath(self, step_size): 62 | 63 | rospy.loginfo('[SweepingGenerator]: planning path') 64 | 65 | # https://ctu-mrs.github.io/mrs_msgs/srv/PathSrv.html 66 | # -> https://ctu-mrs.github.io/mrs_msgs/msg/Path.html 67 | path_msg = PathSrvRequest() 68 | 69 | path_msg.path.header.frame_id = self.frame_id 70 | path_msg.path.header.stamp = rospy.Time.now() 71 | 72 | path_msg.path.fly_now = True 73 | 74 | path_msg.path.use_heading = True 75 | 76 | sign = 1.0 77 | 78 | # fill in the path with a sweeping pattern 79 | for i in numpy.arange(-self.dimensions_x/2.0, self.dimensions_x/2.0, step_size): 80 | 81 | for j in numpy.arange(-self.dimensions_y/2.0, self.dimensions_y/2.0, step_size): 82 | 83 | # https://ctu-mrs.github.io/mrs_msgs/msg/Reference.html 84 | point = Reference() 85 | 86 | point.position.x = self.center_x + i 87 | point.position.y = self.center_y + j*sign 88 | point.position.z = self.center_z 89 | point.heading = 0.0 90 | 91 | path_msg.path.points.append(point) 92 | 93 | if sign > 0.0: 94 | sign = -1.0 95 | else: 96 | sign = 1.0 97 | 98 | return path_msg 99 | 100 | # #} end of planPath() 101 | 102 | ## | ------------------------ callbacks ----------------------- | 103 | 104 | # #{ callbackControlManagerDiagnostics(): 105 | 106 | def callbackControlManagerDiagnostics(self, msg): 107 | 108 | if not self.is_initialized: 109 | return 110 | 111 | rospy.loginfo_once('[SweepingGenerator]: getting ControlManager diagnostics') 112 | 113 | self.sub_control_manager_diag = msg 114 | 115 | # #} end of 116 | 117 | # #{ callbackStart(): 118 | 119 | def callbackStart(self, req): 120 | 121 | if not self.is_initialized: 122 | return Vec1Response(False, "not initialized") 123 | 124 | # set the step size based on the service data 125 | step_size = req.goal 126 | 127 | path_msg = self.planPath(step_size) 128 | 129 | try: 130 | response = self.sc_path.call(path_msg) 131 | except: 132 | rospy.logerr('[SweepingGenerator]: path service not callable') 133 | pass 134 | 135 | if response.success: 136 | rospy.loginfo('[SweepingGenerator]: path set') 137 | else: 138 | rospy.loginfo('[SweepingGenerator]: path setting failed, message: {}'.format(response.message)) 139 | 140 | return Vec1Response(True, "starting") 141 | 142 | # #} end of 143 | 144 | ## | ------------------------- timers ------------------------- | 145 | 146 | # #{ timerMain() 147 | 148 | def timerMain(self, event=None): 149 | 150 | if not self.is_initialized: 151 | return 152 | 153 | rospy.loginfo_once('[SweepingGenerator]: main timer spinning') 154 | 155 | if isinstance(self.sub_control_manager_diag, ControlManagerDiagnostics): 156 | if self.sub_control_manager_diag.tracker_status.have_goal: 157 | rospy.loginfo('[SweepingGenerator]: tracker has goal') 158 | else: 159 | rospy.loginfo('[SweepingGenerator]: waiting for command') 160 | 161 | # #} end of timerMain() 162 | 163 | if __name__ == '__main__': 164 | try: 165 | node = Node() 166 | except rospy.ROSInterruptException: 167 | pass 168 | -------------------------------------------------------------------------------- /python/sweeping_generator/tmux/config/automatic_start.yaml: -------------------------------------------------------------------------------- 1 | # A timeout between the takeoff being triggered and the UAV actually taking off 2 | # while the timeout is counting down, the takeoff can be aborted by switching off 3 | # the offboard mode. 4 | # default = 5 sec 5 | safety_timeout: 1.0 # [s] 6 | -------------------------------------------------------------------------------- /python/sweeping_generator/tmux/config/custom_config.yaml: -------------------------------------------------------------------------------- 1 | # GET ALL PARAMETERS USABLE FOR CUSTOM CONFIG BY RUNNING: 2 | ## -------------------------------------------------------------- 3 | ## | rosrun mrs_uav_core get_public_params.py # 4 | ## -------------------------------------------------------------- 5 | 6 | mrs_uav_managers: 7 | 8 | estimation_manager: 9 | 10 | # loaded state estimator plugins 11 | state_estimators: [ 12 | "gps_garmin", 13 | "gps_baro", 14 | ] 15 | 16 | initial_state_estimator: "gps_baro" # will be used as the first state estimator 17 | agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) 18 | 19 | uav_manager: 20 | 21 | takeoff: 22 | 23 | during_takeoff: 24 | controller: "MpcController" 25 | tracker: "LandoffTracker" 26 | 27 | after_takeoff: 28 | controller: "Se3Controller" 29 | tracker: "MpcTracker" 30 | 31 | midair_activation: 32 | 33 | during_activation: 34 | controller: "MidairActivationController" 35 | tracker: "MidairActivationTracker" 36 | 37 | after_activation: 38 | controller: "Se3Controller" 39 | tracker: "MpcTracker" 40 | -------------------------------------------------------------------------------- /python/sweeping_generator/tmux/config/hw_api.yaml: -------------------------------------------------------------------------------- 1 | input_mode: 2 | actuators: false 3 | control_group: false 4 | attitude_rate: true 5 | attitude: true 6 | acceleration_hdg_rate: false 7 | acceleration_hdg: false 8 | velocity_hdg_rate: false 9 | velocity_hdg: false 10 | position: false 11 | 12 | feedforward: true 13 | 14 | outputs: 15 | distance_sensor: true 16 | gnss: true 17 | rtk: true 18 | imu: true 19 | altitude: true 20 | magnetometer_heading: true 21 | rc_channels: true 22 | battery_state: true 23 | position: true 24 | orientation: true 25 | velocity: true 26 | angular_velocity: true 27 | odometry: true 28 | ground_truth: true 29 | -------------------------------------------------------------------------------- /python/sweeping_generator/tmux/config/network_config.yaml: -------------------------------------------------------------------------------- 1 | # 1. This list is used by NimbroNetwork for mutual communication of the UAVs 2 | # The names of the robots have to match hostnames described in /etc/hosts. 3 | # 4 | # 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. 5 | # The names should match the true "UAV_NAMES" (the topic prefixes). 6 | # 7 | # network_config:=~/config/network_config.yaml 8 | # 9 | # to the core.launch and nimbro.launch. 10 | 11 | network: 12 | 13 | robot_names: [ 14 | 15 | uav1, 16 | uav2, 17 | uav3, 18 | uav4, 19 | 20 | ] 21 | -------------------------------------------------------------------------------- /python/sweeping_generator/tmux/config/simulator.yaml: -------------------------------------------------------------------------------- 1 | simulation_rate: 250 2 | clock_rate: 250 3 | realtime_factor: 1.0 4 | 5 | iterate_without_input: true 6 | 7 | individual_takeoff_platform: 8 | enabled: false 9 | 10 | uav_names: [ 11 | "uav1", 12 | ] 13 | 14 | uav1: 15 | type: "x500" 16 | spawn: 17 | x: 10.0 18 | y: 15.0 19 | z: 2.0 20 | heading: 0 21 | -------------------------------------------------------------------------------- /python/sweeping_generator/tmux/config/world_config.yaml: -------------------------------------------------------------------------------- 1 | world_origin: 2 | 3 | units: "LATLON" # {"UTM, "LATLON"} 4 | 5 | origin_x: 47.397743 6 | origin_y: 8.545594 7 | 8 | safety_area: 9 | 10 | enabled: true 11 | 12 | horizontal: 13 | 14 | # the frame of reference in which the points are expressed 15 | frame_name: "world_origin" 16 | 17 | # polygon 18 | # 19 | # x, y [m] for any frame_name except latlon_origin 20 | # x = latitude, y = longitude [deg] for frame_name=="latlon_origin" 21 | points: [ 22 | -50, -50, 23 | 50, -50, 24 | 50, 50, 25 | -50, 50, 26 | ] 27 | 28 | vertical: 29 | 30 | # the frame of reference in which the max&min z is expressed 31 | frame_name: "world_origin" 32 | 33 | max_z: 15.0 34 | min_z: 0.5 35 | -------------------------------------------------------------------------------- /python/sweeping_generator/tmux/kill.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Absolute path to this script. /home/user/bin/foo.sh 4 | SCRIPT=$(readlink -f $0) 5 | # Absolute path this script is in. /home/user/bin 6 | SCRIPTPATH=`dirname $SCRIPT` 7 | cd "$SCRIPTPATH" 8 | 9 | export TMUX_SESSION_NAME=simulation 10 | export TMUX_SOCKET_NAME=mrs 11 | 12 | # just attach to the session 13 | tmux -L $TMUX_SOCKET_NAME split-window -t $TMUX_SESSION_NAME 14 | 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 15 | -------------------------------------------------------------------------------- /python/sweeping_generator/tmux/layout.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "border": "normal", 4 | "floating": "auto_off", 5 | "fullscreen_mode": 0, 6 | "layout": "splitv", 7 | "percent": 0.285714285714286, 8 | "type": "con", 9 | "nodes": [ 10 | { 11 | "border": "pixel", 12 | "current_border_width": 3, 13 | "floating": "auto_off", 14 | "geometry": { 15 | "height": 1030, 16 | "width": 936, 17 | "x": 4806, 18 | "y": 32 19 | }, 20 | "name": "rviz.rviz* - RViz", 21 | "percent": 0.5, 22 | "swallows": [ 23 | { 24 | "instance": "^rviz$" 25 | } 26 | ], 27 | "type": "con" 28 | }, 29 | { 30 | "border": "pixel", 31 | "current_border_width": 3, 32 | "floating": "auto_off", 33 | "geometry": { 34 | "height": 460, 35 | "width": 724, 36 | "x": 0, 37 | "y": 0 38 | }, 39 | "name": "./start.sh", 40 | "percent": 0.5, 41 | "swallows": [ 42 | { 43 | "instance": "^urxvt$" 44 | } 45 | ], 46 | "type": "con" 47 | } 48 | ] 49 | } 50 | ] 51 | -------------------------------------------------------------------------------- /python/sweeping_generator/tmux/session.yml: -------------------------------------------------------------------------------- 1 | # do not modify these 2 | root: ./ 3 | name: simulation 4 | socket_name: mrs 5 | attach: false 6 | tmux_options: -f /etc/ctu-mrs/tmux.conf 7 | # you can modify these 8 | pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500 9 | startup_window: sweeping_generator 10 | windows: 11 | - roscore: 12 | layout: tiled 13 | panes: 14 | - roscore 15 | - simulator: 16 | layout: tiled 17 | panes: 18 | - waitForRos; roslaunch mrs_multirotor_simulator multirotor_simulator.launch custom_config:=./config/simulator.yaml 19 | - hw_api: 20 | layout: tiled 21 | panes: 22 | - waitForTime; roslaunch mrs_multirotor_simulator hw_api.launch custom_config:=./config/hw_api.yaml 23 | - takeoff: 24 | layout: tiled 25 | panes: 26 | - waitForHw; roslaunch mrs_uav_autostart automatic_start.launch custom_config:=./config/automatic_start.yaml 27 | - waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard 28 | - status: 29 | layout: tiled 30 | panes: 31 | - waitForHw; roslaunch mrs_uav_status status.launch 32 | - control: 33 | layout: tiled 34 | panes: 35 | - waitForHw; roslaunch mrs_uav_core core.launch 36 | platform_config:=`rospack find mrs_multirotor_simulator`/config/mrs_uav_system/$UAV_TYPE.yaml 37 | custom_config:=./config/custom_config.yaml 38 | world_config:=./config/world_config.yaml 39 | network_config:=./config/network_config.yaml 40 | - sweeping_generator: 41 | layout: tiled 42 | panes: 43 | - waitForControl; roslaunch example_sweeping_generator sweeping_generator.launch 44 | - 'history -s rosservice call /$UAV_NAME/sweeping_generator/start 3.0' 45 | - rviz: 46 | layout: tiled 47 | panes: 48 | - waitForControl; roslaunch mrs_uav_core rviz.launch 49 | - waitForControl; roslaunch mrs_rviz_plugins load_robot.launch 50 | - waitForControl; waitForControl; roslaunch mrs_rviz_plugins rviz_interface.launch 51 | - layout: 52 | layout: tiled 53 | panes: 54 | - waitForControl; sleep 5; ~/.i3/layout_manager.sh ./layout.json 55 | -------------------------------------------------------------------------------- /python/sweeping_generator/tmux/start.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Absolute path to this script. /home/user/bin/foo.sh 4 | SCRIPT=$(readlink -f $0) 5 | # Absolute path this script is in. /home/user/bin 6 | SCRIPTPATH=`dirname $SCRIPT` 7 | cd "$SCRIPTPATH" 8 | 9 | export TMUX_SESSION_NAME=simulation 10 | export TMUX_SOCKET_NAME=mrs 11 | 12 | # start tmuxinator 13 | tmuxinator start -p ./session.yml 14 | 15 | # if we are not in tmux 16 | if [ -z $TMUX ]; then 17 | 18 | # just attach to the session 19 | tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME 20 | 21 | # if we are in tmux 22 | else 23 | 24 | # switch to the newly-started session 25 | tmux detach-client -E "tmux -L $TMUX_SOCKET_NAME a -t $TMUX_SESSION_NAME" 26 | 27 | fi 28 | -------------------------------------------------------------------------------- /repurpose_package.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | if [ $# -le 1 ] || [ $1 == "--help" ]; then 4 | echo "usage: ./repurpose_package.sh [--dry-run, --camel-case] 5 | Removes the .git directory, initializes a new one, 6 | replaces all occurences of the original package name with the new one in all files withing all subfolders, 7 | replaces all occurences of the original package name with the new one wthinin all filenames." 8 | exit 1 9 | fi 10 | 11 | ORIG_NAME=$1 12 | NEW_NAME=$2 13 | DRY_RUN=0 14 | CAMEL_CASE=0 15 | 16 | if [ "$3" == "--dry-run" ] || [ "$4" == "--dry-run" ]; then 17 | echo "Dry run mode on - nothing will be changed." 18 | DRY_RUN=1 19 | fi 20 | 21 | if [ "$3" == "--camel-case" ] || [ "$4" == "--camel-case" ]; then 22 | echo "CamelCase occurences will also be replaced." 23 | CAMEL_CASE=1 24 | fi 25 | 26 | # #{ init_new_repo () 27 | 28 | init_new_repo () 29 | { 30 | local DRY_RUN=$1 31 | 32 | # Remove .git and initialize a new repository 33 | 34 | echo "** CREATING NEW GIT REPOSITORY **" 35 | 36 | # Ask for confirmation 37 | echo History of the old repository will be deleted. 38 | read -p "Are you sure? (y/n) " -r 39 | 40 | if [[ $REPLY =~ ^[Yy]$ ]] 41 | then 42 | 43 | if [ "$DRY_RUN" -eq 0 ]; then 44 | rm -rf .git 45 | git init 46 | fi 47 | echo Created a new git repository. 48 | 49 | else 50 | 51 | echo Not creating a new git repository. 52 | 53 | fi 54 | } 55 | 56 | # #} 57 | 58 | # #{ replace_within_files () 59 | 60 | replace_within_files () 61 | { 62 | local ORIG_NAME=$1 63 | local NEW_NAME=$2 64 | local DRY_RUN=$3 65 | 66 | # Replace occurences within files 67 | readarray -d '' within_files < <(grep -rl "$ORIG_NAME" --null) 68 | 69 | echo "** REPLACING OCCURENCES WITHIN FILES **" 70 | 71 | if [[ -z "${within_files[@]}" ]]; then 72 | 73 | echo "No matches found for \"$ORIG_NAME\" within files!" 74 | 75 | else 76 | 77 | # Ask for confirmation 78 | echo These files will be modified: 79 | for file in "${within_files[@]}" 80 | do 81 | echo " - $file" 82 | done 83 | read -p "Are you sure? (y/n) " -r 84 | 85 | if [[ $REPLY =~ ^[Yy]$ ]] 86 | then 87 | 88 | for file in "${within_files[@]}" 89 | do 90 | echo " - Replacing \"$1\" with \"$2\" in file \"$file\"." 91 | if [ "$DRY_RUN" -eq 0 ]; then 92 | sed -i "s/$ORIG_NAME/$NEW_NAME/g" "$file" 93 | fi 94 | done 95 | 96 | echo "Replaced all occurences within files." 97 | 98 | else 99 | 100 | echo "Not replacing occurences within files." 101 | 102 | fi 103 | 104 | fi 105 | } 106 | 107 | # #} 108 | 109 | # #{ replace_within_filenames () 110 | 111 | replace_within_filenames () 112 | { 113 | local ORIG_NAME=$1 114 | local NEW_NAME=$2 115 | local DRY_RUN=$3 116 | 117 | # Replace occurences in file names 118 | readarray -d '' files < <(find -name "*$ORIG_NAME*" -print0) 119 | 120 | echo "** REPLACING OCCURENCES WITHIN FILE NAMES **" 121 | 122 | if [[ -z "${files[@]}" ]]; then 123 | 124 | echo "No matches found for \"$ORIG_NAME\" in file names!" 125 | 126 | else 127 | 128 | # Ask for confirmation 129 | echo These files will be modified: 130 | for file in "${files[@]}" 131 | do 132 | echo " - $file" 133 | done 134 | read -p "Are you sure? (y/n) " -r 135 | 136 | if [[ $REPLY =~ ^[Yy]$ ]] 137 | then 138 | 139 | for file in "${files[@]}" 140 | do 141 | new_file=${file//$ORIG_NAME/$NEW_NAME} 142 | echo " - Renaming file \"$file\" to \"$new_file\"." 143 | if [ "$DRY_RUN" -eq 0 ]; then 144 | mv "$file" "$new_file" 145 | fi 146 | done 147 | 148 | echo "Replaced all occurences within file name." 149 | 150 | else 151 | 152 | echo "Not replacing occurences within file name." 153 | 154 | fi 155 | 156 | fi 157 | } 158 | 159 | # #} 160 | 161 | init_new_repo $DRY_RUN 162 | replace_within_files $ORIG_NAME $NEW_NAME $DRY_RUN 163 | replace_within_filenames $ORIG_NAME $NEW_NAME $DRY_RUN 164 | 165 | if [ "$CAMEL_CASE" -eq 1 ]; then 166 | 167 | ORIG_NAME_CC=`echo "$ORIG_NAME" | sed -r 's/(^|_)([a-z])/\U\2/g'` 168 | NEW_NAME_CC=`echo "$NEW_NAME" | sed -r 's/(^|_)([a-z])/\U\2/g'` 169 | 170 | replace_within_files $ORIG_NAME_CC $NEW_NAME_CC $DRY_RUN 171 | replace_within_filenames $ORIG_NAME_CC $NEW_NAME_CC $DRY_RUN 172 | 173 | fi 174 | 175 | if [ "$DRY_RUN" -eq 1 ]; then 176 | echo "Dry run mode on - nothing was changed." 177 | fi 178 | 179 | --------------------------------------------------------------------------------