├── .gitignore ├── .gitmodules ├── .travis.yml ├── LICENSE ├── README.md ├── crazyflie ├── CMakeLists.txt └── package.xml ├── crazyflie_controller ├── CMakeLists.txt ├── config │ └── crazyflie2.yaml ├── launch │ └── crazyflie2.launch ├── package.xml └── src │ ├── controller.cpp │ └── pid.hpp ├── crazyflie_demo ├── CMakeLists.txt ├── launch │ ├── crazyflie.rviz │ ├── crazyflie_multi.rviz │ ├── crazyflie_pos.rviz │ ├── crazyflie_pos_multi.rviz │ ├── customLogBlocks.launch │ ├── external_pose_vicon.launch │ ├── external_position_vicon.launch │ ├── external_position_vrpn.launch │ ├── hover.launch │ ├── hover_vicon.launch │ ├── hover_vrpn.launch │ ├── multi_hover_vicon.launch │ ├── multi_hover_vrpn.launch │ ├── multi_teleop_ps3.launch │ ├── multi_teleop_xbox360.launch │ ├── multi_waypoint_vicon.launch │ ├── position.launch │ ├── ps3.launch │ ├── swarm_external_position_vrpn.launch │ ├── teleop_ps3.launch │ ├── teleop_vicon.launch │ ├── teleop_xbox360.launch │ └── xbox360.launch ├── package.xml ├── scripts │ ├── Hover.py │ ├── Position.py │ ├── const_thrust.py │ ├── controller.py │ ├── crazyflie.py │ ├── demo.py │ ├── demo1.py │ ├── demo2.py │ ├── execute_trajectory.py │ ├── figure8.csv │ ├── figure8withTakeoffAndLanding.csv │ ├── publish_external_pose_vicon.py │ ├── publish_external_position_vicon.py │ ├── publish_external_position_vrpn.py │ ├── publish_pose.py │ ├── publish_pose_teleop.py │ ├── takeoff.csv │ ├── test_high_level.py │ └── uav_trajectory.py └── src │ └── quadrotor_teleop.cpp ├── crazyflie_description ├── CMakeLists.txt ├── README.md ├── export.png ├── launch │ ├── crazyflie.rviz │ ├── crazyflie2_rviz.launch │ └── crazyflie_rviz.launch ├── meshes │ ├── crazyflie.dae │ ├── crazyflie.skp │ └── crazyflie2.dae ├── package.xml └── urdf │ ├── crazyflie.urdf.xacro │ └── crazyflie2.urdf.xacro └── crazyflie_driver ├── CMakeLists.txt ├── launch ├── crazyflie_add.launch └── crazyflie_server.launch ├── msg ├── FullState.msg ├── GenericLogData.msg ├── Hover.msg ├── LogBlock.msg ├── Position.msg ├── TrajectoryPolynomialPiece.msg ├── VelocityWorld.msg └── crtpPacket.msg ├── package.xml ├── src ├── crazyflie_add.cpp └── crazyflie_server.cpp └── srv ├── AddCrazyflie.srv ├── GoTo.srv ├── Land.srv ├── NotifySetpointsStop.srv ├── RemoveCrazyflie.srv ├── SetGroupMask.srv ├── StartTrajectory.srv ├── Stop.srv ├── Takeoff.srv ├── UpdateParams.srv ├── UploadTrajectory.srv └── sendPacket.srv /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | */CMakeFiles/* 3 | */catkin_generated/* 4 | *.cmake 5 | */Makefile 6 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "crazyflie_cpp"] 2 | path = crazyflie_cpp 3 | url = https://github.com/whoenig/crazyflie_cpp.git 4 | [submodule "crazyflie_tools"] 5 | path = crazyflie_tools 6 | url = https://github.com/whoenig/crazyflie_tools 7 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # Generic .travis.yml file for running continuous integration on Travis-CI for 2 | # any ROS package. 3 | # 4 | # Available here: 5 | # - https://github.com/felixduvallet/ros-travis-integration 6 | # 7 | # This installs ROS on a clean Travis-CI virtual machine, creates a ROS 8 | # workspace, resolves all listed dependencies, and sets environment variables 9 | # (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are 10 | # no compilation errors), and runs all the tests. If any of the compilation/test 11 | # phases fail, the build is marked as a failure. 12 | # 13 | # We handle two types of package dependencies specified in the package manifest: 14 | # - system dependencies that can be installed using `rosdep`, including other 15 | # ROS packages and system libraries. These dependencies must be known to 16 | # `rosdistro` and are installed using apt-get. 17 | # - package dependencies that must be checked out from source. These are handled by 18 | # `wstool`, and should be listed in a file named dependencies.rosinstall. 19 | # 20 | 21 | # There are envioronment variables you may want to change, such as ROS_DISTRO, 22 | # ROSINSTALL_FILE, and the CATKIN_OPTIONS file. See the README.md for more 23 | # information on these flags, and 24 | # https://docs.travis-ci.com/user/environment-variables/ for information about 25 | # Travis environment variables in general. 26 | # 27 | # Author: Felix Duvallet 28 | 29 | # NOTE: The build lifecycle on Travis.ci is something like this: 30 | # before_install 31 | # install 32 | # before_script 33 | # script 34 | # after_success or after_failure 35 | # after_script 36 | # OPTIONAL before_deploy 37 | # OPTIONAL deploy 38 | # OPTIONAL after_deploy 39 | 40 | ################################################################################ 41 | 42 | # blocklist ros2 branch for use with travis (ROS2 uses Github actions instead) 43 | branches: 44 | except: 45 | - ros2 46 | 47 | sudo: required 48 | cache: 49 | - apt 50 | 51 | # Build all valid Ubuntu/ROS combinations available on Travis VMs. 52 | language: generic 53 | matrix: 54 | include: 55 | - name: "Trusty indigo" 56 | dist: trusty 57 | env: ROS_DISTRO=indigo 58 | - name: "Xenial kinetic" 59 | dist: xenial 60 | env: ROS_DISTRO=kinetic 61 | 62 | # Configuration variables. All variables are global now, but this can be used to 63 | # trigger a build matrix for different ROS distributions if desired. 64 | env: 65 | global: 66 | - ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [trusty|xenial|...] 67 | - CI_SOURCE_PATH=$(pwd) 68 | - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall 69 | - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options 70 | - ROS_PARALLEL_JOBS='-j8 -l6' 71 | # Set the python path manually to include /usr/-/python2.7/dist-packages 72 | # as this is where apt-get installs python packages. 73 | - PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages 74 | 75 | ################################################################################ 76 | 77 | # Install system dependencies, namely a very barebones ROS setup. 78 | before_install: 79 | - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" 80 | - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 81 | - sudo apt-get update -qq 82 | - sudo apt-get install dpkg 83 | - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-ros-base 84 | - source /opt/ros/$ROS_DISTRO/setup.bash 85 | # Prepare rosdep to install dependencies. 86 | - sudo rosdep init 87 | - rosdep update --include-eol-distros # Support EOL distros. 88 | 89 | # Create a catkin workspace with the package under integration. 90 | install: 91 | - mkdir -p ~/catkin_ws/src 92 | - cd ~/catkin_ws/src 93 | - catkin_init_workspace 94 | # Create the devel/setup.bash (run catkin_make with an empty workspace) and 95 | # source it to set the path variables. 96 | - cd ~/catkin_ws 97 | - catkin_make 98 | - source devel/setup.bash 99 | # Add the package under integration to the workspace using a symlink. 100 | - cd ~/catkin_ws/src 101 | - ln -s $CI_SOURCE_PATH . 102 | 103 | # Install all dependencies, using wstool first and rosdep second. 104 | # wstool looks for a ROSINSTALL_FILE defined in the environment variables. 105 | before_script: 106 | # source dependencies: install using wstool. 107 | - cd ~/catkin_ws/src 108 | - wstool init 109 | - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi 110 | - wstool up 111 | # package depdencies: install using rosdep. 112 | - cd ~/catkin_ws 113 | - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO 114 | 115 | # Compile and test (mark the build as failed if any step fails). If the 116 | # CATKIN_OPTIONS file exists, use it as an argument to catkin_make, for example 117 | # to blacklist certain packages. 118 | # 119 | # NOTE on testing: `catkin_make run_tests` will show the output of the tests 120 | # (gtest, nosetest, etc..) but always returns 0 (success) even if a test 121 | # fails. Running `catkin_test_results` aggregates all the results and returns 122 | # non-zero when a test fails (which notifies Travis the build failed). 123 | script: 124 | - source /opt/ros/$ROS_DISTRO/setup.bash 125 | - cd ~/catkin_ws 126 | - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS ) 127 | # Run the tests, ensuring the path is set correctly. 128 | - source devel/setup.bash 129 | - catkin_make run_tests && catkin_test_results 130 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 whoenig 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![Build Status](https://travis-ci.org/whoenig/crazyflie_ros.svg?branch=master)](https://travis-ci.org/whoenig/crazyflie_ros) 2 | 3 | > **WARNING**: This repository is deprecated. Please use the [Crazyswarm](http://crazyswarm.readthedocs.io) instead, even when only operating a single Crazyflie. If you are missing features in the Crazyswarm, feel free to [open an open issue](https://github.com/USC-ACTLab/crazyswarm/issues). 4 | 5 | crazyflie_ros 6 | ============= 7 | 8 | ROS stack for Bitcraze Crazyflie (http://www.bitcraze.se/), with the following features: 9 | 10 | * Support for Crazyflie 1.0 and Crazyflie 2.0 (using stock firmware) 11 | * Publishes on-board sensors in ROS standard message formats 12 | * Supports ROS parameters to reconfigure crazyflie parameters 13 | * Support for using multiple Crazyflies with a single Crazyradio 14 | * Includes external controller for waypoint navigation (if motion capture system is available) 15 | * No dependency to the Bitcraze SDK (Driver and Controller written in C++) 16 | 17 | A tutorial (for a slightly older version) is available in W. Hönig and N. Ayanian. "Flying Multiple UAVs Using ROS", Chapter in Robot Operating System (ROS): The Complete Reference (Volume 2), Springer, 2017. (see http://act.usc.edu/publications.html for a free pre-print). 18 | 19 | ## Citing This Work 20 | 21 | This project is published under the very permissive MIT License. However, 22 | if you use the package we appreciate if you credit this project accordingly. 23 | 24 | For academic publications, you can cite the following book chapter: 25 | ``` 26 | @Inbook{crazyflieROS, 27 | author={Wolfgang H{\"o}nig 28 | and Nora Ayanian}, 29 | editor={Anis Koubaa}, 30 | title={Flying Multiple UAVs Using ROS}, 31 | bookTitle={Robot Operating System (ROS): The Complete Reference (Volume 2)}, 32 | year={2017}, 33 | publisher={Springer International Publishing}, 34 | pages={83--118}, 35 | isbn={978-3-319-54927-9}, 36 | doi={10.1007/978-3-319-54927-9_3}, 37 | url={https://doi.org/10.1007/978-3-319-54927-9_3} 38 | } 39 | 40 | ``` 41 | 42 | If your work is related to Mixed Reality, you might cite the paper which introduced the package instead, using the following bibtex entry: 43 | ``` 44 | @conference{HoenigMixedReality2015, 45 | author = {Wolfgang H{\"o}nig and Christina Milanes and Lisa Scaria and Thai Phan and Mark Bolas and Nora Ayanian}, 46 | booktitle = {IEEE/RSJ Intl Conf. Intelligent Robots and Systems}, 47 | pages = {5382 - 5387}, 48 | title = {Mixed Reality for Robotics}, 49 | year = {2015}} 50 | ``` 51 | 52 | For any other mentioning please include my affiliation (ACTLab at University of Southern California or USC in short; The link to our webpage is http://act.usc.edu) as this work was partially done as part of my research at USC. 53 | 54 | ## Installation 55 | 56 | Clone the package into your catkin workspace: 57 | ``` 58 | git clone https://github.com/whoenig/crazyflie_ros.git 59 | cd crazyflie_ros 60 | git submodule init 61 | git submodule update 62 | ``` 63 | 64 | Use `catkin_make` on your workspace to compile. 65 | 66 | ## Usage 67 | 68 | There are six packages included: crazyflie_cpp, crazyflie_driver, crazyflie_tools, crazyflie_description, crazyflie_controller, and crazyflie_demo. 69 | Note that the below description might be slightly out-of-date, as we continue merging the Crazyswarm and crazyflie_ros. 70 | 71 | ### Crazyflie_Cpp 72 | 73 | This package contains a cpp library for the Crazyradio and Crazyflie. It can be used independently of ROS. 74 | 75 | ### Crazyflie_driver 76 | 77 | This package contains the driver. In order to support multiple Crazyflies with a single Crazyradio, there is crazyflie_server (communicating with all Crazyflies) and crazyflie_add to dynamically add Crazyflies. 78 | The server does not communicate to any Crazyflie initially, hence crazyflie_add needs to be used. 79 | 80 | ### Crazyflie_tools 81 | 82 | This package contains tools which are helpful, but not required for normal operation. So far, it just support one tool for scanning for a Crazyflie. 83 | 84 | You can find connected Crazyflies using: 85 | ``` 86 | rosrun crazyflie_tools scan 87 | ``` 88 | 89 | ### Crazyflie_description 90 | 91 | This package contains a 3D model of the Crazyflie (1.0). This is for visualization purposes in rviz. 92 | 93 | ### Crazyflie_controller 94 | 95 | This package contains a simple PID controller for hovering or waypoint navigation. 96 | It can be used with external motion capture systems, such as VICON. 97 | 98 | ### Crazyflie_demo 99 | 100 | This package contains a rich set of examples to get quickly started with the Crazyflie. 101 | 102 | For teleoperation using a joystick, use: 103 | ``` 104 | roslaunch crazyflie_demo teleop_xbox360.launch uri:=radio://0/100/2M 105 | ``` 106 | where the uri specifies the uri of your Crazyflie. You can find valid uris using the scan command in the crazyflie_tools package. 107 | 108 | For hovering at (0,0,1) using VICON, use: 109 | ``` 110 | roslaunch crazyflie_demo hover_vicon.launch uri:=radio://0/100/2M frame:=/vicon/crazyflie/crazyflie x:=0 y:=0 z:=1 111 | ``` 112 | where the uri specifies the uri of your Crazyflie and frame the tf-frame. The launch file runs vicon_bridge automatically. 113 | 114 | For multiple Crazyflies make sure that all Crazyflies have a different address. 115 | Crazyflies which share a dongle should use the same channel and datarate for best performance. 116 | The performance degrades with the number of Crazyflies per dongle due to bandwidth limitations, however it was tested successfully to use 3 CFs per Crazyradio. 117 | ``` 118 | roslaunch crazyflie_demo multi_teleop_xbox360.launch uri1:=radio://0/100/2M/E7E7E7E7E7 uri2:=radio://0/100/2M/E7E7E7E705 119 | ``` 120 | 121 | Please check the launch files in the crazyflie_demo package for other examples, including simple waypoint navigation. 122 | 123 | ## ROS Features 124 | 125 | ### Parameters 126 | 127 | The launch file supports the following arguments: 128 | * uri: Specifier for the crazyflie, e.g. radio://0/80/2M 129 | * tf_prefix: tf prefix for the crazyflie frame(s) 130 | * roll_trim: Trim in degrees, e.g. negative if flie drifts to the left 131 | * pitch_trim: Trim in degrees, e.g. negative if flie drifts forward 132 | 133 | See http://wiki.bitcraze.se/projects:crazyflie:userguide:tips_and_tricks for details on how to obtain good trim values. 134 | 135 | ### Subscribers 136 | 137 | #### cmd_vel 138 | 139 | Similar to the hector_quadrotor, package the fields are used as following: 140 | * linear.y: roll [e.g. -30 to 30 degrees] 141 | * linear.x: pitch [e.g. -30 to 30 degrees] 142 | * angular.z: yawrate [e.g. -200 to 200 degrees/second] 143 | * linear.z: thrust [10000 to 60000 (mapped to PWM output)] 144 | 145 | ### Publishers 146 | 147 | #### imu 148 | * sensor_msgs/IMU 149 | * contains the sensor readings of gyroscope and accelerometer 150 | * The covariance matrices are set to unknown 151 | * orientation is not set (this could be done by the magnetometer readings in the future.) 152 | * update: 10ms (time between crazyflie and ROS not synchronized!) 153 | * can be viewed in rviz 154 | 155 | #### temperature 156 | * sensor_msgs/Temperature 157 | * From Barometer (10DOF version only) in degree Celcius (Sensor readings might be higher than expected because the PCB warms up; see http://www.bitcraze.se/2014/02/logging-and-parameter-frameworks-turtorial/) 158 | * update: 100ms (time between crazyflie and ROS not synchronized!) 159 | 160 | #### magnetic_field 161 | * sensor_msgs/MagneticField 162 | * update: 100ms (time between crazyflie and ROS not synchronized!) 163 | 164 | #### pressure 165 | * Float32 166 | * hPa (or mbar) 167 | * update: 100ms (time between crazyflie and ROS not synchronized!) 168 | 169 | #### battery 170 | * Float32 171 | * Volts 172 | * update: 100ms (time between crazyflie and ROS not synchronized!) 173 | 174 | ## Similar Projects 175 | 176 | * https://github.com/gtagency/crazyflie-ros 177 | * no documentation 178 | * no teleop 179 | * https://github.com/omwdunkley/crazyflieROS 180 | * coupled with GUI 181 | * based on custom firmware 182 | * https://github.com/mbeards/crazyflie-ros 183 | * incomplete 184 | * https://github.com/utexas-air-fri/crazyflie_fly 185 | * not based on official SDK 186 | * no support for logging 187 | * https://github.com/mchenryc/crazyflie 188 | * no documentation 189 | 190 | ## Notes 191 | 192 | * The dynamic_reconfigure package (http://wiki.ros.org/dynamic_reconfigure/) seems like a good fit to map the parameters, however it has severe limitations: 193 | * Changed-Callback does not include which parameter(s) were changed. There is only a notion of a level which is a simple bitmask. This would cause that on any parameter change we would need to update all parameters on the Crazyflie. 194 | * Parameters are statically generated. There are hacks to add parameters at runtime, however those might not work with future versions of dynamic_reconfigure. 195 | * Groups not fully supported (https://github.com/ros-visualization/rqt_common_plugins/issues/162; This seems to be closed now, however the Indigo binary packages did not pick up the fixes yet). 196 | -------------------------------------------------------------------------------- /crazyflie/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(crazyflie) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /crazyflie/package.xml: -------------------------------------------------------------------------------- 1 | 2 | crazyflie 3 | 0.0.1 4 | ROS integration for Crazyflie Nano Quadcopter from Bitcraze 5 | 6 | Wolfgang Hoenig 7 | 8 | MIT 9 | 10 | http://ros.org/wiki/crazyflie 11 | https://github.com/whoenig/crazyflie_ros/issues 12 | 13 | Wolfgang Hoenig 14 | 15 | 16 | catkin 17 | 18 | 19 | crazyflie_cpp 20 | crazyflie_driver 21 | crazyflie_description 22 | crazyflie_controller 23 | crazyflie_demo 24 | crazyflie_tools 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /crazyflie_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(crazyflie_controller) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | std_msgs 6 | tf 7 | ) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 28 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 29 | ## pulled in transitively but can be declared for certainty nonetheless: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################### 70 | ## catkin specific configuration ## 71 | ################################### 72 | ## The catkin_package macro generates cmake config files for your package 73 | ## Declare things to be passed to dependent projects 74 | ## INCLUDE_DIRS: uncomment this if you package contains header files 75 | ## LIBRARIES: libraries you create in this project that dependent projects also need 76 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 77 | ## DEPENDS: system dependencies of this project that dependent projects also need 78 | catkin_package( 79 | # INCLUDE_DIRS include 80 | # LIBRARIES crazyflie_controller 81 | # CATKIN_DEPENDS other_catkin_pkg 82 | # DEPENDS system_lib 83 | ) 84 | 85 | ########### 86 | ## Build ## 87 | ########### 88 | 89 | ## Specify additional locations of header files 90 | ## Your package locations should be listed before other locations 91 | include_directories( 92 | ${catkin_INCLUDE_DIRS} 93 | ) 94 | 95 | ## Declare a cpp library 96 | # add_library(crazyflie_controller 97 | # src/${PROJECT_NAME}/crazyflie_controller.cpp 98 | # ) 99 | 100 | # Declare a cpp executable 101 | add_executable(crazyflie_controller 102 | src/controller.cpp) 103 | 104 | # Add cmake target dependencies of the executable/library 105 | # as an example, message headers may need to be generated before nodes 106 | # add_dependencies(crazyflie_controller crazyflie_controller_generate_messages_cpp) 107 | 108 | # Specify libraries to link a library or executable target against 109 | target_link_libraries(crazyflie_controller 110 | ${catkin_LIBRARIES} 111 | ) 112 | 113 | ############# 114 | ## Install ## 115 | ############# 116 | 117 | # all install targets should use catkin DESTINATION variables 118 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 119 | 120 | ## Mark executable scripts (Python etc.) for installation 121 | ## in contrast to setup.py, you can choose the destination 122 | # install(PROGRAMS 123 | # scripts/my_python_script 124 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 125 | # ) 126 | 127 | ## Mark executables and/or libraries for installation 128 | # install(TARGETS crazyflie_controller crazyflie_controller_node 129 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 130 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 131 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 132 | # ) 133 | 134 | ## Mark cpp header files for installation 135 | # install(DIRECTORY include/${PROJECT_NAME}/ 136 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 137 | # FILES_MATCHING PATTERN "*.h" 138 | # PATTERN ".svn" EXCLUDE 139 | # ) 140 | 141 | ## Mark other files for installation (e.g. launch and bag files, etc.) 142 | # install(FILES 143 | # # myfile1 144 | # # myfile2 145 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 146 | # ) 147 | 148 | ############# 149 | ## Testing ## 150 | ############# 151 | 152 | ## Add gtest based cpp test target and link libraries 153 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_crazyflie_controller.cpp) 154 | # if(TARGET ${PROJECT_NAME}-test) 155 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 156 | # endif() 157 | 158 | ## Add folders to be run by python nosetests 159 | # catkin_add_nosetests(test) 160 | -------------------------------------------------------------------------------- /crazyflie_controller/config/crazyflie2.yaml: -------------------------------------------------------------------------------- 1 | PIDs: 2 | X: 3 | kp: 40.0 4 | kd: 20.0 5 | ki: 2.0 6 | minOutput: -10.0 7 | maxOutput: 10.0 8 | integratorMin: -0.1 9 | integratorMax: 0.1 10 | Y: 11 | kp: -40.0 12 | kd: -20.0 13 | ki: -2.0 14 | minOutput: -10.0 15 | maxOutput: 10.0 16 | integratorMin: -0.1 17 | integratorMax: 0.1 18 | Z: 19 | kp: 5000.0 20 | kd: 6000.0 21 | ki: 3500.0 22 | minOutput: 10000.0 23 | maxOutput: 60000.0 24 | integratorMin: -1000.0 25 | integratorMax: 1000.0 26 | Yaw: 27 | kp: -200.0 28 | kd: -20.0 29 | ki: 0.0 30 | minOutput: -200.0 31 | maxOutput: 200.0 32 | integratorMin: 0.0 33 | integratorMax: 0.0 34 | -------------------------------------------------------------------------------- /crazyflie_controller/launch/crazyflie2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /crazyflie_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | crazyflie_controller 4 | 0.0.1 5 | Controller to fly a Crazyflie in a space with location feedback (such as motion capture). 6 | 7 | Wolfgang Hoenig 8 | 9 | MIT 10 | 11 | http://ros.org/wiki/crazyflie 12 | https://github.com/whoenig/crazyflie_ros/issues 13 | 14 | Wolfgang Hoenig 15 | 16 | catkin 17 | std_msgs 18 | tf 19 | 20 | std_msgs 21 | tf 22 | 23 | -------------------------------------------------------------------------------- /crazyflie_controller/src/controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | #include "pid.hpp" 8 | 9 | double get( 10 | const ros::NodeHandle& n, 11 | const std::string& name) { 12 | double value; 13 | n.getParam(name, value); 14 | return value; 15 | } 16 | 17 | class Controller 18 | { 19 | public: 20 | 21 | Controller( 22 | const std::string& worldFrame, 23 | const std::string& frame, 24 | const ros::NodeHandle& n) 25 | : m_worldFrame(worldFrame) 26 | , m_frame(frame) 27 | , m_pubNav() 28 | , m_listener() 29 | , m_pidX( 30 | get(n, "PIDs/X/kp"), 31 | get(n, "PIDs/X/kd"), 32 | get(n, "PIDs/X/ki"), 33 | get(n, "PIDs/X/minOutput"), 34 | get(n, "PIDs/X/maxOutput"), 35 | get(n, "PIDs/X/integratorMin"), 36 | get(n, "PIDs/X/integratorMax"), 37 | "x") 38 | , m_pidY( 39 | get(n, "PIDs/Y/kp"), 40 | get(n, "PIDs/Y/kd"), 41 | get(n, "PIDs/Y/ki"), 42 | get(n, "PIDs/Y/minOutput"), 43 | get(n, "PIDs/Y/maxOutput"), 44 | get(n, "PIDs/Y/integratorMin"), 45 | get(n, "PIDs/Y/integratorMax"), 46 | "y") 47 | , m_pidZ( 48 | get(n, "PIDs/Z/kp"), 49 | get(n, "PIDs/Z/kd"), 50 | get(n, "PIDs/Z/ki"), 51 | get(n, "PIDs/Z/minOutput"), 52 | get(n, "PIDs/Z/maxOutput"), 53 | get(n, "PIDs/Z/integratorMin"), 54 | get(n, "PIDs/Z/integratorMax"), 55 | "z") 56 | , m_pidYaw( 57 | get(n, "PIDs/Yaw/kp"), 58 | get(n, "PIDs/Yaw/kd"), 59 | get(n, "PIDs/Yaw/ki"), 60 | get(n, "PIDs/Yaw/minOutput"), 61 | get(n, "PIDs/Yaw/maxOutput"), 62 | get(n, "PIDs/Yaw/integratorMin"), 63 | get(n, "PIDs/Yaw/integratorMax"), 64 | "yaw") 65 | , m_state(Idle) 66 | , m_goal() 67 | , m_subscribeGoal() 68 | , m_serviceTakeoff() 69 | , m_serviceLand() 70 | , m_thrust(0) 71 | , m_startZ(0) 72 | { 73 | ros::NodeHandle nh; 74 | m_listener.waitForTransform(m_worldFrame, m_frame, ros::Time(0), ros::Duration(10.0)); 75 | m_pubNav = nh.advertise("cmd_vel", 1); 76 | m_subscribeGoal = nh.subscribe("goal", 1, &Controller::goalChanged, this); 77 | m_serviceTakeoff = nh.advertiseService("takeoff", &Controller::takeoff, this); 78 | m_serviceLand = nh.advertiseService("land", &Controller::land, this); 79 | } 80 | 81 | void run(double frequency) 82 | { 83 | ros::NodeHandle node; 84 | ros::Timer timer = node.createTimer(ros::Duration(1.0/frequency), &Controller::iteration, this); 85 | ros::spin(); 86 | } 87 | 88 | private: 89 | void goalChanged( 90 | const geometry_msgs::PoseStamped::ConstPtr& msg) 91 | { 92 | m_goal = *msg; 93 | } 94 | 95 | bool takeoff( 96 | std_srvs::Empty::Request& req, 97 | std_srvs::Empty::Response& res) 98 | { 99 | ROS_INFO("Takeoff requested!"); 100 | m_state = TakingOff; 101 | 102 | tf::StampedTransform transform; 103 | m_listener.lookupTransform(m_worldFrame, m_frame, ros::Time(0), transform); 104 | m_startZ = transform.getOrigin().z(); 105 | 106 | return true; 107 | } 108 | 109 | bool land( 110 | std_srvs::Empty::Request& req, 111 | std_srvs::Empty::Response& res) 112 | { 113 | ROS_INFO("Landing requested!"); 114 | m_state = Landing; 115 | 116 | return true; 117 | } 118 | 119 | void getTransform( 120 | const std::string& sourceFrame, 121 | const std::string& targetFrame, 122 | tf::StampedTransform& result) 123 | { 124 | m_listener.lookupTransform(sourceFrame, targetFrame, ros::Time(0), result); 125 | } 126 | 127 | void pidReset() 128 | { 129 | m_pidX.reset(); 130 | m_pidY.reset(); 131 | m_pidZ.reset(); 132 | m_pidYaw.reset(); 133 | } 134 | 135 | void iteration(const ros::TimerEvent& e) 136 | { 137 | float dt = e.current_real.toSec() - e.last_real.toSec(); 138 | 139 | switch(m_state) 140 | { 141 | case TakingOff: 142 | { 143 | tf::StampedTransform transform; 144 | m_listener.lookupTransform(m_worldFrame, m_frame, ros::Time(0), transform); 145 | if (transform.getOrigin().z() > m_startZ + 0.05 || m_thrust > 50000) 146 | { 147 | pidReset(); 148 | m_pidZ.setIntegral(m_thrust / m_pidZ.ki()); 149 | m_state = Automatic; 150 | m_thrust = 0; 151 | } 152 | else 153 | { 154 | m_thrust += 10000 * dt; 155 | geometry_msgs::Twist msg; 156 | msg.linear.z = m_thrust; 157 | m_pubNav.publish(msg); 158 | } 159 | 160 | } 161 | break; 162 | case Landing: 163 | { 164 | m_goal.pose.position.z = m_startZ + 0.05; 165 | tf::StampedTransform transform; 166 | m_listener.lookupTransform(m_worldFrame, m_frame, ros::Time(0), transform); 167 | if (transform.getOrigin().z() <= m_startZ + 0.05) { 168 | m_state = Idle; 169 | geometry_msgs::Twist msg; 170 | m_pubNav.publish(msg); 171 | } 172 | } 173 | // intentional fall-thru 174 | case Automatic: 175 | { 176 | tf::StampedTransform transform; 177 | m_listener.lookupTransform(m_worldFrame, m_frame, ros::Time(0), transform); 178 | 179 | geometry_msgs::PoseStamped targetWorld; 180 | targetWorld.header.stamp = transform.stamp_; 181 | targetWorld.header.frame_id = m_worldFrame; 182 | targetWorld.pose = m_goal.pose; 183 | 184 | geometry_msgs::PoseStamped targetDrone; 185 | m_listener.transformPose(m_frame, targetWorld, targetDrone); 186 | 187 | tfScalar roll, pitch, yaw; 188 | tf::Matrix3x3( 189 | tf::Quaternion( 190 | targetDrone.pose.orientation.x, 191 | targetDrone.pose.orientation.y, 192 | targetDrone.pose.orientation.z, 193 | targetDrone.pose.orientation.w 194 | )).getRPY(roll, pitch, yaw); 195 | 196 | geometry_msgs::Twist msg; 197 | msg.linear.x = m_pidX.update(0, targetDrone.pose.position.x); 198 | msg.linear.y = m_pidY.update(0.0, targetDrone.pose.position.y); 199 | msg.linear.z = m_pidZ.update(0.0, targetDrone.pose.position.z); 200 | msg.angular.z = m_pidYaw.update(0.0, yaw); 201 | m_pubNav.publish(msg); 202 | 203 | 204 | } 205 | break; 206 | case Idle: 207 | { 208 | geometry_msgs::Twist msg; 209 | m_pubNav.publish(msg); 210 | } 211 | break; 212 | } 213 | } 214 | 215 | private: 216 | 217 | enum State 218 | { 219 | Idle = 0, 220 | Automatic = 1, 221 | TakingOff = 2, 222 | Landing = 3, 223 | }; 224 | 225 | private: 226 | std::string m_worldFrame; 227 | std::string m_frame; 228 | ros::Publisher m_pubNav; 229 | tf::TransformListener m_listener; 230 | PID m_pidX; 231 | PID m_pidY; 232 | PID m_pidZ; 233 | PID m_pidYaw; 234 | State m_state; 235 | geometry_msgs::PoseStamped m_goal; 236 | ros::Subscriber m_subscribeGoal; 237 | ros::ServiceServer m_serviceTakeoff; 238 | ros::ServiceServer m_serviceLand; 239 | float m_thrust; 240 | float m_startZ; 241 | }; 242 | 243 | int main(int argc, char **argv) 244 | { 245 | ros::init(argc, argv, "controller"); 246 | 247 | // Read parameters 248 | ros::NodeHandle n("~"); 249 | std::string worldFrame; 250 | n.param("worldFrame", worldFrame, "/world"); 251 | std::string frame; 252 | n.getParam("frame", frame); 253 | double frequency; 254 | n.param("frequency", frequency, 50.0); 255 | 256 | Controller controller(worldFrame, frame, n); 257 | controller.run(frequency); 258 | 259 | return 0; 260 | } 261 | -------------------------------------------------------------------------------- /crazyflie_controller/src/pid.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | class PID 6 | { 7 | public: 8 | PID( 9 | float kp, 10 | float kd, 11 | float ki, 12 | float minOutput, 13 | float maxOutput, 14 | float integratorMin, 15 | float integratorMax, 16 | const std::string& name) 17 | : m_kp(kp) 18 | , m_kd(kd) 19 | , m_ki(ki) 20 | , m_minOutput(minOutput) 21 | , m_maxOutput(maxOutput) 22 | , m_integratorMin(integratorMin) 23 | , m_integratorMax(integratorMax) 24 | , m_integral(0) 25 | , m_previousError(0) 26 | , m_previousTime(ros::Time::now()) 27 | { 28 | } 29 | 30 | void reset() 31 | { 32 | m_integral = 0; 33 | m_previousError = 0; 34 | m_previousTime = ros::Time::now(); 35 | } 36 | 37 | void setIntegral(float integral) 38 | { 39 | m_integral = integral; 40 | } 41 | 42 | float ki() const 43 | { 44 | return m_ki; 45 | } 46 | 47 | float update(float value, float targetValue) 48 | { 49 | ros::Time time = ros::Time::now(); 50 | float dt = time.toSec() - m_previousTime.toSec(); 51 | float error = targetValue - value; 52 | m_integral += error * dt; 53 | m_integral = std::max(std::min(m_integral, m_integratorMax), m_integratorMin); 54 | float p = m_kp * error; 55 | float d = 0; 56 | if (dt > 0) 57 | { 58 | d = m_kd * (error - m_previousError) / dt; 59 | } 60 | float i = m_ki * m_integral; 61 | float output = p + d + i; 62 | m_previousError = error; 63 | m_previousTime = time; 64 | // self.pubOutput.publish(output) 65 | // self.pubError.publish(error) 66 | // self.pubP.publish(p) 67 | // self.pubD.publish(d) 68 | // self.pubI.publish(i) 69 | return std::max(std::min(output, m_maxOutput), m_minOutput); 70 | } 71 | 72 | private: 73 | float m_kp; 74 | float m_kd; 75 | float m_ki; 76 | float m_minOutput; 77 | float m_maxOutput; 78 | float m_integratorMin; 79 | float m_integratorMax; 80 | float m_integral; 81 | float m_previousError; 82 | ros::Time m_previousTime; 83 | }; 84 | -------------------------------------------------------------------------------- /crazyflie_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(crazyflie_demo) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | ) 10 | 11 | ## System dependencies are found with CMake's conventions 12 | # find_package(Boost REQUIRED COMPONENTS system) 13 | 14 | 15 | ## Uncomment this if the package has a setup.py. This macro ensures 16 | ## modules and global scripts declared therein get installed 17 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 18 | # catkin_python_setup() 19 | 20 | ################################################ 21 | ## Declare ROS messages, services and actions ## 22 | ################################################ 23 | 24 | ## To declare and build messages, services or actions from within this 25 | ## package, follow these steps: 26 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 27 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 28 | ## * In the file package.xml: 29 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 30 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 31 | ## pulled in transitively but can be declared for certainty nonetheless: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a run_depend tag for "message_runtime" 34 | ## * In this file (CMakeLists.txt): 35 | ## * add "message_generation" and every package in MSG_DEP_SET to 36 | ## find_package(catkin REQUIRED COMPONENTS ...) 37 | ## * add "message_runtime" and every package in MSG_DEP_SET to 38 | ## catkin_package(CATKIN_DEPENDS ...) 39 | ## * uncomment the add_*_files sections below as needed 40 | ## and list every .msg/.srv/.action file to be processed 41 | ## * uncomment the generate_messages entry below 42 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 43 | 44 | ## Generate messages in the 'msg' folder 45 | # add_message_files( 46 | # FILES 47 | # Message1.msg 48 | # Message2.msg 49 | # ) 50 | 51 | ## Generate services in the 'srv' folder 52 | # add_service_files( 53 | # FILES 54 | # Service1.srv 55 | # Service2.srv 56 | # ) 57 | 58 | ## Generate actions in the 'action' folder 59 | # add_action_files( 60 | # FILES 61 | # Action1.action 62 | # Action2.action 63 | # ) 64 | 65 | ## Generate added messages and services with any dependencies listed here 66 | # generate_messages( 67 | # DEPENDENCIES 68 | # std_msgs # Or other packages containing msgs 69 | # ) 70 | 71 | ################################### 72 | ## catkin specific configuration ## 73 | ################################### 74 | ## The catkin_package macro generates cmake config files for your package 75 | ## Declare things to be passed to dependent projects 76 | ## INCLUDE_DIRS: uncomment this if you package contains header files 77 | ## LIBRARIES: libraries you create in this project that dependent projects also need 78 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 79 | ## DEPENDS: system dependencies of this project that dependent projects also need 80 | catkin_package( 81 | # INCLUDE_DIRS include 82 | # LIBRARIES crazyflie_demo 83 | CATKIN_DEPENDS roscpp 84 | # DEPENDS system_lib 85 | ) 86 | 87 | ########### 88 | ## Build ## 89 | ########### 90 | 91 | ## Specify additional locations of header files 92 | ## Your package locations should be listed before other locations 93 | include_directories( 94 | ${catkin_INCLUDE_DIRS} 95 | ) 96 | 97 | ## Declare a cpp library 98 | # add_library(crazyflie_demo 99 | # src/${PROJECT_NAME}/crazyflie_demo.cpp 100 | # ) 101 | 102 | # Declare a cpp executable 103 | add_executable(quadrotor_teleop src/quadrotor_teleop.cpp) 104 | 105 | ## Add cmake target dependencies of the executable/library 106 | ## as an example, message headers may need to be generated before nodes 107 | # add_dependencies(crazyflie_demo_node crazyflie_demo_generate_messages_cpp) 108 | 109 | # Specify libraries to link a library or executable target against 110 | target_link_libraries(quadrotor_teleop 111 | ${catkin_LIBRARIES} 112 | ) 113 | 114 | ############# 115 | ## Install ## 116 | ############# 117 | 118 | # all install targets should use catkin DESTINATION variables 119 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 120 | 121 | ## Mark executable scripts (Python etc.) for installation 122 | ## in contrast to setup.py, you can choose the destination 123 | # install(PROGRAMS 124 | # scripts/my_python_script 125 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 126 | # ) 127 | 128 | ## Mark executables and/or libraries for installation 129 | # install(TARGETS crazyflie_demo crazyflie_demo_node 130 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 131 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 132 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 133 | # ) 134 | 135 | ## Mark cpp header files for installation 136 | # install(DIRECTORY include/${PROJECT_NAME}/ 137 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 138 | # FILES_MATCHING PATTERN "*.h" 139 | # PATTERN ".svn" EXCLUDE 140 | # ) 141 | 142 | ## Mark other files for installation (e.g. launch and bag files, etc.) 143 | # install(FILES 144 | # # myfile1 145 | # # myfile2 146 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 147 | # ) 148 | 149 | ############# 150 | ## Testing ## 151 | ############# 152 | 153 | ## Add gtest based cpp test target and link libraries 154 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_crazyflie_demo.cpp) 155 | # if(TARGET ${PROJECT_NAME}-test) 156 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 157 | # endif() 158 | 159 | ## Add folders to be run by python nosetests 160 | # catkin_add_nosetests(test) 161 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/crazyflie.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 81 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Imu1 10 | Splitter Ratio: 0.5 11 | Tree Height: 535 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.03 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Class: rviz_plugin_tutorials/Imu 54 | Color: 204; 51; 204 55 | Enabled: true 56 | History Length: 1 57 | Name: Imu 58 | Topic: /crazyflie/imu 59 | Value: true 60 | Enabled: true 61 | Global Options: 62 | Background Color: 48; 48; 48 63 | Fixed Frame: crazyflie/base_link 64 | Frame Rate: 30 65 | Name: root 66 | Tools: 67 | - Class: rviz/Interact 68 | Hide Inactive Objects: true 69 | - Class: rviz/MoveCamera 70 | - Class: rviz/Select 71 | - Class: rviz/FocusCamera 72 | - Class: rviz/Measure 73 | - Class: rviz/SetInitialPose 74 | Topic: /initialpose 75 | - Class: rviz/SetGoal 76 | Topic: /move_base_simple/goal 77 | - Class: rviz/PublishPoint 78 | Single click: true 79 | Topic: /clicked_point 80 | Value: true 81 | Views: 82 | Current: 83 | Class: rviz/Orbit 84 | Distance: 35.1983 85 | Enable Stereo Rendering: 86 | Stereo Eye Separation: 0.06 87 | Stereo Focal Distance: 1 88 | Swap Stereo Eyes: false 89 | Value: false 90 | Focal Point: 91 | X: 0 92 | Y: 0 93 | Z: 0 94 | Name: Current View 95 | Near Clip Distance: 0.01 96 | Pitch: 0.815398 97 | Target Frame: 98 | Value: Orbit (rviz) 99 | Yaw: 6.21859 100 | Saved: ~ 101 | Window Geometry: 102 | Displays: 103 | collapsed: false 104 | Height: 846 105 | Hide Left Dock: false 106 | Hide Right Dock: false 107 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000002a9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000002a9000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000043000002a9000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000023a00fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002a900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 108 | Selection: 109 | collapsed: false 110 | Time: 111 | collapsed: false 112 | Tool Properties: 113 | collapsed: false 114 | Views: 115 | collapsed: false 116 | Width: 1200 117 | X: 60 118 | Y: 60 119 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/crazyflie_multi.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 81 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Imu1 10 | - /Imu2 11 | Splitter Ratio: 0.5 12 | Tree Height: 535 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.03 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 1 54 | Class: rviz_plugin_tutorials/Imu 55 | Color: 204; 51; 204 56 | Enabled: true 57 | History Length: 1 58 | Name: Imu 59 | Topic: /crazyflie1/imu 60 | Value: true 61 | - Alpha: 1 62 | Class: rviz_plugin_tutorials/Imu 63 | Color: 0; 0; 255 64 | Enabled: true 65 | History Length: 1 66 | Name: Imu 67 | Topic: /crazyflie2/imu 68 | Value: true 69 | Enabled: true 70 | Global Options: 71 | Background Color: 48; 48; 48 72 | Fixed Frame: world 73 | Frame Rate: 30 74 | Name: root 75 | Tools: 76 | - Class: rviz/Interact 77 | Hide Inactive Objects: true 78 | - Class: rviz/MoveCamera 79 | - Class: rviz/Select 80 | - Class: rviz/FocusCamera 81 | - Class: rviz/Measure 82 | - Class: rviz/SetInitialPose 83 | Topic: /initialpose 84 | - Class: rviz/SetGoal 85 | Topic: /move_base_simple/goal 86 | - Class: rviz/PublishPoint 87 | Single click: true 88 | Topic: /clicked_point 89 | Value: true 90 | Views: 91 | Current: 92 | Class: rviz/Orbit 93 | Distance: 35.1983 94 | Enable Stereo Rendering: 95 | Stereo Eye Separation: 0.06 96 | Stereo Focal Distance: 1 97 | Swap Stereo Eyes: false 98 | Value: false 99 | Focal Point: 100 | X: 0 101 | Y: 0 102 | Z: 0 103 | Name: Current View 104 | Near Clip Distance: 0.01 105 | Pitch: 0.815398 106 | Target Frame: 107 | Value: Orbit (rviz) 108 | Yaw: 6.21859 109 | Saved: ~ 110 | Window Geometry: 111 | Displays: 112 | collapsed: false 113 | Height: 846 114 | Hide Left Dock: false 115 | Hide Right Dock: false 116 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000002a9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000002a9000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000043000002a9000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000023a00fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002a900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 117 | Selection: 118 | collapsed: false 119 | Time: 120 | collapsed: false 121 | Tool Properties: 122 | collapsed: false 123 | Views: 124 | collapsed: false 125 | Width: 1200 126 | X: 60 127 | Y: 60 128 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/crazyflie_pos.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 81 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.617241 8 | Tree Height: 689 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.588679 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: "" 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 1 33 | Class: rviz/Grid 34 | Color: 160; 160; 164 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.03 38 | Value: Lines 39 | Name: Grid 40 | Normal Cell Count: 0 41 | Offset: 42 | X: 0 43 | Y: 0 44 | Z: 0 45 | Plane: XY 46 | Plane Cell Count: 10 47 | Reference Frame: 48 | Value: true 49 | - Alpha: 1 50 | Class: rviz/RobotModel 51 | Collision Enabled: false 52 | Enabled: true 53 | Links: 54 | All Links Enabled: true 55 | Expand Joint Details: false 56 | Expand Link Details: false 57 | Expand Tree: false 58 | Link Tree Style: "" 59 | base_link: 60 | Alpha: 1 61 | Show Axes: false 62 | Show Trail: false 63 | Value: true 64 | Name: RobotModel 65 | Robot Description: robot_description 66 | TF Prefix: crazyflie 67 | Update Interval: 0 68 | Value: true 69 | Visual Enabled: true 70 | - Alpha: 0.5 71 | Axes Length: 1 72 | Axes Radius: 0.1 73 | Class: rviz/Pose 74 | Color: 255; 25; 0 75 | Enabled: true 76 | Head Length: 0.1 77 | Head Radius: 0.05 78 | Name: Pose 79 | Shaft Length: 0.1 80 | Shaft Radius: 0.02 81 | Shape: Arrow 82 | Topic: /crazyflie/goal 83 | Value: true 84 | Enabled: true 85 | Global Options: 86 | Background Color: 48; 48; 48 87 | Fixed Frame: world 88 | Frame Rate: 30 89 | Name: root 90 | Tools: 91 | - Class: rviz/Interact 92 | Hide Inactive Objects: true 93 | - Class: rviz/MoveCamera 94 | - Class: rviz/Select 95 | - Class: rviz/FocusCamera 96 | - Class: rviz/Measure 97 | - Class: rviz/SetInitialPose 98 | Topic: /initialpose 99 | - Class: rviz/SetGoal 100 | Topic: /move_base_simple/goal 101 | - Class: rviz/PublishPoint 102 | Single click: true 103 | Topic: /clicked_point 104 | Value: true 105 | Views: 106 | Current: 107 | Class: rviz/Orbit 108 | Distance: 2.66686 109 | Enable Stereo Rendering: 110 | Stereo Eye Separation: 0.06 111 | Stereo Focal Distance: 1 112 | Swap Stereo Eyes: false 113 | Value: false 114 | Focal Point: 115 | X: 0.956974 116 | Y: 0.0562646 117 | Z: 0.145126 118 | Name: Current View 119 | Near Clip Distance: 0.01 120 | Pitch: 0.555398 121 | Target Frame: 122 | Value: Orbit (rviz) 123 | Yaw: 3.19541 124 | Saved: ~ 125 | Window Geometry: 126 | Displays: 127 | collapsed: false 128 | Height: 973 129 | Hide Left Dock: false 130 | Hide Right Dock: false 131 | QMainWindow State: 000000ff00000000fd00000004000000000000012400000343fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000343000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000343fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000343000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005c90000003efc0100000002fb0000000800540069006d00650100000000000005c9000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000038a0000034300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 132 | Selection: 133 | collapsed: false 134 | Time: 135 | collapsed: false 136 | Tool Properties: 137 | collapsed: false 138 | Views: 139 | collapsed: false 140 | Width: 1481 141 | X: 55 142 | Y: 20 143 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/crazyflie_pos_multi.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 81 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Pose1 8 | - /Pose2 9 | Splitter Ratio: 0.617241 10 | Tree Height: 689 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz/RobotModel 53 | Collision Enabled: false 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: "" 61 | base_link: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | Value: true 66 | Name: RobotModel 67 | Robot Description: robot_description 68 | TF Prefix: crazyflie1 69 | Update Interval: 0 70 | Value: true 71 | Visual Enabled: true 72 | - Alpha: 0.5 73 | Axes Length: 1 74 | Axes Radius: 0.1 75 | Class: rviz/Pose 76 | Color: 255; 25; 0 77 | Enabled: true 78 | Head Length: 0.1 79 | Head Radius: 0.05 80 | Name: Pose 81 | Shaft Length: 0.1 82 | Shaft Radius: 0.02 83 | Shape: Arrow 84 | Topic: /crazyflie1/goal 85 | Value: true 86 | - Alpha: 1 87 | Class: rviz/RobotModel 88 | Collision Enabled: false 89 | Enabled: true 90 | Links: 91 | All Links Enabled: true 92 | Expand Joint Details: false 93 | Expand Link Details: false 94 | Expand Tree: false 95 | Link Tree Style: Links in Alphabetic Order 96 | base_link: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | Name: RobotModel 102 | Robot Description: robot_description 103 | TF Prefix: crazyflie2 104 | Update Interval: 0 105 | Value: true 106 | Visual Enabled: true 107 | - Alpha: 0.5 108 | Axes Length: 1 109 | Axes Radius: 0.1 110 | Class: rviz/Pose 111 | Color: 0; 0; 255 112 | Enabled: true 113 | Head Length: 0.1 114 | Head Radius: 0.05 115 | Name: Pose 116 | Shaft Length: 0.1 117 | Shaft Radius: 0.02 118 | Shape: Arrow 119 | Topic: /crazyflie2/goal 120 | Value: true 121 | Enabled: true 122 | Global Options: 123 | Background Color: 48; 48; 48 124 | Fixed Frame: world 125 | Frame Rate: 30 126 | Name: root 127 | Tools: 128 | - Class: rviz/Interact 129 | Hide Inactive Objects: true 130 | - Class: rviz/MoveCamera 131 | - Class: rviz/Select 132 | - Class: rviz/FocusCamera 133 | - Class: rviz/Measure 134 | - Class: rviz/SetInitialPose 135 | Topic: /initialpose 136 | - Class: rviz/SetGoal 137 | Topic: /move_base_simple/goal 138 | - Class: rviz/PublishPoint 139 | Single click: true 140 | Topic: /clicked_point 141 | Value: true 142 | Views: 143 | Current: 144 | Class: rviz/Orbit 145 | Distance: 4.07685 146 | Enable Stereo Rendering: 147 | Stereo Eye Separation: 0.06 148 | Stereo Focal Distance: 1 149 | Swap Stereo Eyes: false 150 | Value: false 151 | Focal Point: 152 | X: 0.956974 153 | Y: 0.0562646 154 | Z: 0.145126 155 | Name: Current View 156 | Near Clip Distance: 0.01 157 | Pitch: 0.360398 158 | Target Frame: 159 | Value: Orbit (rviz) 160 | Yaw: 3.30041 161 | Saved: ~ 162 | Window Geometry: 163 | Displays: 164 | collapsed: false 165 | Height: 973 166 | Hide Left Dock: false 167 | Hide Right Dock: false 168 | QMainWindow State: 000000ff00000000fd00000004000000000000012400000343fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000343000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000343fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000343000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005c90000003efc0100000002fb0000000800540069006d00650100000000000005c9000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000038a0000034300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 169 | Selection: 170 | collapsed: false 171 | Time: 172 | collapsed: false 173 | Tool Properties: 174 | collapsed: false 175 | Views: 176 | collapsed: false 177 | Width: 1481 178 | X: 55 179 | Y: 14 180 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/customLogBlocks.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | genericLogTopics: ["log1", "log2"] 16 | genericLogTopicFrequencies: [10, 100] 17 | genericLogTopic_log1_Variables: ["pm.vbat"] 18 | genericLogTopic_log2_Variables: ["acc.x", "acc.y", "acc.z"] 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/external_pose_vicon.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | genericLogTopics: ["log1"] 21 | genericLogTopicFrequencies: [100] 22 | genericLogTopic_log1_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z", "stabilizer.yaw"] 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/external_position_vicon.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | genericLogTopics: ["log1"] 21 | genericLogTopicFrequencies: [100] 22 | genericLogTopic_log1_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"] 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/external_position_vrpn.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | genericLogTopics: ["log1"] 24 | genericLogTopicFrequencies: [100] 25 | genericLogTopic_log1_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"] 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | server: $(arg ip) 38 | port: $(arg port) 39 | update_frequency: 100.0 40 | frame_id: /world 41 | child_frame_id: $(arg frame) 42 | use_server_time: false 43 | broadcast_tf: false 44 | refresh_tracker_frequency: 1.0 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/hover.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/hover_vicon.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 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/hover_vrpn.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 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/multi_hover_vicon.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 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/multi_hover_vrpn.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 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/multi_teleop_ps3.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 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/multi_teleop_xbox360.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 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/multi_waypoint_vicon.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 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/position.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/ps3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/swarm_external_position_vrpn.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 | genericLogTopics: ["log1"] 28 | genericLogTopicFrequencies: [100] 29 | genericLogTopic_log1_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"] 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | genericLogTopics: ["log1"] 51 | genericLogTopicFrequencies: [100] 52 | genericLogTopic_log1_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"] 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | genericLogTopics: ["log1"] 76 | genericLogTopicFrequencies: [100] 77 | genericLogTopic_log1_Variables: ["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"] 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | server: $(arg ip) 90 | port: $(arg port) 91 | update_frequency: 100.0 92 | frame_id: /world 93 | use_server_time: false 94 | broadcast_tf: false 95 | refresh_tracker_frequency: 1.0 96 | 97 | 98 | 99 | 100 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/teleop_ps3.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 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/teleop_vicon.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 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/teleop_xbox360.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 | -------------------------------------------------------------------------------- /crazyflie_demo/launch/xbox360.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /crazyflie_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | crazyflie_demo 4 | 0.0.1 5 | Examples on how to use the crazyflie package (teleoperation, rviz integration) 6 | 7 | Wolfgang Hoenig 8 | 9 | MIT 10 | 11 | http://ros.org/wiki/crazyflie 12 | https://github.com/whoenig/crazyflie_ros/issues 13 | 14 | Wolfgang Hoenig 15 | 16 | catkin 17 | 18 | roscpp 19 | 20 | roscpp 21 | 22 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/Hover.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from crazyflie_driver.msg import Hover 6 | from std_msgs.msg import Empty 7 | from crazyflie_driver.srv import UpdateParams 8 | from threading import Thread 9 | 10 | class Crazyflie: 11 | def __init__(self, prefix): 12 | self.prefix = prefix 13 | 14 | worldFrame = rospy.get_param("~worldFrame", "/world") 15 | self.rate = rospy.Rate(10) 16 | 17 | rospy.wait_for_service(prefix + '/update_params') 18 | rospy.loginfo("found update_params service") 19 | self.update_params = rospy.ServiceProxy(prefix + '/update_params', UpdateParams) 20 | 21 | self.setParam("kalman/resetEstimation", 1) 22 | 23 | self.pub = rospy.Publisher(prefix + "/cmd_hover", Hover, queue_size=1) 24 | self.msg = Hover() 25 | self.msg.header.seq = 0 26 | self.msg.header.stamp = rospy.Time.now() 27 | self.msg.header.frame_id = worldFrame 28 | self.msg.yawrate = 0 29 | 30 | self.stop_pub = rospy.Publisher(prefix + "/cmd_stop", Empty, queue_size=1) 31 | self.stop_msg = Empty() 32 | 33 | 34 | # determine direction of speed based on distance 35 | def getSpeed(self, distance): 36 | if distance > 0: 37 | return 0.1 38 | elif distance < 0: 39 | return -0.1 40 | else: 41 | return 0 42 | 43 | def setParam(self, name, value): 44 | rospy.set_param(self.prefix + "/" + name, value) 45 | self.update_params([name]) 46 | 47 | # x, y is the x, y distance relative to itself 48 | # z is absolute z distance 49 | # TODO: solve 0 50 | def goTo (self, x, y, zDistance, yaw): 51 | duration = 0 52 | duration_x = 0 53 | duration_y = 0 54 | duration_z = 0 55 | vx = 0 56 | vy = 0 57 | z = self.msg.zDistance # the zDistance we have before 58 | z_scale = self.getSpeed(z) # the z distance each time z has to increment, will be changed 59 | 60 | # for x, in secs 61 | if x != 0: 62 | duration_x = abs(x/0.1) 63 | vx = self.getSpeed(x) 64 | 65 | # for y, in secs 66 | if y != 0: 67 | duration_y = abs(y/0.1) 68 | vy = self.getSpeed(y) 69 | 70 | duration_z = abs(z-zDistance)/0.1 71 | durations = [duration_x, duration_y, duration_z] 72 | duration = max(durations) 73 | 74 | if duration == 0: 75 | return 76 | elif duration == duration_x: 77 | # x is the longest path 78 | vy *= abs(y/x) 79 | z_scale *= abs((z-zDistance)/x) 80 | elif duration == duration_y: 81 | # y is the longest path 82 | vx *= abs(x/y) 83 | z_scale *= abs((z-zDistance)/y) 84 | elif duration == duration_z: 85 | # z is the longest path 86 | vx *= abs(x/(z-zDistance)) 87 | vy *= abs(y/(z-zDistance)) 88 | 89 | print(vx) 90 | print(vy) 91 | print(z_scale) 92 | print(duration) 93 | 94 | start = rospy.get_time() 95 | while not rospy.is_shutdown(): 96 | self.msg.vx = vx 97 | self.msg.vy = vy 98 | self.msg.yawrate = 0.0 99 | self.msg.zDistance = z 100 | if z < zDistance: 101 | print(zDistance) 102 | print(z) 103 | z += z_scale 104 | else: 105 | z = zDistance 106 | now = rospy.get_time() 107 | if (now - start > duration): 108 | break 109 | self.msg.header.seq += 1 110 | self.msg.header.stamp = rospy.Time.now() 111 | rospy.loginfo("sending...") 112 | rospy.loginfo(self.msg.vx) 113 | rospy.loginfo(self.msg.vy) 114 | rospy.loginfo(self.msg.yawrate) 115 | rospy.loginfo(self.msg.zDistance) 116 | self.pub.publish(self.msg) 117 | self.rate.sleep() 118 | 119 | # take off to z distance 120 | def takeOff(self, zDistance): 121 | time_range = 1 + int(10*zDistance/0.4) 122 | while not rospy.is_shutdown(): 123 | for y in range(time_range): 124 | self.msg.vx = 0.0 125 | self.msg.vy = 0.0 126 | self.msg.yawrate = 0.0 127 | self.msg.zDistance = y / 25.0 128 | self.msg.header.seq += 1 129 | self.msg.header.stamp = rospy.Time.now() 130 | self.pub.publish(self.msg) 131 | self.rate.sleep() 132 | for y in range(20): 133 | self.msg.vx = 0.0 134 | self.msg.vy = 0.0 135 | self.msg.yawrate = 0.0 136 | self.msg.zDistance = zDistance 137 | self.msg.header.seq += 1 138 | self.msg.header.stamp = rospy.Time.now() 139 | self.pub.publish(self.msg) 140 | self.rate.sleep() 141 | break 142 | 143 | # land from last zDistance 144 | def land (self): 145 | # get last height 146 | zDistance = self.msg.zDistance 147 | 148 | while not rospy.is_shutdown(): 149 | while zDistance > 0: 150 | self.msg.vx = 0.0 151 | self.msg.vy = 0.0 152 | self.msg.yawrate = 0.0 153 | self.msg.zDistance = zDistance 154 | self.msg.header.seq += 1 155 | self.msg.header.stamp = rospy.Time.now() 156 | self.pub.publish(self.msg) 157 | self.rate.sleep() 158 | zDistance -= 0.2 159 | self.stop_pub.publish(self.stop_msg) 160 | 161 | def handler(cf): 162 | cf.takeOff(0.4) 163 | cf.goTo(0.4, 0.1, 0.2, 0) 164 | cf.land() 165 | 166 | if __name__ == '__main__': 167 | rospy.init_node('hover', anonymous=True) 168 | 169 | cf1 = Crazyflie("cf1") 170 | cf2 = Crazyflie("cf2") 171 | 172 | t1 = Thread(target=handler, args=(cf1,)) 173 | t2 = Thread(target=handler, args=(cf2,)) 174 | t1.start() 175 | t2.start() 176 | 177 | 178 | 179 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/Position.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from crazyflie_driver.msg import Position 6 | from std_msgs.msg import Empty 7 | from crazyflie_driver.srv import UpdateParams 8 | 9 | if __name__ == '__main__': 10 | rospy.init_node('position', anonymous=True) 11 | worldFrame = rospy.get_param("~worldFrame", "/world") 12 | 13 | rate = rospy.Rate(10) # 10 hz 14 | name = "cmd_position" 15 | 16 | msg = Position() 17 | msg.header.seq = 0 18 | msg.header.stamp = rospy.Time.now() 19 | msg.header.frame_id = worldFrame 20 | msg.x = 0.0 21 | msg.y = 0.0 22 | msg.z = 0.0 23 | msg.yaw = 0.0 24 | 25 | pub = rospy.Publisher(name, Position, queue_size=1) 26 | 27 | stop_pub = rospy.Publisher("cmd_stop", Empty, queue_size=1) 28 | stop_msg = Empty() 29 | 30 | rospy.wait_for_service('update_params') 31 | rospy.loginfo("found update_params service") 32 | update_params = rospy.ServiceProxy('update_params', UpdateParams) 33 | 34 | rospy.set_param("kalman/resetEstimation", 1) 35 | update_params(["kalman/resetEstimation"]) 36 | rospy.sleep(0.1) 37 | rospy.set_param("kalman/resetEstimation", 0) 38 | update_params(["kalman/resetEstimation"]) 39 | rospy.sleep(0.5) 40 | 41 | # take off 42 | while not rospy.is_shutdown(): 43 | for y in range(10): 44 | msg.x = 0.0 45 | msg.y = 0.0 46 | msg.yaw = 0.0 47 | msg.z = y / 25.0 48 | now = rospy.get_time() 49 | msg.header.seq += 1 50 | msg.header.stamp = rospy.Time.now() 51 | rospy.loginfo("sending...") 52 | rospy.loginfo(msg.x) 53 | rospy.loginfo(msg.y) 54 | rospy.loginfo(msg.z) 55 | rospy.loginfo(msg.yaw) 56 | # rospy.loginfo(now) 57 | pub.publish(msg) 58 | rate.sleep() 59 | for y in range(20): 60 | msg.x = 0.0 61 | msg.y = 0.0 62 | msg.yaw = 0.0 63 | msg.z = 0.4 64 | msg.header.seq += 1 65 | msg.header.stamp = rospy.Time.now() 66 | rospy.loginfo("sending...") 67 | rospy.loginfo(msg.x) 68 | rospy.loginfo(msg.y) 69 | rospy.loginfo(msg.z) 70 | rospy.loginfo(msg.yaw) 71 | # rospy.loginfo(now) 72 | pub.publish(msg) 73 | rate.sleep() 74 | break 75 | 76 | # go to x: 0.2 y: 0.2 77 | start = rospy.get_time() 78 | while not rospy.is_shutdown(): 79 | msg.x = 0.2 80 | msg.y = 0.2 81 | msg.yaw = 0.0 82 | msg.z = 0.4 83 | now = rospy.get_time() 84 | if (now - start > 3.0): 85 | break 86 | msg.header.seq += 1 87 | msg.header.stamp = rospy.Time.now() 88 | rospy.loginfo("sending...") 89 | rospy.loginfo(msg.x) 90 | rospy.loginfo(msg.y) 91 | rospy.loginfo(msg.z) 92 | rospy.loginfo(msg.yaw) 93 | pub.publish(msg) 94 | rate.sleep() 95 | 96 | # land, spend 1 secs 97 | start = rospy.get_time() 98 | while not rospy.is_shutdown(): 99 | msg.x = 0.0 100 | msg.y = 0.0 101 | msg.z = 0.0 102 | msg.yaw = 0.0 103 | now = rospy.get_time() 104 | if (now - start > 1.0): 105 | break 106 | msg.header.seq += 1 107 | msg.header.stamp = rospy.Time.now() 108 | rospy.loginfo("sending...") 109 | rospy.loginfo(msg.x) 110 | rospy.loginfo(msg.y) 111 | rospy.loginfo(msg.z) 112 | rospy.loginfo(msg.yaw) 113 | pub.publish(msg) 114 | rate.sleep() 115 | 116 | stop_pub.publish(stop_msg) 117 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/const_thrust.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from geometry_msgs.msg import Twist 5 | 6 | if __name__ == '__main__': 7 | rospy.init_node('crazyflie_demo_const_thrust', anonymous=True) 8 | p = rospy.Publisher('cmd_vel', Twist) 9 | twist = Twist() 10 | r = rospy.Rate(50) 11 | #for i in range(0, 100): 12 | # p.publish(twist) 13 | # r.sleep() 14 | 15 | twist.linear.z = 12000 16 | while not rospy.is_shutdown(): 17 | p.publish(twist) 18 | r.sleep() 19 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from sensor_msgs.msg import Joy 5 | from crazyflie_driver.srv import UpdateParams 6 | from std_srvs.srv import Empty 7 | 8 | class Controller(): 9 | def __init__(self, use_controller, joy_topic): 10 | rospy.wait_for_service('update_params') 11 | rospy.loginfo("found update_params service") 12 | self._update_params = rospy.ServiceProxy('update_params', UpdateParams) 13 | 14 | rospy.loginfo("waiting for emergency service") 15 | rospy.wait_for_service('emergency') 16 | rospy.loginfo("found emergency service") 17 | self._emergency = rospy.ServiceProxy('emergency', Empty) 18 | 19 | if use_controller: 20 | rospy.loginfo("waiting for land service") 21 | rospy.wait_for_service('land') 22 | rospy.loginfo("found land service") 23 | self._land = rospy.ServiceProxy('land', Empty) 24 | 25 | rospy.loginfo("waiting for takeoff service") 26 | rospy.wait_for_service('takeoff') 27 | rospy.loginfo("found takeoff service") 28 | self._takeoff = rospy.ServiceProxy('takeoff', Empty) 29 | else: 30 | self._land = None 31 | self._takeoff = None 32 | 33 | # subscribe to the joystick at the end to make sure that all required 34 | # services were found 35 | self._buttons = None 36 | rospy.Subscriber(joy_topic, Joy, self._joyChanged) 37 | 38 | def _joyChanged(self, data): 39 | for i in range(0, len(data.buttons)): 40 | if self._buttons == None or data.buttons[i] != self._buttons[i]: 41 | if i == 0 and data.buttons[i] == 1 and self._land != None: 42 | self._land() 43 | if i == 1 and data.buttons[i] == 1: 44 | self._emergency() 45 | if i == 2 and data.buttons[i] == 1 and self._takeoff != None: 46 | self._takeoff() 47 | if i == 4 and data.buttons[i] == 1: 48 | value = int(rospy.get_param("ring/headlightEnable")) 49 | if value == 0: 50 | rospy.set_param("ring/headlightEnable", 1) 51 | else: 52 | rospy.set_param("ring/headlightEnable", 0) 53 | self._update_params(["ring/headlightEnable"]) 54 | print(not value) 55 | 56 | self._buttons = data.buttons 57 | 58 | if __name__ == '__main__': 59 | rospy.init_node('crazyflie_demo_controller', anonymous=True) 60 | use_controller = rospy.get_param("~use_crazyflie_controller", False) 61 | joy_topic = rospy.get_param("~joy_topic", "joy") 62 | controller = Controller(use_controller, joy_topic) 63 | rospy.spin() 64 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/crazyflie.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import numpy as np 5 | from crazyflie_driver.srv import * 6 | from crazyflie_driver.msg import TrajectoryPolynomialPiece 7 | 8 | def arrayToGeometryPoint(a): 9 | return geometry_msgs.msg.Point(a[0], a[1], a[2]) 10 | 11 | class Crazyflie: 12 | def __init__(self, prefix, tf): 13 | self.prefix = prefix 14 | self.tf = tf 15 | 16 | rospy.wait_for_service(prefix + "/set_group_mask") 17 | self.setGroupMaskService = rospy.ServiceProxy(prefix + "/set_group_mask", SetGroupMask) 18 | rospy.wait_for_service(prefix + "/takeoff") 19 | self.takeoffService = rospy.ServiceProxy(prefix + "/takeoff", Takeoff) 20 | rospy.wait_for_service(prefix + "/land") 21 | self.landService = rospy.ServiceProxy(prefix + "/land", Land) 22 | rospy.wait_for_service(prefix + "/stop") 23 | self.stopService = rospy.ServiceProxy(prefix + "/stop", Stop) 24 | rospy.wait_for_service(prefix + "/go_to") 25 | self.goToService = rospy.ServiceProxy(prefix + "/go_to", GoTo) 26 | rospy.wait_for_service(prefix + "/upload_trajectory") 27 | self.uploadTrajectoryService = rospy.ServiceProxy(prefix + "/upload_trajectory", UploadTrajectory) 28 | rospy.wait_for_service(prefix + "/start_trajectory") 29 | self.startTrajectoryService = rospy.ServiceProxy(prefix + "/start_trajectory", StartTrajectory) 30 | rospy.wait_for_service(prefix + "/update_params") 31 | self.updateParamsService = rospy.ServiceProxy(prefix + "/update_params", UpdateParams) 32 | 33 | def setGroupMask(self, groupMask): 34 | self.setGroupMaskService(groupMask) 35 | 36 | def takeoff(self, targetHeight, duration, groupMask = 0): 37 | self.takeoffService(groupMask, targetHeight, rospy.Duration.from_sec(duration)) 38 | 39 | def land(self, targetHeight, duration, groupMask = 0): 40 | self.landService(groupMask, targetHeight, rospy.Duration.from_sec(duration)) 41 | 42 | def stop(self, groupMask = 0): 43 | self.stopService(groupMask) 44 | 45 | def goTo(self, goal, yaw, duration, relative = False, groupMask = 0): 46 | gp = arrayToGeometryPoint(goal) 47 | self.goToService(groupMask, relative, gp, yaw, rospy.Duration.from_sec(duration)) 48 | 49 | def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory): 50 | pieces = [] 51 | for poly in trajectory.polynomials: 52 | piece = TrajectoryPolynomialPiece() 53 | piece.duration = rospy.Duration.from_sec(poly.duration) 54 | piece.poly_x = poly.px.p 55 | piece.poly_y = poly.py.p 56 | piece.poly_z = poly.pz.p 57 | piece.poly_yaw = poly.pyaw.p 58 | pieces.append(piece) 59 | self.uploadTrajectoryService(trajectoryId, pieceOffset, pieces) 60 | 61 | def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relative = True, groupMask = 0): 62 | self.startTrajectoryService(groupMask, trajectoryId, timescale, reverse, relative) 63 | 64 | def position(self): 65 | self.tf.waitForTransform("/world", "/cf" + str(self.id), rospy.Time(0), rospy.Duration(10)) 66 | position, quaternion = self.tf.lookupTransform("/world", "/cf" + str(self.id), rospy.Time(0)) 67 | return np.array(position) 68 | 69 | def getParam(self, name): 70 | return rospy.get_param(self.prefix + "/" + name) 71 | 72 | def setParam(self, name, value): 73 | rospy.set_param(self.prefix + "/" + name, value) 74 | self.updateParamsService([name]) 75 | 76 | def setParams(self, params): 77 | for name, value in params.iteritems(): 78 | rospy.set_param(self.prefix + "/" + name, value) 79 | self.updateParamsService(params.keys()) 80 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import math 5 | import tf 6 | import numpy as np 7 | import time 8 | from tf import TransformListener 9 | from geometry_msgs.msg import PoseStamped 10 | 11 | class Demo(): 12 | def __init__(self, goals): 13 | rospy.init_node('demo', anonymous=True) 14 | self.worldFrame = rospy.get_param("~worldFrame", "/world") 15 | self.frame = rospy.get_param("~frame") 16 | self.pubGoal = rospy.Publisher('goal', PoseStamped, queue_size=1) 17 | self.listener = TransformListener() 18 | self.goals = goals 19 | self.goalIndex = 0 20 | 21 | def run(self): 22 | self.listener.waitForTransform(self.worldFrame, self.frame, rospy.Time(), rospy.Duration(5.0)) 23 | goal = PoseStamped() 24 | goal.header.seq = 0 25 | goal.header.frame_id = self.worldFrame 26 | while not rospy.is_shutdown(): 27 | goal.header.seq += 1 28 | goal.header.stamp = rospy.Time.now() 29 | goal.pose.position.x = self.goals[self.goalIndex][0] 30 | goal.pose.position.y = self.goals[self.goalIndex][1] 31 | goal.pose.position.z = self.goals[self.goalIndex][2] 32 | quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goals[self.goalIndex][3]) 33 | goal.pose.orientation.x = quaternion[0] 34 | goal.pose.orientation.y = quaternion[1] 35 | goal.pose.orientation.z = quaternion[2] 36 | goal.pose.orientation.w = quaternion[3] 37 | 38 | self.pubGoal.publish(goal) 39 | 40 | t = self.listener.getLatestCommonTime(self.worldFrame, self.frame) 41 | if self.listener.canTransform(self.worldFrame, self.frame, t): 42 | position, quaternion = self.listener.lookupTransform(self.worldFrame, self.frame, t) 43 | rpy = tf.transformations.euler_from_quaternion(quaternion) 44 | if math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.3 \ 45 | and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.3 \ 46 | and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.3 \ 47 | and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10) \ 48 | and self.goalIndex < len(self.goals) - 1: 49 | rospy.sleep(self.goals[self.goalIndex][4]) 50 | self.goalIndex += 1 51 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/demo1.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from demo import Demo 3 | 4 | if __name__ == '__main__': 5 | demo = Demo( 6 | [ 7 | #x , y, z, yaw, sleep 8 | [0.0 , 0.0, 0.5, 0, 2], 9 | [1.5 , 0.0, 0.5, 0, 2], 10 | [-1.5 , 0.0, 0.75, 0, 2], 11 | [-1.5 , 0.5, 0.5, 0, 2], 12 | [0.0 , 0.0, 0.5, 0, 0], 13 | #[0.0 , 1.0, 0.3, 0], 14 | #[1.0 , 1.0, 0.5, 0], 15 | #[0.0 , 0.5, 0.2, 0], 16 | #[0.0 , 0.5, 0.0, 0], 17 | ] 18 | ) 19 | demo.run() -------------------------------------------------------------------------------- /crazyflie_demo/scripts/demo2.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from demo import Demo 3 | 4 | if __name__ == '__main__': 5 | demo = Demo( 6 | [ 7 | #x , y, z, yaw, sleep 8 | [0.0 , -1.0, 0.5, 0, 2], 9 | [1.5 , -1.0, 0.5, 0, 2], 10 | [-1.5 , -1.0, 0.75, 0, 2], 11 | [-1.5 , -1.5, 0.5, 0, 2], 12 | [0.0 , -1.0, 0.5, 0, 0], 13 | ] 14 | ) 15 | demo.run() 16 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/execute_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import rospy 5 | import tf_conversions 6 | from crazyflie_driver.msg import FullState 7 | import geometry_msgs 8 | import uav_trajectory 9 | 10 | 11 | if __name__ == '__main__': 12 | parser = argparse.ArgumentParser() 13 | parser.add_argument("trajectory", type=str, help="CSV file containing trajectory") 14 | args = parser.parse_args() 15 | 16 | rospy.init_node('full_state') 17 | 18 | traj = uav_trajectory.Trajectory() 19 | traj.loadcsv(args.trajectory) 20 | 21 | rate = rospy.Rate(100) 22 | 23 | msg = FullState() 24 | msg.header.seq = 0 25 | msg.header.stamp = rospy.Time.now() 26 | msg.header.frame_id = "/world" 27 | 28 | pub = rospy.Publisher("/crazyflie/cmd_full_state", FullState, queue_size=1) 29 | start_time = rospy.Time.now() 30 | 31 | while not rospy.is_shutdown(): 32 | msg.header.seq += 1 33 | msg.header.stamp = rospy.Time.now() 34 | t = (msg.header.stamp - start_time).to_sec() 35 | print(t) 36 | if t > traj.duration: 37 | break 38 | # t = traj.duration 39 | 40 | e = traj.eval(t) 41 | 42 | msg.pose.position.x = e.pos[0] 43 | msg.pose.position.y = e.pos[1] 44 | msg.pose.position.z = e.pos[2] 45 | msg.twist.linear.x = e.vel[0] 46 | msg.twist.linear.y = e.vel[1] 47 | msg.twist.linear.z = e.vel[2] 48 | msg.acc.x = e.acc[0] 49 | msg.acc.y = e.acc[1] 50 | msg.acc.z = e.acc[2] 51 | msg.pose.orientation = geometry_msgs.msg.Quaternion(*tf_conversions.transformations.quaternion_from_euler(0, 0, e.yaw)) 52 | msg.twist.angular.x = e.omega[0] 53 | msg.twist.angular.y = e.omega[1] 54 | msg.twist.angular.z = e.omega[2] 55 | 56 | pub.publish(msg) 57 | rate.sleep() 58 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/figure8.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.050000,0.000000,-0.000000,0.000000,-0.000000,0.830443,-0.276140,-0.384219,0.180493,-0.000000,0.000000,-0.000000,0.000000,-1.356107,0.688430,0.587426,-0.329106,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 0.710000,0.396058,0.918033,0.128965,-0.773546,0.339704,0.034310,-0.026417,-0.030049,-0.445604,-0.684403,0.888433,1.493630,-1.361618,-0.139316,0.158875,0.095799,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 0.620000,0.922409,0.405715,-0.582968,-0.092188,-0.114670,0.101046,0.075834,-0.037926,-0.291165,0.967514,0.421451,-1.086348,0.545211,0.030109,-0.050046,-0.068177,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 0.700000,0.923174,-0.431533,-0.682975,0.177173,0.319468,-0.043852,-0.111269,0.023166,0.289869,0.724722,-0.512011,-0.209623,-0.218710,0.108797,0.128756,-0.055461,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 0.560000,0.405364,-0.834716,0.158939,0.288175,-0.373738,-0.054995,0.036090,0.078627,0.450742,-0.385534,-0.954089,0.128288,0.442620,0.055630,-0.060142,-0.076163,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 0.560000,0.001062,-0.646270,-0.012560,-0.324065,0.125327,0.119738,0.034567,-0.063130,0.001593,-1.031457,0.015159,0.820816,-0.152665,-0.130729,-0.045679,0.080444,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 0.700000,-0.402804,-0.820508,-0.132914,0.236278,0.235164,-0.053551,-0.088687,0.031253,-0.449354,-0.411507,0.902946,0.185335,-0.239125,-0.041696,0.016857,0.016709,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 0.620000,-0.921641,-0.464596,0.661875,0.286582,-0.228921,-0.051987,0.004669,0.038463,-0.292459,0.777682,0.565788,-0.432472,-0.060568,-0.082048,-0.009439,0.041158,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 0.710000,-0.923935,0.447832,0.627381,-0.259808,-0.042325,-0.032258,0.001420,0.005294,0.288570,0.873350,-0.515586,-0.730207,-0.026023,0.288755,0.215678,-0.148061,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | 1.053185,-0.398611,0.850510,-0.144007,-0.485368,-0.079781,0.176330,0.234482,-0.153567,0.447039,-0.532729,-0.855023,0.878509,0.775168,-0.391051,-0.713519,0.391628,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 12 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/figure8withTakeoffAndLanding.csv: -------------------------------------------------------------------------------- 1 | Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7 2 | 1.20367,0,0,0,0,0.832848,-1.72666,1.20348,-0.285653,0,0,0,0,0.411004,-0.848433,0.589231,-0.139607,0,0,0,0,4.3626,-8.02347,5.27481,-1.20938,0,0,0,0,0,0,0,0 3 | 1.39935,0,-0.149141,-0.134608,-0.0175285,-2.17877,3.93034,-2.37529,0.487337,0,-0.077144,-0.0777131,-0.0188344,-0.70314,1.00636,-0.514888,0.0927063,0.5,0.440461,-0.081984,-0.116844,-1.53838,2.47463,-1.36944,0.263393,0,0,0,0,0,0,0,0 4 | 1.3624,-0.5,-0.012876,0.185585,-0.0205929,0.789539,-1.13473,0.590816,-0.108697,-0.5,-0.521826,-0.0104919,-0.0241408,-0.494533,1.28085,-0.910668,0.207379,0.5,-0.137313,0.0754433,0.0286866,0.19348,-0.327731,0.177371,-0.0330612,0,0,0,0,0,0,0,0 5 | 1.29387,0,0.590821,0.0177605,-0.00586113,0.524592,-1.53727,1.19257,-0.291994,-1,0.0132049,0.152778,0.00371847,1.98454,-3.47301,2.13239,-0.453618,0.5,0.0548397,-0.0327681,-0.0159661,-0.0662387,0.122721,-0.0677818,0.0127061,0,0,0,0,0,0,0,0 6 | 1.26367,0.5,-0.0322012,-0.134951,-0.00476974,-1.93497,3.39274,-2.10415,0.453869,-0.5,0.451126,-0.0261397,0.0148841,0.601052,-1.34921,0.968261,-0.230516,0.5,-0.0217914,0.0162344,0.00565815,0.0786492,-0.163052,0.107831,-0.024105,0,0,0,0,0,0,0,0 7 | 1.25438,0,-0.502111,-7.21775e-15,-0.0203045,-1.73375,3.87536,-2.77092,0.65924,0,0.255298,6.52969e-15,0.0118359,1.05111,-1.81042,1.11847,-0.241602,0.5,-5.67414e-15,-0.00567065,4.02574e-15,-0.0558094,0.123972,-0.0859765,0.0198541,0,0,0,0,0,0,0,0 8 | 1.33801,-0.5,0.0322012,0.134951,0.00269789,0.781159,-1.12474,0.584326,-0.107366,0.5,0.451126,0.0150303,0.0148841,1.03287,-2.18707,1.46484,-0.325982,0.5,0.0191704,0.0162344,-0.00282907,0.0808142,-0.215371,0.154965,-0.035498,0,0,0,0,0,0,0,0 9 | 1.33801,0,0.590821,0.0177605,-0.00251191,0.636615,-1.67673,1.22557,-0.286347,1,0.0170756,-0.152778,-0.00190136,-1.42862,2.3002,-1.31494,0.262443,0.5,-0.0681167,-0.0327681,0.0159661,0.0163781,0.13597,-0.143421,0.0389345,0,0,0,0,0,0,0,0 10 | 1.37517,0.5,-0.112436,-0.185585,-0.0205929,-2.23302,4.15094,-2.57015,0.538999,0.5,-0.521826,0.0104919,-0.0241408,-0.434982,1.13023,-0.80468,0.18277,0.5,0.139358,0.0754433,0.034381,0.867322,-2.11289,1.46461,-0.326827,0,0,0,0,0,0,0,0 11 | 1.17383,0,-0.0668803,0.134608,-0.0175285,-0.0901785,0.0029561,0.0602155,-0.0232904,0,-0.0366434,0.0777131,-0.0188344,-0.046709,0.0104186,0.0245163,-0.0105112,0.5,-0.472038,-0.0755315,0.116844,-3.23053,7.2325,-5.44632,1.3781,0,0,0,0,0,0,0,0 12 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/publish_external_pose_vicon.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from geometry_msgs.msg import PoseStamped, TransformStamped 6 | from crazyflie_driver.srv import UpdateParams, Takeoff, Land 7 | 8 | def onNewTransform(transform): 9 | global msg 10 | global pub 11 | global firstTransform 12 | 13 | if firstTransform: 14 | # initialize kalman filter 15 | rospy.set_param("kalman/initialX", transform.transform.translation.x) 16 | rospy.set_param("kalman/initialY", transform.transform.translation.y) 17 | rospy.set_param("kalman/initialZ", transform.transform.translation.z) 18 | update_params(["kalman/initialX", "kalman/initialY", "kalman/initialZ"]) 19 | 20 | rospy.set_param("kalman/resetEstimation", 1) 21 | # rospy.set_param("locSrv/extPosStdDev", 1e-4) 22 | update_params(["kalman/resetEstimation"]) #, "locSrv/extPosStdDev"]) 23 | firstTransform = False 24 | else: 25 | msg.header.frame_id = transform.header.frame_id 26 | msg.header.stamp = transform.header.stamp 27 | msg.header.seq += 1 28 | msg.pose.position.x = transform.transform.translation.x 29 | msg.pose.position.y = transform.transform.translation.y 30 | msg.pose.position.z = transform.transform.translation.z 31 | msg.pose.orientation.x = transform.rotation.x 32 | msg.pose.orientation.y = transform.rotation.y 33 | msg.pose.orientation.z = transform.rotation.z 34 | msg.pose.orientation.w = transform.rotation.w 35 | pub.publish(msg) 36 | 37 | 38 | if __name__ == '__main__': 39 | rospy.init_node('publish_external_position_vicon', anonymous=True) 40 | topic = rospy.get_param("~topic", "/vicon/cf/cf") 41 | 42 | rospy.wait_for_service('update_params') 43 | rospy.loginfo("found update_params service") 44 | update_params = rospy.ServiceProxy('update_params', UpdateParams) 45 | 46 | firstTransform = True 47 | 48 | msg = PoseStamped() 49 | msg.header.seq = 0 50 | msg.header.stamp = rospy.Time.now() 51 | 52 | pub = rospy.Publisher("external_pose", PoseStamped, queue_size=1) 53 | rospy.Subscriber(topic, TransformStamped, onNewTransform) 54 | 55 | rospy.spin() 56 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/publish_external_position_vicon.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from geometry_msgs.msg import PointStamped, TransformStamped 6 | from crazyflie_driver.srv import UpdateParams, Takeoff, Land 7 | 8 | def onNewTransform(transform): 9 | global msg 10 | global pub 11 | global firstTransform 12 | 13 | if firstTransform: 14 | # initialize kalman filter 15 | rospy.set_param("kalman/initialX", transform.transform.translation.x) 16 | rospy.set_param("kalman/initialY", transform.transform.translation.y) 17 | rospy.set_param("kalman/initialZ", transform.transform.translation.z) 18 | update_params(["kalman/initialX", "kalman/initialY", "kalman/initialZ"]) 19 | 20 | rospy.set_param("kalman/resetEstimation", 1) 21 | # rospy.set_param("locSrv/extPosStdDev", 1e-4) 22 | update_params(["kalman/resetEstimation"]) #, "locSrv/extPosStdDev"]) 23 | firstTransform = False 24 | else: 25 | msg.header.frame_id = transform.header.frame_id 26 | msg.header.stamp = transform.header.stamp 27 | msg.header.seq += 1 28 | msg.point.x = transform.transform.translation.x 29 | msg.point.y = transform.transform.translation.y 30 | msg.point.z = transform.transform.translation.z 31 | pub.publish(msg) 32 | 33 | 34 | if __name__ == '__main__': 35 | rospy.init_node('publish_external_position_vicon', anonymous=True) 36 | topic = rospy.get_param("~topic", "/vicon/cf/cf") 37 | 38 | rospy.wait_for_service('update_params') 39 | rospy.loginfo("found update_params service") 40 | update_params = rospy.ServiceProxy('update_params', UpdateParams) 41 | 42 | firstTransform = True 43 | 44 | msg = PointStamped() 45 | msg.header.seq = 0 46 | msg.header.stamp = rospy.Time.now() 47 | 48 | pub = rospy.Publisher("external_position", PointStamped, queue_size=1) 49 | rospy.Subscriber(topic, TransformStamped, onNewTransform) 50 | 51 | rospy.spin() 52 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/publish_external_position_vrpn.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from geometry_msgs.msg import PointStamped, TransformStamped, PoseStamped #PoseStamped added to support vrpn_client 6 | from crazyflie_driver.srv import UpdateParams 7 | 8 | def onNewTransform(pose): 9 | global msg 10 | global pub 11 | global firstTransform 12 | 13 | if firstTransform: 14 | # initialize kalman filter 15 | rospy.set_param("kalman/initialX", pose.pose.position.x) 16 | rospy.set_param("kalman/initialY", pose.pose.position.y) 17 | rospy.set_param("kalman/initialZ", pose.pose.position.z) 18 | update_params(["kalman/initialX", "kalman/initialY", "kalman/initialZ"]) 19 | 20 | rospy.set_param("kalman/resetEstimation", 1) 21 | update_params(["kalman/resetEstimation"]) 22 | firstTransform = False 23 | 24 | else: 25 | msg.header.frame_id = pose.header.frame_id 26 | msg.header.stamp = pose.header.stamp 27 | msg.header.seq += 1 28 | msg.point.x = pose.pose.position.x 29 | msg.point.y = pose.pose.position.y 30 | msg.point.z = pose.pose.position.z 31 | pub.publish(msg) 32 | 33 | 34 | if __name__ == '__main__': 35 | rospy.init_node('publish_external_position_vrpn', anonymous=True) 36 | topic = rospy.get_param("~topic", "/crazyflie1/vrpn_client_node/crazyflie1/pose") 37 | 38 | rospy.wait_for_service('update_params') 39 | rospy.loginfo("found update_params service") 40 | update_params = rospy.ServiceProxy('update_params', UpdateParams) 41 | 42 | firstTransform = True 43 | 44 | msg = PointStamped() 45 | msg.header.seq = 0 46 | msg.header.stamp = rospy.Time.now() 47 | 48 | pub = rospy.Publisher("external_position", PointStamped, queue_size=1) 49 | rospy.Subscriber(topic, PoseStamped, onNewTransform) 50 | 51 | rospy.spin() 52 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/publish_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from geometry_msgs.msg import PoseStamped 6 | 7 | if __name__ == '__main__': 8 | rospy.init_node('publish_pose', anonymous=True) 9 | worldFrame = rospy.get_param("~worldFrame", "/world") 10 | name = rospy.get_param("~name") 11 | r = rospy.get_param("~rate") 12 | x = rospy.get_param("~x") 13 | y = rospy.get_param("~y") 14 | z = rospy.get_param("~z") 15 | 16 | rate = rospy.Rate(r) 17 | 18 | msg = PoseStamped() 19 | msg.header.seq = 0 20 | msg.header.stamp = rospy.Time.now() 21 | msg.header.frame_id = worldFrame 22 | msg.pose.position.x = x 23 | msg.pose.position.y = y 24 | msg.pose.position.z = z 25 | quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) 26 | msg.pose.orientation.x = quaternion[0] 27 | msg.pose.orientation.y = quaternion[1] 28 | msg.pose.orientation.z = quaternion[2] 29 | msg.pose.orientation.w = quaternion[3] 30 | 31 | pub = rospy.Publisher(name, PoseStamped, queue_size=1) 32 | 33 | while not rospy.is_shutdown(): 34 | msg.header.seq += 1 35 | msg.header.stamp = rospy.Time.now() 36 | pub.publish(msg) 37 | rate.sleep() 38 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/publish_pose_teleop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from geometry_msgs.msg import PoseStamped 6 | from sensor_msgs.msg import Joy 7 | from math import fabs 8 | 9 | lastData = None 10 | 11 | def joyChanged(data): 12 | global lastData 13 | lastData = data 14 | # print(data) 15 | 16 | if __name__ == '__main__': 17 | rospy.init_node('publish_pose', anonymous=True) 18 | worldFrame = rospy.get_param("~worldFrame", "/world") 19 | name = rospy.get_param("~name") 20 | r = rospy.get_param("~rate") 21 | joy_topic = rospy.get_param("~joy_topic", "joy") 22 | x = rospy.get_param("~x") 23 | y = rospy.get_param("~y") 24 | z = rospy.get_param("~z") 25 | 26 | rate = rospy.Rate(r) 27 | 28 | msg = PoseStamped() 29 | msg.header.seq = 0 30 | msg.header.stamp = rospy.Time.now() 31 | msg.header.frame_id = worldFrame 32 | msg.pose.position.x = x 33 | msg.pose.position.y = y 34 | msg.pose.position.z = z 35 | yaw = 0 36 | quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw) 37 | msg.pose.orientation.x = quaternion[0] 38 | msg.pose.orientation.y = quaternion[1] 39 | msg.pose.orientation.z = quaternion[2] 40 | msg.pose.orientation.w = quaternion[3] 41 | 42 | pub = rospy.Publisher(name, PoseStamped, queue_size=1) 43 | rospy.Subscriber(joy_topic, Joy, joyChanged) 44 | 45 | while not rospy.is_shutdown(): 46 | global lastData 47 | if lastData != None: 48 | if fabs(lastData.axes[1]) > 0.1: 49 | msg.pose.position.z += lastData.axes[1] / r / 2 50 | if fabs(lastData.axes[4]) > 0.1: 51 | msg.pose.position.x += lastData.axes[4] / r * 1 52 | if fabs(lastData.axes[3]) > 0.1: 53 | msg.pose.position.y += lastData.axes[3] / r * 1 54 | if fabs(lastData.axes[0]) > 0.1: 55 | yaw += lastData.axes[0] / r * 2 56 | quaternion = tf.transformations.quaternion_from_euler(0, 0, yaw) 57 | msg.pose.orientation.x = quaternion[0] 58 | msg.pose.orientation.y = quaternion[1] 59 | msg.pose.orientation.z = quaternion[2] 60 | msg.pose.orientation.w = quaternion[3] 61 | # print(pose) 62 | msg.header.seq += 1 63 | msg.header.stamp = rospy.Time.now() 64 | pub.publish(msg) 65 | rate.sleep() 66 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/takeoff.csv: -------------------------------------------------------------------------------- 1 | Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7 2 | 1.15839,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1.06614,-1.57474,0.868105,-0.172428,0,0,0,0,0,0,0,0 3 | 1.13522,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.25,0.399333,3.03717e-14,-0.0697493,0.0551273,-0.643041,0.742072,-0.234074,0,0,0,0,0,0,0,0 4 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/test_high_level.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import crazyflie 5 | import time 6 | import uav_trajectory 7 | 8 | if __name__ == '__main__': 9 | rospy.init_node('test_high_level') 10 | 11 | cf = crazyflie.Crazyflie("crazyflie", "/vicon/crazyflie/crazyflie") 12 | 13 | cf.setParam("commander/enHighLevel", 1) 14 | cf.setParam("stabilizer/estimator", 2) # Use EKF 15 | cf.setParam("stabilizer/controller", 2) # Use mellinger controller 16 | 17 | # reset kalman 18 | cf.setParam("kalman/resetEstimation", 1) 19 | 20 | # cf.takeoff(targetHeight = 0.5, duration = 2.0) 21 | # time.sleep(3.0) 22 | 23 | # cf.goTo(goal = [0.5, 0.0, 0.0], yaw=0.2, duration = 2.0, relative = True) 24 | # time.sleep(3.0) 25 | 26 | # cf.land(targetHeight = 0.0, duration = 2.0) 27 | 28 | traj1 = uav_trajectory.Trajectory() 29 | traj1.loadcsv("takeoff.csv") 30 | 31 | traj2 = uav_trajectory.Trajectory() 32 | traj2.loadcsv("figure8.csv") 33 | 34 | print(traj1.duration) 35 | 36 | cf.uploadTrajectory(0, 0, traj1) 37 | cf.uploadTrajectory(1, len(traj1.polynomials), traj2) 38 | 39 | cf.startTrajectory(0, timescale=1.0) 40 | time.sleep(traj1.duration * 2.0) 41 | 42 | cf.startTrajectory(1, timescale=2.0) 43 | time.sleep(traj2.duration * 2.0) 44 | 45 | cf.startTrajectory(0, timescale=1.0, reverse=True) 46 | time.sleep(traj1.duration * 1.0) 47 | 48 | cf.stop() 49 | -------------------------------------------------------------------------------- /crazyflie_demo/scripts/uav_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | 4 | def normalize(v): 5 | norm = np.linalg.norm(v) 6 | assert norm > 0 7 | return v / norm 8 | 9 | 10 | class Polynomial: 11 | def __init__(self, p): 12 | self.p = p 13 | 14 | # evaluate a polynomial using horner's rule 15 | def eval(self, t): 16 | assert t >= 0 17 | x = 0.0 18 | for i in range(0, len(self.p)): 19 | x = x * t + self.p[len(self.p) - 1 - i] 20 | return x 21 | 22 | # compute and return derivative 23 | def derivative(self): 24 | return Polynomial([(i+1) * self.p[i+1] for i in range(0, len(self.p) - 1)]) 25 | 26 | 27 | class TrajectoryOutput: 28 | def __init__(self): 29 | self.pos = None # position [m] 30 | self.vel = None # velocity [m/s] 31 | self.acc = None # acceleration [m/s^2] 32 | self.omega = None # angular velocity [rad/s] 33 | self.yaw = None # yaw angle [rad] 34 | 35 | 36 | # 4d single polynomial piece for x-y-z-yaw, includes duration. 37 | class Polynomial4D: 38 | def __init__(self, duration, px, py, pz, pyaw): 39 | self.duration = duration 40 | self.px = Polynomial(px) 41 | self.py = Polynomial(py) 42 | self.pz = Polynomial(pz) 43 | self.pyaw = Polynomial(pyaw) 44 | 45 | # compute and return derivative 46 | def derivative(self): 47 | return Polynomial4D( 48 | self.duration, 49 | self.px.derivative().p, 50 | self.py.derivative().p, 51 | self.pz.derivative().p, 52 | self.pyaw.derivative().p) 53 | 54 | def eval(self, t): 55 | result = TrajectoryOutput() 56 | # flat variables 57 | result.pos = np.array([self.px.eval(t), self.py.eval(t), self.pz.eval(t)]) 58 | result.yaw = self.pyaw.eval(t) 59 | 60 | # 1st derivative 61 | derivative = self.derivative() 62 | result.vel = np.array([derivative.px.eval(t), derivative.py.eval(t), derivative.pz.eval(t)]) 63 | dyaw = derivative.pyaw.eval(t) 64 | 65 | # 2nd derivative 66 | derivative2 = derivative.derivative() 67 | result.acc = np.array([derivative2.px.eval(t), derivative2.py.eval(t), derivative2.pz.eval(t)]) 68 | 69 | # 3rd derivative 70 | derivative3 = derivative2.derivative() 71 | jerk = np.array([derivative3.px.eval(t), derivative3.py.eval(t), derivative3.pz.eval(t)]) 72 | 73 | thrust = result.acc + np.array([0, 0, 9.81]) # add gravity 74 | 75 | z_body = normalize(thrust) 76 | x_world = np.array([np.cos(result.yaw), np.sin(result.yaw), 0]) 77 | y_body = normalize(np.cross(z_body, x_world)) 78 | x_body = np.cross(y_body, z_body) 79 | 80 | jerk_orth_zbody = jerk - (np.dot(jerk, z_body) * z_body) 81 | h_w = jerk_orth_zbody / np.linalg.norm(thrust) 82 | 83 | result.omega = np.array([-np.dot(h_w, y_body), np.dot(h_w, x_body), z_body[2] * dyaw]) 84 | return result 85 | 86 | 87 | class Trajectory: 88 | def __init__(self): 89 | self.polynomials = None 90 | self.duration = None 91 | 92 | def loadcsv(self, filename): 93 | data = np.loadtxt(filename, delimiter=",", skiprows=1, usecols=range(33)) 94 | self.polynomials = [Polynomial4D(row[0], row[1:9], row[9:17], row[17:25], row[25:33]) for row in data] 95 | self.duration = np.sum(data[:,0]) 96 | 97 | def eval(self, t): 98 | assert t >= 0 99 | assert t <= self.duration 100 | 101 | current_t = 0.0 102 | for p in self.polynomials: 103 | if t < current_t + p.duration: 104 | return p.eval(t - current_t) 105 | current_t = current_t + p.duration 106 | -------------------------------------------------------------------------------- /crazyflie_demo/src/quadrotor_teleop.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2012, Johannes Meyer, TU Darmstadt 3 | // 2018, Wolfgang Hoenig, USC 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 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of the Flight Systems and Automatic Control group, 14 | // TU Darmstadt, nor the names of its contributors may be used to 15 | // endorse or promote products derived from this software without 16 | // specific prior written permission. 17 | 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 22 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | //================================================================================================= 29 | 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | class Teleop 36 | { 37 | private: 38 | ros::NodeHandle node_handle_; 39 | ros::Subscriber joy_subscriber_; 40 | 41 | ros::Publisher velocity_publisher_; 42 | geometry_msgs::Twist velocity_; 43 | 44 | struct Axis 45 | { 46 | int axis; 47 | double max; 48 | }; 49 | 50 | struct Button 51 | { 52 | int button; 53 | }; 54 | 55 | struct 56 | { 57 | Axis x; 58 | Axis y; 59 | Axis z; 60 | Axis yaw; 61 | } axes_; 62 | 63 | double frequency_; 64 | 65 | public: 66 | Teleop() 67 | { 68 | ros::NodeHandle params("~"); 69 | 70 | params.param("x_axis", axes_.x.axis, 4); 71 | params.param("y_axis", axes_.y.axis, 3); 72 | params.param("z_axis", axes_.z.axis, 2); 73 | params.param("yaw_axis", axes_.yaw.axis, 1); 74 | 75 | params.param("yaw_velocity_max", axes_.yaw.max, 90.0 * M_PI / 180.0); 76 | 77 | params.param("x_velocity_max", axes_.x.max, 2.0); 78 | params.param("y_velocity_max", axes_.y.max, 2.0); 79 | params.param("z_velocity_max", axes_.z.max, 2.0); 80 | 81 | params.param("frequency", frequency_, 100); 82 | 83 | joy_subscriber_ = node_handle_.subscribe("joy", 1, boost::bind(&Teleop::joyTwistCallback, this, _1)); 84 | velocity_publisher_ = node_handle_.advertise("cmd_vel", 10); 85 | } 86 | 87 | ~Teleop() 88 | { 89 | stop(); 90 | } 91 | 92 | void execute() 93 | { 94 | ros::Rate loop_rate(frequency_); 95 | while (ros::ok()) { 96 | velocity_publisher_.publish(velocity_); 97 | 98 | ros::spinOnce(); 99 | loop_rate.sleep(); 100 | } 101 | } 102 | 103 | void joyTwistCallback(const sensor_msgs::JoyConstPtr &joy) 104 | { 105 | velocity_.linear.x = getAxis(joy, axes_.x); 106 | velocity_.linear.y = getAxis(joy, axes_.y); 107 | velocity_.linear.z = getAxis(joy, axes_.z); 108 | velocity_.angular.z = getAxis(joy, axes_.yaw); 109 | } 110 | 111 | sensor_msgs::Joy::_axes_type::value_type getAxis(const sensor_msgs::JoyConstPtr &joy, Axis axis) 112 | { 113 | if (axis.axis == 0) { 114 | return 0; 115 | } 116 | sensor_msgs::Joy::_axes_type::value_type sign = 1.0; 117 | if (axis.axis < 0) { 118 | sign = -1.0; 119 | axis.axis = -axis.axis; 120 | } 121 | if ((size_t) axis.axis > joy->axes.size()) { 122 | return 0; 123 | } 124 | return sign * joy->axes[axis.axis - 1] * axis.max; 125 | } 126 | 127 | sensor_msgs::Joy::_buttons_type::value_type getButton(const sensor_msgs::JoyConstPtr &joy, int button) 128 | { 129 | if (button <= 0) { 130 | return 0; 131 | } 132 | if ((size_t) button > joy->buttons.size()) { 133 | return 0; 134 | } 135 | return joy->buttons[button - 1]; 136 | } 137 | 138 | void stop() 139 | { 140 | if(velocity_publisher_.getNumSubscribers() > 0) { 141 | velocity_ = geometry_msgs::Twist(); 142 | velocity_publisher_.publish(velocity_); 143 | } 144 | } 145 | }; 146 | 147 | int main(int argc, char **argv) 148 | { 149 | ros::init(argc, argv, "quadrotor_teleop"); 150 | 151 | Teleop teleop; 152 | teleop.execute(); 153 | 154 | return 0; 155 | } 156 | -------------------------------------------------------------------------------- /crazyflie_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(crazyflie_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 28 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 29 | ## pulled in transitively but can be declared for certainty nonetheless: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################### 70 | ## catkin specific configuration ## 71 | ################################### 72 | ## The catkin_package macro generates cmake config files for your package 73 | ## Declare things to be passed to dependent projects 74 | ## INCLUDE_DIRS: uncomment this if you package contains header files 75 | ## LIBRARIES: libraries you create in this project that dependent projects also need 76 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 77 | ## DEPENDS: system dependencies of this project that dependent projects also need 78 | catkin_package( 79 | # INCLUDE_DIRS include 80 | # LIBRARIES crazyflie_description 81 | # CATKIN_DEPENDS other_catkin_pkg 82 | # DEPENDS system_lib 83 | ) 84 | 85 | ########### 86 | ## Build ## 87 | ########### 88 | 89 | ## Specify additional locations of header files 90 | ## Your package locations should be listed before other locations 91 | # include_directories(include) 92 | 93 | ## Declare a cpp library 94 | # add_library(crazyflie_description 95 | # src/${PROJECT_NAME}/crazyflie_description.cpp 96 | # ) 97 | 98 | ## Declare a cpp executable 99 | # add_executable(crazyflie_description_node src/crazyflie_description_node.cpp) 100 | 101 | ## Add cmake target dependencies of the executable/library 102 | ## as an example, message headers may need to be generated before nodes 103 | # add_dependencies(crazyflie_description_node crazyflie_description_generate_messages_cpp) 104 | 105 | ## Specify libraries to link a library or executable target against 106 | # target_link_libraries(crazyflie_description_node 107 | # ${catkin_LIBRARIES} 108 | # ) 109 | 110 | ############# 111 | ## Install ## 112 | ############# 113 | 114 | # all install targets should use catkin DESTINATION variables 115 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 116 | 117 | ## Mark executable scripts (Python etc.) for installation 118 | ## in contrast to setup.py, you can choose the destination 119 | # install(PROGRAMS 120 | # scripts/my_python_script 121 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 122 | # ) 123 | 124 | ## Mark executables and/or libraries for installation 125 | # install(TARGETS crazyflie_description crazyflie_description_node 126 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 127 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 128 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 129 | # ) 130 | 131 | ## Mark cpp header files for installation 132 | # install(DIRECTORY include/${PROJECT_NAME}/ 133 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 134 | # FILES_MATCHING PATTERN "*.h" 135 | # PATTERN ".svn" EXCLUDE 136 | # ) 137 | 138 | ## Mark other files for installation (e.g. launch and bag files, etc.) 139 | # install(FILES 140 | # # myfile1 141 | # # myfile2 142 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 143 | # ) 144 | 145 | ############# 146 | ## Testing ## 147 | ############# 148 | 149 | ## Add gtest based cpp test target and link libraries 150 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_crazyflie_description.cpp) 151 | # if(TARGET ${PROJECT_NAME}-test) 152 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 153 | # endif() 154 | 155 | ## Add folders to be run by python nosetests 156 | # catkin_add_nosetests(test) 157 | -------------------------------------------------------------------------------- /crazyflie_description/README.md: -------------------------------------------------------------------------------- 1 | crazyflie_description 2 | ===================== 3 | 4 | URDF Model for the Crazyflie Nano Quadcopter from Bitcraze. 5 | 6 | ## Model 7 | 8 | * The model is based on https://github.com/bitcraze/bitcraze-mechanics/blob/master/models/cf_model_s_revA.skp 9 | * Use SketchUp (Tested with 14.1) to update/change the model 10 | * Export using the following options: ![Export settings](export.png) 11 | 12 | ## Issues 13 | 14 | * Inertia matrix not correct yet 15 | * No collision model yet 16 | -------------------------------------------------------------------------------- /crazyflie_description/export.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whoenig/crazyflie_ros/df3ce76f700953cfa2794b8fd4628ea6b93c92c4/crazyflie_description/export.png -------------------------------------------------------------------------------- /crazyflie_description/launch/crazyflie.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 81 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /RobotModel1 10 | Splitter Ratio: 0.5 11 | Tree Height: 535 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.03 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: 0 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Alpha: 1 53 | Class: rviz/RobotModel 54 | Collision Enabled: false 55 | Enabled: true 56 | Links: 57 | All Links Enabled: true 58 | Expand Joint Details: false 59 | Expand Link Details: false 60 | Expand Tree: false 61 | Link Tree Style: "" 62 | base_link: 63 | Alpha: 1 64 | Show Axes: false 65 | Show Trail: false 66 | Value: true 67 | Name: RobotModel 68 | Robot Description: robot_description 69 | TF Prefix: "" 70 | Update Interval: 0 71 | Value: true 72 | Visual Enabled: true 73 | Enabled: true 74 | Global Options: 75 | Background Color: 48; 48; 48 76 | Fixed Frame: base_link 77 | Frame Rate: 30 78 | Name: root 79 | Tools: 80 | - Class: rviz/Interact 81 | Hide Inactive Objects: true 82 | - Class: rviz/MoveCamera 83 | - Class: rviz/Select 84 | - Class: rviz/FocusCamera 85 | - Class: rviz/Measure 86 | - Class: rviz/SetInitialPose 87 | Topic: /initialpose 88 | - Class: rviz/SetGoal 89 | Topic: /move_base_simple/goal 90 | - Class: rviz/PublishPoint 91 | Single click: true 92 | Topic: /clicked_point 93 | Value: true 94 | Views: 95 | Current: 96 | Class: rviz/Orbit 97 | Distance: 0.198154 98 | Enable Stereo Rendering: 99 | Stereo Eye Separation: 0.06 100 | Stereo Focal Distance: 1 101 | Swap Stereo Eyes: false 102 | Value: false 103 | Focal Point: 104 | X: 0 105 | Y: 0 106 | Z: 0 107 | Name: Current View 108 | Near Clip Distance: 0.01 109 | Pitch: 0.785398 110 | Target Frame: 111 | Value: Orbit (rviz) 112 | Yaw: 0.785398 113 | Saved: ~ 114 | Window Geometry: 115 | Displays: 116 | collapsed: false 117 | Height: 846 118 | Hide Left Dock: false 119 | Hide Right Dock: false 120 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000002a9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000002a9000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000043000002a9000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000023a00fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002a900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 121 | Selection: 122 | collapsed: false 123 | Time: 124 | collapsed: false 125 | Tool Properties: 126 | collapsed: false 127 | Views: 128 | collapsed: false 129 | Width: 1200 130 | X: 60 131 | Y: 60 132 | -------------------------------------------------------------------------------- /crazyflie_description/launch/crazyflie2_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /crazyflie_description/launch/crazyflie_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /crazyflie_description/meshes/crazyflie.skp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/whoenig/crazyflie_ros/df3ce76f700953cfa2794b8fd4628ea6b93c92c4/crazyflie_description/meshes/crazyflie.skp -------------------------------------------------------------------------------- /crazyflie_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | crazyflie_description 4 | 0.0.1 5 | URDF Model for the Crazyflie Nano Quadcopter from Bitcraze 6 | 7 | Wolfgang Hoenig 8 | 9 | MIT 10 | 11 | http://ros.org/wiki/crazyflie 12 | https://github.com/whoenig/crazyflie_ros/issues 13 | 14 | Wolfgang Hoenig 15 | 16 | catkin 17 | 18 | 19 | -------------------------------------------------------------------------------- /crazyflie_description/urdf/crazyflie.urdf.xacro: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /crazyflie_description/urdf/crazyflie2.urdf.xacro: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /crazyflie_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(crazyflie_driver) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | message_generation 9 | std_msgs 10 | tf 11 | crazyflie_cpp 12 | ) 13 | 14 | set (CMAKE_CXX_STANDARD 11) 15 | set (CMAKE_CXX_STANDARD_REQUIRED ON) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | # catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## Generate services in the 'srv' folder 31 | add_service_files( 32 | FILES 33 | AddCrazyflie.srv 34 | GoTo.srv 35 | Land.srv 36 | NotifySetpointsStop.srv 37 | RemoveCrazyflie.srv 38 | SetGroupMask.srv 39 | StartTrajectory.srv 40 | Stop.srv 41 | Takeoff.srv 42 | UpdateParams.srv 43 | UploadTrajectory.srv 44 | sendPacket.srv 45 | ) 46 | 47 | add_message_files( 48 | FILES 49 | LogBlock.msg 50 | GenericLogData.msg 51 | FullState.msg 52 | VelocityWorld.msg 53 | TrajectoryPolynomialPiece.msg 54 | crtpPacket.msg 55 | Hover.msg 56 | Position.msg 57 | ) 58 | 59 | ## Generate added messages and services with any dependencies listed here 60 | generate_messages( 61 | DEPENDENCIES 62 | std_msgs 63 | geometry_msgs 64 | ) 65 | 66 | ################################### 67 | ## catkin specific configuration ## 68 | ################################### 69 | ## The catkin_package macro generates cmake config files for your package 70 | ## Declare things to be passed to dependent projects 71 | ## INCLUDE_DIRS: uncomment this if you package contains header files 72 | ## LIBRARIES: libraries you create in this project that dependent projects also need 73 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 74 | ## DEPENDS: system dependencies of this project that dependent projects also need 75 | catkin_package( 76 | # INCLUDE_DIRS include 77 | # LIBRARIES crazyflie 78 | CATKIN_DEPENDS 79 | message_runtime 80 | std_msgs 81 | tf 82 | crazyflie_cpp 83 | # DEPENDS system_lib 84 | ) 85 | 86 | ########### 87 | ## Build ## 88 | ########### 89 | 90 | include_directories( 91 | ${catkin_INCLUDE_DIRS} 92 | ) 93 | 94 | ## Declare a cpp executable 95 | add_executable(crazyflie_server 96 | src/crazyflie_server.cpp 97 | ) 98 | 99 | ## Add cmake target dependencies of the executable/library 100 | ## as an example, message headers may need to be generated before nodes 101 | add_dependencies(crazyflie_server 102 | crazyflie_driver_generate_messages_cpp 103 | ) 104 | 105 | ## Specify libraries to link a library or executable target against 106 | target_link_libraries(crazyflie_server 107 | ${catkin_LIBRARIES} 108 | ) 109 | 110 | ## Declare a cpp executable 111 | add_executable(crazyflie_add 112 | src/crazyflie_add.cpp 113 | ) 114 | 115 | add_dependencies(crazyflie_add 116 | crazyflie_driver_generate_messages_cpp 117 | ) 118 | 119 | target_link_libraries(crazyflie_add 120 | ${catkin_LIBRARIES} 121 | ) 122 | 123 | ############# 124 | ## Install ## 125 | ############# 126 | 127 | # all install targets should use catkin DESTINATION variables 128 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 129 | 130 | ## Mark executable scripts (Python etc.) for installation 131 | ## in contrast to setup.py, you can choose the destination 132 | # install(PROGRAMS 133 | # scripts/my_python_script 134 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 135 | # ) 136 | 137 | ## Mark executables and/or libraries for installation 138 | # install(TARGETS crazyflie crazyflie_node 139 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 140 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 141 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 142 | # ) 143 | 144 | ## Mark cpp header files for installation 145 | # install(DIRECTORY include/${PROJECT_NAME}/ 146 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 147 | # FILES_MATCHING PATTERN "*.h" 148 | # PATTERN ".svn" EXCLUDE 149 | # ) 150 | 151 | ## Mark other files for installation (e.g. launch and bag files, etc.) 152 | # install(FILES 153 | # # myfile1 154 | # # myfile2 155 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 156 | # ) 157 | 158 | ############# 159 | ## Testing ## 160 | ############# 161 | 162 | ## Add gtest based cpp test target and link libraries 163 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_crazyflie.cpp) 164 | # if(TARGET ${PROJECT_NAME}-test) 165 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 166 | # endif() 167 | 168 | ## Add folders to be run by python nosetests 169 | # catkin_add_nosetests(test) 170 | -------------------------------------------------------------------------------- /crazyflie_driver/launch/crazyflie_add.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 | -------------------------------------------------------------------------------- /crazyflie_driver/launch/crazyflie_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /crazyflie_driver/msg/FullState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose pose 3 | geometry_msgs/Twist twist 4 | geometry_msgs/Vector3 acc 5 | -------------------------------------------------------------------------------- /crazyflie_driver/msg/GenericLogData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64[] values 3 | -------------------------------------------------------------------------------- /crazyflie_driver/msg/Hover.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 vx 3 | float32 vy 4 | float32 yawrate 5 | float32 zDistance 6 | -------------------------------------------------------------------------------- /crazyflie_driver/msg/LogBlock.msg: -------------------------------------------------------------------------------- 1 | string topic_name 2 | int16 frequency 3 | string[] variables 4 | -------------------------------------------------------------------------------- /crazyflie_driver/msg/Position.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 x 3 | float32 y 4 | float32 z 5 | float32 yaw 6 | -------------------------------------------------------------------------------- /crazyflie_driver/msg/TrajectoryPolynomialPiece.msg: -------------------------------------------------------------------------------- 1 | # 2 | 3 | float32[] poly_x 4 | float32[] poly_y 5 | float32[] poly_z 6 | float32[] poly_yaw 7 | duration duration -------------------------------------------------------------------------------- /crazyflie_driver/msg/VelocityWorld.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Vector3 vel 3 | float32 yawRate -------------------------------------------------------------------------------- /crazyflie_driver/msg/crtpPacket.msg: -------------------------------------------------------------------------------- 1 | uint8 size 2 | uint8 header 3 | uint8[30] data 4 | -------------------------------------------------------------------------------- /crazyflie_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | crazyflie_driver 4 | 0.0.1 5 | ROS driver for Crazyflie Nano Quadcopter from Bitcraze 6 | 7 | Wolfgang Hoenig 8 | 9 | MIT 10 | 11 | http://ros.org/wiki/crazyflie 12 | https://github.com/whoenig/crazyflie_ros/issues 13 | 14 | Wolfgang Hoenig 15 | 16 | 17 | catkin 18 | 19 | message_generation 20 | std_msgs 21 | tf 22 | crazyflie_cpp 23 | crazyflie_demo 24 | 25 | message_runtime 26 | std_msgs 27 | tf 28 | crazyflie_cpp 29 | crazyflie_demo 30 | 31 | 32 | -------------------------------------------------------------------------------- /crazyflie_driver/src/crazyflie_add.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "crazyflie_driver/AddCrazyflie.h" 3 | #include "crazyflie_driver/LogBlock.h" 4 | 5 | int main(int argc, char **argv) 6 | { 7 | ros::init(argc, argv, "crazyflie_add", ros::init_options::AnonymousName); 8 | ros::NodeHandle n("~"); 9 | 10 | // read paramaters 11 | std::string uri; 12 | std::string tf_prefix; 13 | double roll_trim; 14 | double pitch_trim; 15 | bool enable_logging; 16 | bool enable_parameters; 17 | bool use_ros_time; 18 | bool enable_logging_imu; 19 | bool enable_logging_temperature; 20 | bool enable_logging_magnetic_field; 21 | bool enable_logging_pressure; 22 | bool enable_logging_battery; 23 | bool enable_logging_pose; 24 | bool enable_logging_packets; 25 | 26 | n.getParam("uri", uri); 27 | n.getParam("tf_prefix", tf_prefix); 28 | n.param("roll_trim", roll_trim, 0.0); 29 | n.param("pitch_trim", pitch_trim, 0.0); 30 | n.param("enable_logging", enable_logging, true); 31 | n.param("enable_parameters", enable_parameters, true); 32 | n.param("use_ros_time", use_ros_time, true); 33 | n.param("enable_logging_imu", enable_logging_imu, true); 34 | n.param("enable_logging_temperature", enable_logging_temperature, true); 35 | n.param("enable_logging_magnetic_field", enable_logging_magnetic_field, true); 36 | n.param("enable_logging_pressure", enable_logging_pressure, true); 37 | n.param("enable_logging_battery", enable_logging_battery, true); 38 | n.param("enable_logging_pose", enable_logging_pose, false); 39 | n.param("enable_logging_packets", enable_logging_packets, true); 40 | 41 | 42 | ROS_INFO("wait_for_service /add_crazyflie"); 43 | ros::ServiceClient addCrazyflieService = n.serviceClient("/add_crazyflie"); 44 | addCrazyflieService.waitForExistence(); 45 | ROS_INFO("found /add_crazyflie"); 46 | crazyflie_driver::AddCrazyflie addCrazyflie; 47 | addCrazyflie.request.uri = uri; 48 | addCrazyflie.request.tf_prefix = tf_prefix; 49 | addCrazyflie.request.roll_trim = roll_trim; 50 | addCrazyflie.request.pitch_trim = pitch_trim; 51 | addCrazyflie.request.enable_logging = enable_logging; 52 | addCrazyflie.request.enable_parameters = enable_parameters; 53 | addCrazyflie.request.use_ros_time = use_ros_time; 54 | addCrazyflie.request.enable_logging_imu = enable_logging_imu; 55 | addCrazyflie.request.enable_logging_temperature = enable_logging_temperature; 56 | addCrazyflie.request.enable_logging_magnetic_field = enable_logging_magnetic_field; 57 | addCrazyflie.request.enable_logging_pressure = enable_logging_pressure; 58 | addCrazyflie.request.enable_logging_battery = enable_logging_battery; 59 | addCrazyflie.request.enable_logging_pose = enable_logging_pose; 60 | addCrazyflie.request.enable_logging_packets = enable_logging_packets; 61 | 62 | std::vector genericLogTopics; 63 | n.param("genericLogTopics", genericLogTopics, std::vector()); 64 | std::vector genericLogTopicFrequencies; 65 | n.param("genericLogTopicFrequencies", genericLogTopicFrequencies, std::vector()); 66 | 67 | if (genericLogTopics.size() == genericLogTopicFrequencies.size()) 68 | { 69 | size_t i = 0; 70 | for (auto& topic : genericLogTopics) 71 | { 72 | crazyflie_driver::LogBlock logBlock; 73 | logBlock.topic_name = topic; 74 | logBlock.frequency = genericLogTopicFrequencies[i]; 75 | n.getParam("genericLogTopic_" + topic + "_Variables", logBlock.variables); 76 | addCrazyflie.request.log_blocks.push_back(logBlock); 77 | ++i; 78 | } 79 | } 80 | else 81 | { 82 | ROS_ERROR("Cardinality of genericLogTopics and genericLogTopicFrequencies does not match!"); 83 | } 84 | 85 | 86 | addCrazyflieService.call(addCrazyflie); 87 | 88 | return 0; 89 | } 90 | -------------------------------------------------------------------------------- /crazyflie_driver/src/crazyflie_server.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "crazyflie_driver/AddCrazyflie.h" 5 | #include "crazyflie_driver/GoTo.h" 6 | #include "crazyflie_driver/Land.h" 7 | #include "crazyflie_driver/NotifySetpointsStop.h" 8 | #include "crazyflie_driver/RemoveCrazyflie.h" 9 | #include "crazyflie_driver/SetGroupMask.h" 10 | #include "crazyflie_driver/StartTrajectory.h" 11 | #include "crazyflie_driver/Stop.h" 12 | #include "crazyflie_driver/Takeoff.h" 13 | #include "crazyflie_driver/UpdateParams.h" 14 | #include "crazyflie_driver/UploadTrajectory.h" 15 | #include "crazyflie_driver/sendPacket.h" 16 | 17 | #include "crazyflie_driver/LogBlock.h" 18 | #include "crazyflie_driver/GenericLogData.h" 19 | #include "crazyflie_driver/FullState.h" 20 | #include "crazyflie_driver/Hover.h" 21 | #include "crazyflie_driver/Stop.h" 22 | #include "crazyflie_driver/Position.h" 23 | #include "crazyflie_driver/VelocityWorld.h" 24 | #include "crazyflie_driver/crtpPacket.h" 25 | #include "crazyflie_cpp/Crazyradio.h" 26 | #include "crazyflie_cpp/crtp.h" 27 | #include "std_srvs/Empty.h" 28 | #include 29 | #include "geometry_msgs/Twist.h" 30 | #include "geometry_msgs/PointStamped.h" 31 | #include "geometry_msgs/PoseStamped.h" 32 | #include "sensor_msgs/Imu.h" 33 | #include "sensor_msgs/Temperature.h" 34 | #include "sensor_msgs/MagneticField.h" 35 | #include "std_msgs/Float32.h" 36 | 37 | //#include 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | 46 | constexpr double pi() { return 3.141592653589793238462643383279502884; } 47 | 48 | double degToRad(double deg) { 49 | return deg / 180.0 * pi(); 50 | } 51 | 52 | double radToDeg(double rad) { 53 | return rad * 180.0 / pi(); 54 | } 55 | 56 | class ROSLogger : public Logger 57 | { 58 | public: 59 | ROSLogger() 60 | : Logger() 61 | { 62 | } 63 | 64 | virtual ~ROSLogger() {} 65 | 66 | virtual void info(const std::string& msg) 67 | { 68 | ROS_INFO("%s", msg.c_str()); 69 | } 70 | 71 | virtual void warning(const std::string& msg) 72 | { 73 | ROS_WARN("%s", msg.c_str()); 74 | } 75 | 76 | virtual void error(const std::string& msg) 77 | { 78 | ROS_ERROR("%s", msg.c_str()); 79 | } 80 | }; 81 | 82 | static ROSLogger rosLogger; 83 | 84 | class CrazyflieROS 85 | { 86 | public: 87 | CrazyflieROS( 88 | const std::string& link_uri, 89 | const std::string& tf_prefix, 90 | float roll_trim, 91 | float pitch_trim, 92 | bool enable_logging, 93 | bool enable_parameters, 94 | std::vector& log_blocks, 95 | bool use_ros_time, 96 | bool enable_logging_imu, 97 | bool enable_logging_temperature, 98 | bool enable_logging_magnetic_field, 99 | bool enable_logging_pressure, 100 | bool enable_logging_battery, 101 | bool enable_logging_pose, 102 | bool enable_logging_packets) 103 | : m_tf_prefix(tf_prefix) 104 | , m_cf( 105 | link_uri, 106 | rosLogger, 107 | std::bind(&CrazyflieROS::onConsole, this, std::placeholders::_1)) 108 | , m_isEmergency(false) 109 | , m_roll_trim(roll_trim) 110 | , m_pitch_trim(pitch_trim) 111 | , m_enableLogging(enable_logging) 112 | , m_enableParameters(enable_parameters) 113 | , m_logBlocks(log_blocks) 114 | , m_use_ros_time(use_ros_time) 115 | , m_enable_logging_imu(enable_logging_imu) 116 | , m_enable_logging_temperature(enable_logging_temperature) 117 | , m_enable_logging_magnetic_field(enable_logging_magnetic_field) 118 | , m_enable_logging_pressure(enable_logging_pressure) 119 | , m_enable_logging_battery(enable_logging_battery) 120 | , m_enable_logging_pose(enable_logging_pose) 121 | , m_enable_logging_packets(enable_logging_packets) 122 | , m_serviceEmergency() 123 | , m_serviceUpdateParams() 124 | , m_serviceSetGroupMask() 125 | , m_serviceTakeoff() 126 | , m_serviceLand() 127 | , m_serviceStop() 128 | , m_serviceGoTo() 129 | , m_serviceUploadTrajectory() 130 | , m_serviceStartTrajectory() 131 | , m_serviceNotifySetpointsStop() 132 | , m_subscribeCmdVel() 133 | , m_subscribeCmdFullState() 134 | , m_subscribeCmdVelocityWorld() 135 | , m_subscribeCmdHover() 136 | , m_subscribeCmdStop() 137 | , m_subscribeCmdPosition() 138 | , m_subscribeExternalPosition() 139 | , m_pubImu() 140 | , m_pubTemp() 141 | , m_pubMag() 142 | , m_pubPressure() 143 | , m_pubBattery() 144 | , m_pubRssi() 145 | , m_sentSetpoint(false) 146 | , m_sentExternalPosition(false) 147 | { 148 | m_thread = std::thread(&CrazyflieROS::run, this); 149 | } 150 | 151 | void stop() 152 | { 153 | ROS_INFO_NAMED(m_tf_prefix, "Disconnecting ..."); 154 | m_isEmergency = true; 155 | m_thread.join(); 156 | } 157 | 158 | /** 159 | * Service callback which transmits a packet to the crazyflie 160 | * @param req The service request, which contains a crtpPacket to transmit. 161 | * @param res The service response, which is not used. 162 | * @return returns true always 163 | */ 164 | bool sendPacket ( 165 | crazyflie_driver::sendPacket::Request &req, 166 | crazyflie_driver::sendPacket::Response &res) 167 | { 168 | /** Convert the message struct to the packet struct */ 169 | crtpPacket_t packet; 170 | packet.size = req.packet.size; 171 | packet.header = req.packet.header; 172 | for (int i = 0; i < CRTP_MAX_DATA_SIZE; i++) { 173 | packet.data[i] = req.packet.data[i]; 174 | } 175 | m_cf.queueOutgoingPacket(packet); 176 | return true; 177 | } 178 | 179 | private: 180 | struct logImu { 181 | float acc_x; 182 | float acc_y; 183 | float acc_z; 184 | float gyro_x; 185 | float gyro_y; 186 | float gyro_z; 187 | } __attribute__((packed)); 188 | 189 | struct log2 { 190 | float mag_x; 191 | float mag_y; 192 | float mag_z; 193 | float baro_temp; 194 | float baro_pressure; 195 | float pm_vbat; 196 | } __attribute__((packed)); 197 | 198 | struct logPose { 199 | float x; 200 | float y; 201 | float z; 202 | int32_t quatCompressed; 203 | } __attribute__((packed)); 204 | 205 | private: 206 | bool emergency( 207 | std_srvs::Empty::Request& req, 208 | std_srvs::Empty::Response& res) 209 | { 210 | ROS_FATAL_NAMED(m_tf_prefix, "Emergency requested!"); 211 | m_isEmergency = true; 212 | m_cf.emergencyStop(); 213 | 214 | return true; 215 | } 216 | 217 | template 218 | void updateParam(uint16_t id, const std::string& ros_param) { 219 | U value; 220 | ros::param::get(ros_param, value); 221 | m_cf.setParam(id, (T)value); 222 | } 223 | 224 | void cmdHoverSetpoint( 225 | const crazyflie_driver::Hover::ConstPtr& msg) 226 | { 227 | //ROS_INFO("got a hover setpoint"); 228 | if (!m_isEmergency) { 229 | float vx = msg->vx; 230 | float vy = msg->vy; 231 | float yawRate = msg->yawrate; 232 | float zDistance = msg->zDistance; 233 | 234 | m_cf.sendHoverSetpoint(vx, vy, yawRate, zDistance); 235 | m_sentSetpoint = true; 236 | //ROS_INFO("set a hover setpoint"); 237 | } 238 | } 239 | 240 | void cmdStop( 241 | const std_msgs::Empty::ConstPtr& msg) 242 | { 243 | //ROS_INFO("got a stop setpoint"); 244 | if (!m_isEmergency) { 245 | m_cf.sendStop(); 246 | m_sentSetpoint = true; 247 | //ROS_INFO("set a stop setpoint"); 248 | } 249 | } 250 | 251 | void cmdPositionSetpoint( 252 | const crazyflie_driver::Position::ConstPtr& msg) 253 | { 254 | if(!m_isEmergency) { 255 | float x = msg->x; 256 | float y = msg->y; 257 | float z = msg->z; 258 | float yaw = msg->yaw; 259 | 260 | m_cf.sendPositionSetpoint(x, y, z, yaw); 261 | m_sentSetpoint = true; 262 | } 263 | } 264 | 265 | bool updateParams( 266 | crazyflie_driver::UpdateParams::Request& req, 267 | crazyflie_driver::UpdateParams::Response& res) 268 | { 269 | ROS_INFO_NAMED(m_tf_prefix, "Update parameters"); 270 | for (auto&& p : req.params) { 271 | std::string ros_param = "/" + m_tf_prefix + "/" + p; 272 | size_t pos = p.find("/"); 273 | std::string group(p.begin(), p.begin() + pos); 274 | std::string name(p.begin() + pos + 1, p.end()); 275 | 276 | auto entry = m_cf.getParamTocEntry(group, name); 277 | if (entry) 278 | { 279 | switch (entry->type) { 280 | case Crazyflie::ParamTypeUint8: 281 | updateParam(entry->id, ros_param); 282 | break; 283 | case Crazyflie::ParamTypeInt8: 284 | updateParam(entry->id, ros_param); 285 | break; 286 | case Crazyflie::ParamTypeUint16: 287 | updateParam(entry->id, ros_param); 288 | break; 289 | case Crazyflie::ParamTypeInt16: 290 | updateParam(entry->id, ros_param); 291 | break; 292 | case Crazyflie::ParamTypeUint32: 293 | updateParam(entry->id, ros_param); 294 | break; 295 | case Crazyflie::ParamTypeInt32: 296 | updateParam(entry->id, ros_param); 297 | break; 298 | case Crazyflie::ParamTypeFloat: 299 | updateParam(entry->id, ros_param); 300 | break; 301 | } 302 | } 303 | else { 304 | ROS_ERROR_NAMED(m_tf_prefix, "Could not find param %s/%s", group.c_str(), name.c_str()); 305 | } 306 | } 307 | return true; 308 | } 309 | 310 | void cmdVelChanged( 311 | const geometry_msgs::Twist::ConstPtr& msg) 312 | { 313 | if (!m_isEmergency) { 314 | float roll = msg->linear.y + m_roll_trim; 315 | float pitch = - (msg->linear.x + m_pitch_trim); 316 | float yawrate = msg->angular.z; 317 | uint16_t thrust = std::min(std::max(msg->linear.z, 0.0), 60000); 318 | 319 | m_cf.sendSetpoint(roll, pitch, yawrate, thrust); 320 | m_sentSetpoint = true; 321 | } 322 | } 323 | 324 | void cmdFullStateSetpoint( 325 | const crazyflie_driver::FullState::ConstPtr& msg) 326 | { 327 | //ROS_INFO("got a full state setpoint"); 328 | if (!m_isEmergency) { 329 | float x = msg->pose.position.x; 330 | float y = msg->pose.position.y; 331 | float z = msg->pose.position.z; 332 | float vx = msg->twist.linear.x; 333 | float vy = msg->twist.linear.y; 334 | float vz = msg->twist.linear.z; 335 | float ax = msg->acc.x; 336 | float ay = msg->acc.y; 337 | float az = msg->acc.z; 338 | 339 | float qx = msg->pose.orientation.x; 340 | float qy = msg->pose.orientation.y; 341 | float qz = msg->pose.orientation.z; 342 | float qw = msg->pose.orientation.w; 343 | float rollRate = msg->twist.angular.x; 344 | float pitchRate = msg->twist.angular.y; 345 | float yawRate = msg->twist.angular.z; 346 | 347 | m_cf.sendFullStateSetpoint( 348 | x, y, z, 349 | vx, vy, vz, 350 | ax, ay, az, 351 | qx, qy, qz, qw, 352 | rollRate, pitchRate, yawRate); 353 | m_sentSetpoint = true; 354 | //ROS_INFO("set a full state setpoint"); 355 | } 356 | } 357 | 358 | void cmdVelocityWorldSetpoint( 359 | const crazyflie_driver::VelocityWorld::ConstPtr& msg) 360 | { 361 | //ROS_INFO("got a velocity world setpoint"); 362 | if (!m_isEmergency) { 363 | float x = msg->vel.x; 364 | float y = msg->vel.y; 365 | float z = msg->vel.z; 366 | float yawRate = msg->yawRate; 367 | 368 | m_cf.sendVelocityWorldSetpoint( 369 | x, y, z, yawRate); 370 | m_sentSetpoint = true; 371 | //ROS_INFO("set a velocity world setpoint"); 372 | } 373 | } 374 | 375 | void positionMeasurementChanged( 376 | const geometry_msgs::PointStamped::ConstPtr& msg) 377 | { 378 | m_cf.sendExternalPositionUpdate(msg->point.x, msg->point.y, msg->point.z); 379 | m_sentExternalPosition = true; 380 | } 381 | 382 | void poseMeasurementChanged( 383 | const geometry_msgs::PoseStamped::ConstPtr& msg) 384 | { 385 | m_cf.sendExternalPoseUpdate( 386 | msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, 387 | msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w); 388 | m_sentExternalPosition = true; 389 | } 390 | 391 | void run() 392 | { 393 | ros::NodeHandle n; 394 | n.setCallbackQueue(&m_callback_queue); 395 | 396 | m_subscribeCmdVel = n.subscribe(m_tf_prefix + "/cmd_vel", 1, &CrazyflieROS::cmdVelChanged, this); 397 | m_subscribeCmdFullState = n.subscribe(m_tf_prefix + "/cmd_full_state", 1, &CrazyflieROS::cmdFullStateSetpoint, this); 398 | m_subscribeCmdVelocityWorld = n.subscribe(m_tf_prefix+"/cmd_velocity_world", 1, &CrazyflieROS::cmdVelocityWorldSetpoint, this); 399 | m_subscribeExternalPosition = n.subscribe(m_tf_prefix + "/external_position", 1, &CrazyflieROS::positionMeasurementChanged, this); 400 | m_subscribeExternalPose = n.subscribe(m_tf_prefix + "/external_pose", 1, &CrazyflieROS::poseMeasurementChanged, this); 401 | m_serviceEmergency = n.advertiseService(m_tf_prefix + "/emergency", &CrazyflieROS::emergency, this); 402 | m_subscribeCmdHover = n.subscribe(m_tf_prefix + "/cmd_hover", 1, &CrazyflieROS::cmdHoverSetpoint, this); 403 | m_subscribeCmdStop = n.subscribe(m_tf_prefix + "/cmd_stop", 1, &CrazyflieROS::cmdStop, this); 404 | m_subscribeCmdPosition = n.subscribe(m_tf_prefix + "/cmd_position", 1, &CrazyflieROS::cmdPositionSetpoint, this); 405 | 406 | 407 | m_serviceSetGroupMask = n.advertiseService(m_tf_prefix + "/set_group_mask", &CrazyflieROS::setGroupMask, this); 408 | m_serviceTakeoff = n.advertiseService(m_tf_prefix + "/takeoff", &CrazyflieROS::takeoff, this); 409 | m_serviceLand = n.advertiseService(m_tf_prefix + "/land", &CrazyflieROS::land, this); 410 | m_serviceStop = n.advertiseService(m_tf_prefix + "/stop", &CrazyflieROS::stop, this); 411 | m_serviceGoTo = n.advertiseService(m_tf_prefix + "/go_to", &CrazyflieROS::goTo, this); 412 | m_serviceUploadTrajectory = n.advertiseService(m_tf_prefix + "/upload_trajectory", &CrazyflieROS::uploadTrajectory, this); 413 | m_serviceStartTrajectory = n.advertiseService(m_tf_prefix + "/start_trajectory", &CrazyflieROS::startTrajectory, this); 414 | m_serviceNotifySetpointsStop = n.advertiseService(m_tf_prefix + "/notify_setpoints_stop", &CrazyflieROS::notifySetpointsStop, this); 415 | 416 | if (m_enable_logging_imu) { 417 | m_pubImu = n.advertise(m_tf_prefix + "/imu", 10); 418 | } 419 | if (m_enable_logging_temperature) { 420 | m_pubTemp = n.advertise(m_tf_prefix + "/temperature", 10); 421 | } 422 | if (m_enable_logging_magnetic_field) { 423 | m_pubMag = n.advertise(m_tf_prefix + "/magnetic_field", 10); 424 | } 425 | if (m_enable_logging_pressure) { 426 | m_pubPressure = n.advertise(m_tf_prefix + "/pressure", 10); 427 | } 428 | if (m_enable_logging_battery) { 429 | m_pubBattery = n.advertise(m_tf_prefix + "/battery", 10); 430 | } 431 | if (m_enable_logging_pose) { 432 | m_pubPose = n.advertise(m_tf_prefix + "/pose", 10); 433 | } 434 | if (m_enable_logging_packets) { 435 | m_pubPackets = n.advertise(m_tf_prefix + "/packets", 10); 436 | std::function cb_genericPacket = std::bind(&CrazyflieROS::onGenericPacket, this, std::placeholders::_1); 437 | m_cf.setGenericPacketCallback(cb_genericPacket); 438 | } 439 | m_pubRssi = n.advertise(m_tf_prefix + "/rssi", 10); 440 | 441 | for (auto& logBlock : m_logBlocks) 442 | { 443 | m_pubLogDataGeneric.push_back(n.advertise(m_tf_prefix + "/" + logBlock.topic_name, 10)); 444 | } 445 | 446 | m_sendPacketServer = n.advertiseService(m_tf_prefix + "/send_packet" , &CrazyflieROS::sendPacket, this); 447 | 448 | // m_cf.reboot(); 449 | 450 | auto start = std::chrono::system_clock::now(); 451 | 452 | m_cf.logReset(); 453 | 454 | std::function cb_lq = std::bind(&CrazyflieROS::onLinkQuality, this, std::placeholders::_1); 455 | m_cf.setLinkQualityCallback(cb_lq); 456 | 457 | if (m_enableParameters) 458 | { 459 | ROS_INFO_NAMED(m_tf_prefix, "Requesting parameters..."); 460 | m_cf.requestParamToc(); 461 | for (auto iter = m_cf.paramsBegin(); iter != m_cf.paramsEnd(); ++iter) { 462 | auto entry = *iter; 463 | std::string paramName = "/" + m_tf_prefix + "/" + entry.group + "/" + entry.name; 464 | switch (entry.type) { 465 | case Crazyflie::ParamTypeUint8: 466 | ros::param::set(paramName, m_cf.getParam(entry.id)); 467 | break; 468 | case Crazyflie::ParamTypeInt8: 469 | ros::param::set(paramName, m_cf.getParam(entry.id)); 470 | break; 471 | case Crazyflie::ParamTypeUint16: 472 | ros::param::set(paramName, m_cf.getParam(entry.id)); 473 | break; 474 | case Crazyflie::ParamTypeInt16: 475 | ros::param::set(paramName, m_cf.getParam(entry.id)); 476 | break; 477 | case Crazyflie::ParamTypeUint32: 478 | ros::param::set(paramName, (int)m_cf.getParam(entry.id)); 479 | break; 480 | case Crazyflie::ParamTypeInt32: 481 | ros::param::set(paramName, m_cf.getParam(entry.id)); 482 | break; 483 | case Crazyflie::ParamTypeFloat: 484 | ros::param::set(paramName, m_cf.getParam(entry.id)); 485 | break; 486 | } 487 | } 488 | m_serviceUpdateParams = n.advertiseService(m_tf_prefix + "/update_params", &CrazyflieROS::updateParams, this); 489 | } 490 | 491 | std::unique_ptr > logBlockImu; 492 | std::unique_ptr > logBlock2; 493 | std::unique_ptr > logBlockPose; 494 | std::vector > logBlocksGeneric(m_logBlocks.size()); 495 | if (m_enableLogging) { 496 | 497 | std::function cb_ack = std::bind(&CrazyflieROS::onEmptyAck, this, std::placeholders::_1); 498 | m_cf.setEmptyAckCallback(cb_ack); 499 | 500 | ROS_INFO_NAMED(m_tf_prefix, "Requesting Logging variables..."); 501 | m_cf.requestLogToc(); 502 | 503 | if (m_enable_logging_imu) { 504 | std::function cb = std::bind(&CrazyflieROS::onImuData, this, std::placeholders::_1, std::placeholders::_2); 505 | 506 | logBlockImu.reset(new LogBlock( 507 | &m_cf,{ 508 | {"acc", "x"}, 509 | {"acc", "y"}, 510 | {"acc", "z"}, 511 | {"gyro", "x"}, 512 | {"gyro", "y"}, 513 | {"gyro", "z"}, 514 | }, cb)); 515 | logBlockImu->start(1); // 10ms 516 | } 517 | 518 | if ( m_enable_logging_temperature 519 | || m_enable_logging_magnetic_field 520 | || m_enable_logging_pressure 521 | || m_enable_logging_battery) 522 | { 523 | std::function cb2 = std::bind(&CrazyflieROS::onLog2Data, this, std::placeholders::_1, std::placeholders::_2); 524 | 525 | logBlock2.reset(new LogBlock( 526 | &m_cf,{ 527 | {"mag", "x"}, 528 | {"mag", "y"}, 529 | {"mag", "z"}, 530 | {"baro", "temp"}, 531 | {"baro", "pressure"}, 532 | {"pm", "vbat"}, 533 | }, cb2)); 534 | logBlock2->start(10); // 100ms 535 | } 536 | 537 | if (m_enable_logging_pose) { 538 | std::function cb = std::bind(&CrazyflieROS::onPoseData, this, std::placeholders::_1, std::placeholders::_2); 539 | 540 | logBlockPose.reset(new LogBlock( 541 | &m_cf,{ 542 | {"stateEstimate", "x"}, 543 | {"stateEstimate", "y"}, 544 | {"stateEstimate", "z"}, 545 | {"stateEstimateZ", "quat"} 546 | }, cb)); 547 | logBlockPose->start(1); // 10ms 548 | } 549 | 550 | // custom log blocks 551 | size_t i = 0; 552 | for (auto& logBlock : m_logBlocks) 553 | { 554 | std::function*, void* userData)> cb = 555 | std::bind( 556 | &CrazyflieROS::onLogCustom, 557 | this, 558 | std::placeholders::_1, 559 | std::placeholders::_2, 560 | std::placeholders::_3); 561 | 562 | logBlocksGeneric[i].reset(new LogBlockGeneric( 563 | &m_cf, 564 | logBlock.variables, 565 | (void*)&m_pubLogDataGeneric[i], 566 | cb)); 567 | logBlocksGeneric[i]->start(logBlock.frequency / 10); 568 | ++i; 569 | } 570 | 571 | 572 | } 573 | 574 | ROS_INFO_NAMED(m_tf_prefix, "Requesting memories..."); 575 | m_cf.requestMemoryToc(); 576 | 577 | ROS_INFO_NAMED(m_tf_prefix, "Ready..."); 578 | auto end = std::chrono::system_clock::now(); 579 | std::chrono::duration elapsedSeconds = end-start; 580 | ROS_INFO_NAMED(m_tf_prefix, "Elapsed: %f s", elapsedSeconds.count()); 581 | 582 | // Send 0 thrust initially for thrust-lock 583 | for (int i = 0; i < 100; ++i) { 584 | m_cf.sendSetpoint(0, 0, 0, 0); 585 | } 586 | 587 | while(!m_isEmergency) { 588 | // make sure we ping often enough to stream data out 589 | if (m_enableLogging && !m_sentSetpoint && !m_sentExternalPosition) { 590 | m_cf.transmitPackets(); 591 | m_cf.sendPing(); 592 | } 593 | m_sentSetpoint = false; 594 | m_sentExternalPosition = false; 595 | 596 | // Execute any ROS related functions now 597 | m_callback_queue.callAvailable(ros::WallDuration(0.0)); 598 | std::this_thread::sleep_for(std::chrono::milliseconds(1)); 599 | } 600 | 601 | // Make sure we turn the engines off 602 | for (int i = 0; i < 100; ++i) { 603 | m_cf.sendSetpoint(0, 0, 0, 0); 604 | } 605 | 606 | } 607 | 608 | void onImuData(uint32_t time_in_ms, logImu* data) { 609 | if (m_enable_logging_imu) { 610 | sensor_msgs::Imu msg; 611 | if (m_use_ros_time) { 612 | msg.header.stamp = ros::Time::now(); 613 | } else { 614 | msg.header.stamp = ros::Time(time_in_ms / 1000.0); 615 | } 616 | msg.header.frame_id = m_tf_prefix + "/base_link"; 617 | msg.orientation_covariance[0] = -1; 618 | 619 | // measured in deg/s; need to convert to rad/s 620 | msg.angular_velocity.x = degToRad(data->gyro_x); 621 | msg.angular_velocity.y = degToRad(data->gyro_y); 622 | msg.angular_velocity.z = degToRad(data->gyro_z); 623 | 624 | // measured in mG; need to convert to m/s^2 625 | msg.linear_acceleration.x = data->acc_x * 9.81; 626 | msg.linear_acceleration.y = data->acc_y * 9.81; 627 | msg.linear_acceleration.z = data->acc_z * 9.81; 628 | 629 | m_pubImu.publish(msg); 630 | } 631 | } 632 | 633 | void onLog2Data(uint32_t time_in_ms, log2* data) { 634 | 635 | if (m_enable_logging_temperature) { 636 | sensor_msgs::Temperature msg; 637 | if (m_use_ros_time) { 638 | msg.header.stamp = ros::Time::now(); 639 | } else { 640 | msg.header.stamp = ros::Time(time_in_ms / 1000.0); 641 | } 642 | msg.header.frame_id = m_tf_prefix + "/base_link"; 643 | // measured in degC 644 | msg.temperature = data->baro_temp; 645 | m_pubTemp.publish(msg); 646 | } 647 | 648 | if (m_enable_logging_magnetic_field) { 649 | sensor_msgs::MagneticField msg; 650 | if (m_use_ros_time) { 651 | msg.header.stamp = ros::Time::now(); 652 | } else { 653 | msg.header.stamp = ros::Time(time_in_ms / 1000.0); 654 | } 655 | msg.header.frame_id = m_tf_prefix + "/base_link"; 656 | 657 | // measured in Tesla 658 | msg.magnetic_field.x = data->mag_x; 659 | msg.magnetic_field.y = data->mag_y; 660 | msg.magnetic_field.z = data->mag_z; 661 | m_pubMag.publish(msg); 662 | } 663 | 664 | if (m_enable_logging_pressure) { 665 | std_msgs::Float32 msg; 666 | // hPa (=mbar) 667 | msg.data = data->baro_pressure; 668 | m_pubPressure.publish(msg); 669 | } 670 | 671 | if (m_enable_logging_battery) { 672 | std_msgs::Float32 msg; 673 | // V 674 | msg.data = data->pm_vbat; 675 | m_pubBattery.publish(msg); 676 | } 677 | } 678 | 679 | void onPoseData(uint32_t time_in_ms, logPose* data) { 680 | if (m_enable_logging_pose) { 681 | geometry_msgs::PoseStamped msg; 682 | if (m_use_ros_time) { 683 | msg.header.stamp = ros::Time::now(); 684 | } else { 685 | msg.header.stamp = ros::Time(time_in_ms / 1000.0); 686 | } 687 | msg.header.frame_id = m_tf_prefix + "/base_link"; 688 | 689 | msg.pose.position.x = data->x; 690 | msg.pose.position.y = data->y; 691 | msg.pose.position.z = data->z; 692 | 693 | float q[4]; 694 | quatdecompress(data->quatCompressed, q); 695 | msg.pose.orientation.x = q[0]; 696 | msg.pose.orientation.y = q[1]; 697 | msg.pose.orientation.z = q[2]; 698 | msg.pose.orientation.w = q[3]; 699 | 700 | m_pubPose.publish(msg); 701 | } 702 | } 703 | 704 | void onLogCustom(uint32_t time_in_ms, std::vector* values, void* userData) { 705 | 706 | ros::Publisher* pub = reinterpret_cast(userData); 707 | 708 | crazyflie_driver::GenericLogData msg; 709 | if (m_use_ros_time) { 710 | msg.header.stamp = ros::Time::now(); 711 | } else { 712 | msg.header.stamp = ros::Time(time_in_ms / 1000.0); 713 | } 714 | msg.header.frame_id = m_tf_prefix + "/base_link"; 715 | msg.values = *values; 716 | 717 | pub->publish(msg); 718 | } 719 | 720 | void onEmptyAck(const crtpPlatformRSSIAck* data) { 721 | std_msgs::Float32 msg; 722 | // dB 723 | msg.data = data->rssi; 724 | m_pubRssi.publish(msg); 725 | } 726 | 727 | void onLinkQuality(float linkQuality) { 728 | if (linkQuality < 0.7) { 729 | ROS_WARN_NAMED(m_tf_prefix, "Link Quality low (%f)", linkQuality); 730 | } 731 | } 732 | 733 | void onConsole(const char* msg) { 734 | static std::string messageBuffer; 735 | messageBuffer += msg; 736 | size_t pos = messageBuffer.find('\n'); 737 | if (pos != std::string::npos) { 738 | messageBuffer[pos] = 0; 739 | ROS_INFO_NAMED(m_tf_prefix, "CF Console: %s", messageBuffer.c_str()); 740 | messageBuffer.erase(0, pos+1); 741 | } 742 | } 743 | 744 | void onGenericPacket(const ITransport::Ack& ack) { 745 | crazyflie_driver::crtpPacket packet; 746 | packet.size = ack.size; 747 | packet.header = ack.data[0]; 748 | memcpy(&packet.data[0], &ack.data[1], ack.size); 749 | m_pubPackets.publish(packet); 750 | } 751 | 752 | bool setGroupMask( 753 | crazyflie_driver::SetGroupMask::Request& req, 754 | crazyflie_driver::SetGroupMask::Response& res) 755 | { 756 | ROS_INFO_NAMED(m_tf_prefix, "SetGroupMask requested"); 757 | m_cf.setGroupMask(req.groupMask); 758 | return true; 759 | } 760 | 761 | bool takeoff( 762 | crazyflie_driver::Takeoff::Request& req, 763 | crazyflie_driver::Takeoff::Response& res) 764 | { 765 | ROS_INFO_NAMED(m_tf_prefix, "Takeoff requested"); 766 | m_cf.takeoff(req.height, req.duration.toSec(), req.groupMask); 767 | return true; 768 | } 769 | 770 | bool land( 771 | crazyflie_driver::Land::Request& req, 772 | crazyflie_driver::Land::Response& res) 773 | { 774 | ROS_INFO_NAMED(m_tf_prefix, "Land requested"); 775 | m_cf.land(req.height, req.duration.toSec(), req.groupMask); 776 | return true; 777 | } 778 | 779 | bool stop( 780 | crazyflie_driver::Stop::Request& req, 781 | crazyflie_driver::Stop::Response& res) 782 | { 783 | ROS_INFO_NAMED(m_tf_prefix, "Stop requested"); 784 | m_cf.stop(req.groupMask); 785 | return true; 786 | } 787 | 788 | bool goTo( 789 | crazyflie_driver::GoTo::Request& req, 790 | crazyflie_driver::GoTo::Response& res) 791 | { 792 | ROS_INFO_NAMED(m_tf_prefix, "GoTo requested"); 793 | m_cf.goTo(req.goal.x, req.goal.y, req.goal.z, req.yaw, req.duration.toSec(), req.relative, req.groupMask); 794 | return true; 795 | } 796 | 797 | bool uploadTrajectory( 798 | crazyflie_driver::UploadTrajectory::Request& req, 799 | crazyflie_driver::UploadTrajectory::Response& res) 800 | { 801 | ROS_INFO_NAMED(m_tf_prefix, "UploadTrajectory requested"); 802 | 803 | std::vector pieces(req.pieces.size()); 804 | for (size_t i = 0; i < pieces.size(); ++i) { 805 | if ( req.pieces[i].poly_x.size() != 8 806 | || req.pieces[i].poly_y.size() != 8 807 | || req.pieces[i].poly_z.size() != 8 808 | || req.pieces[i].poly_yaw.size() != 8) { 809 | ROS_FATAL_NAMED(m_tf_prefix, "Wrong number of pieces!"); 810 | return false; 811 | } 812 | pieces[i].duration = req.pieces[i].duration.toSec(); 813 | for (size_t j = 0; j < 8; ++j) { 814 | pieces[i].p[0][j] = req.pieces[i].poly_x[j]; 815 | pieces[i].p[1][j] = req.pieces[i].poly_y[j]; 816 | pieces[i].p[2][j] = req.pieces[i].poly_z[j]; 817 | pieces[i].p[3][j] = req.pieces[i].poly_yaw[j]; 818 | } 819 | } 820 | m_cf.uploadTrajectory(req.trajectoryId, req.pieceOffset, pieces); 821 | 822 | ROS_INFO_NAMED(m_tf_prefix, "Upload completed!"); 823 | return true; 824 | } 825 | 826 | bool startTrajectory( 827 | crazyflie_driver::StartTrajectory::Request& req, 828 | crazyflie_driver::StartTrajectory::Response& res) 829 | { 830 | ROS_INFO_NAMED(m_tf_prefix, "StartTrajectory requested"); 831 | m_cf.startTrajectory(req.trajectoryId, req.timescale, req.reversed, req.relative, req.groupMask); 832 | return true; 833 | } 834 | 835 | bool notifySetpointsStop( 836 | crazyflie_driver::NotifySetpointsStop::Request& req, 837 | crazyflie_driver::NotifySetpointsStop::Response& res) 838 | { 839 | ROS_INFO_NAMED(m_tf_prefix, "NotifySetpointsStop requested"); 840 | m_cf.notifySetpointsStop(req.remainValidMillisecs); 841 | return true; 842 | } 843 | 844 | private: 845 | std::string m_tf_prefix; 846 | Crazyflie m_cf; 847 | bool m_isEmergency; 848 | float m_roll_trim; 849 | float m_pitch_trim; 850 | bool m_enableLogging; 851 | bool m_enableParameters; 852 | std::vector m_logBlocks; 853 | bool m_use_ros_time; 854 | bool m_enable_logging_imu; 855 | bool m_enable_logging_temperature; 856 | bool m_enable_logging_magnetic_field; 857 | bool m_enable_logging_pressure; 858 | bool m_enable_logging_battery; 859 | bool m_enable_logging_pose; 860 | bool m_enable_logging_packets; 861 | 862 | ros::ServiceServer m_serviceEmergency; 863 | ros::ServiceServer m_serviceUpdateParams; 864 | ros::ServiceServer m_sendPacketServer; 865 | 866 | // High-level setpoints 867 | ros::ServiceServer m_serviceSetGroupMask; 868 | ros::ServiceServer m_serviceTakeoff; 869 | ros::ServiceServer m_serviceLand; 870 | ros::ServiceServer m_serviceStop; 871 | ros::ServiceServer m_serviceGoTo; 872 | ros::ServiceServer m_serviceUploadTrajectory; 873 | ros::ServiceServer m_serviceStartTrajectory; 874 | ros::ServiceServer m_serviceNotifySetpointsStop; 875 | 876 | ros::Subscriber m_subscribeCmdVel; 877 | ros::Subscriber m_subscribeCmdFullState; 878 | ros::Subscriber m_subscribeCmdHover; 879 | ros::Subscriber m_subscribeCmdStop; 880 | ros::Subscriber m_subscribeCmdPosition; 881 | ros::Subscriber m_subscribeExternalPosition; 882 | ros::Subscriber m_subscribeExternalPose; 883 | ros::Subscriber m_subscribeCmdVelocityWorld; 884 | ros::Publisher m_pubImu; 885 | ros::Publisher m_pubTemp; 886 | ros::Publisher m_pubMag; 887 | ros::Publisher m_pubPressure; 888 | ros::Publisher m_pubBattery; 889 | ros::Publisher m_pubPose; 890 | ros::Publisher m_pubPackets; 891 | ros::Publisher m_pubRssi; 892 | std::vector m_pubLogDataGeneric; 893 | 894 | bool m_sentSetpoint, m_sentExternalPosition; 895 | 896 | std::thread m_thread; 897 | ros::CallbackQueue m_callback_queue; 898 | }; 899 | 900 | class CrazyflieServer 901 | { 902 | public: 903 | CrazyflieServer() 904 | { 905 | 906 | } 907 | 908 | void run() 909 | { 910 | ros::NodeHandle n; 911 | ros::CallbackQueue callback_queue; 912 | n.setCallbackQueue(&callback_queue); 913 | 914 | ros::ServiceServer serviceAdd = n.advertiseService("add_crazyflie", &CrazyflieServer::add_crazyflie, this); 915 | ros::ServiceServer serviceRemove = n.advertiseService("remove_crazyflie", &CrazyflieServer::remove_crazyflie, this); 916 | 917 | // // High-level API 918 | // ros::ServiceServer serviceTakeoff = n.advertiseService("takeoff", &CrazyflieServer::takeoff, this); 919 | // ros::ServiceServer serviceLand = n.advertiseService("land", &CrazyflieROS::land, this); 920 | // ros::ServiceServer serviceStop = n.advertiseService("stop", &CrazyflieROS::stop, this); 921 | // ros::ServiceServer serviceGoTo = n.advertiseService("go_to", &CrazyflieROS::goTo, this); 922 | // ros::ServiceServer startTrajectory = n.advertiseService("start_trajectory", &CrazyflieROS::startTrajectory, this); 923 | 924 | while(ros::ok()) { 925 | // Execute any ROS related functions now 926 | callback_queue.callAvailable(ros::WallDuration(0.0)); 927 | std::this_thread::sleep_for(std::chrono::milliseconds(1)); 928 | } 929 | } 930 | 931 | private: 932 | 933 | bool add_crazyflie( 934 | crazyflie_driver::AddCrazyflie::Request &req, 935 | crazyflie_driver::AddCrazyflie::Response &res) 936 | { 937 | ROS_INFO("Adding %s as %s with trim(%f, %f). Logging: %d, Parameters: %d, Use ROS time: %d", 938 | req.uri.c_str(), 939 | req.tf_prefix.c_str(), 940 | req.roll_trim, 941 | req.pitch_trim, 942 | req.enable_parameters, 943 | req.enable_logging, 944 | req.use_ros_time); 945 | 946 | // Ignore if the uri is already in use 947 | if (m_crazyflies.find(req.uri) != m_crazyflies.end()) { 948 | ROS_ERROR("Cannot add %s, already added.", req.uri.c_str()); 949 | return false; 950 | } 951 | 952 | CrazyflieROS* cf = new CrazyflieROS( 953 | req.uri, 954 | req.tf_prefix, 955 | req.roll_trim, 956 | req.pitch_trim, 957 | req.enable_logging, 958 | req.enable_parameters, 959 | req.log_blocks, 960 | req.use_ros_time, 961 | req.enable_logging_imu, 962 | req.enable_logging_temperature, 963 | req.enable_logging_magnetic_field, 964 | req.enable_logging_pressure, 965 | req.enable_logging_battery, 966 | req.enable_logging_pose, 967 | req.enable_logging_packets); 968 | 969 | m_crazyflies[req.uri] = cf; 970 | 971 | return true; 972 | } 973 | 974 | bool remove_crazyflie( 975 | crazyflie_driver::RemoveCrazyflie::Request &req, 976 | crazyflie_driver::RemoveCrazyflie::Response &res) 977 | { 978 | 979 | if (m_crazyflies.find(req.uri) == m_crazyflies.end()) { 980 | ROS_ERROR("Cannot remove %s, not connected.", req.uri.c_str()); 981 | return false; 982 | } 983 | 984 | ROS_INFO("Removing crazyflie at uri %s.", req.uri.c_str()); 985 | 986 | m_crazyflies[req.uri]->stop(); 987 | delete m_crazyflies[req.uri]; 988 | m_crazyflies.erase(req.uri); 989 | 990 | ROS_INFO("Crazyflie %s removed.", req.uri.c_str()); 991 | 992 | return true; 993 | } 994 | 995 | // bool takeoff( 996 | // crazyflie_driver::Takeoff::Request& req, 997 | // crazyflie_driver::Takeoff::Response& res) 998 | // { 999 | // ROS_INFO("Takeoff requested"); 1000 | // m_cfbc.takeoff(req.height, req.duration.toSec(), req.groupMask); 1001 | // return true; 1002 | // } 1003 | 1004 | // bool land( 1005 | // crazyflie_driver::Land::Request& req, 1006 | // crazyflie_driver::Land::Response& res) 1007 | // { 1008 | // ROS_INFO("Land requested"); 1009 | // m_cfbc.land(req.height, req.duration.toSec(), req.groupMask); 1010 | // return true; 1011 | // } 1012 | 1013 | // bool stop( 1014 | // crazyflie_driver::Stop::Request& req, 1015 | // crazyflie_driver::Stop::Response& res) 1016 | // { 1017 | // ROS_INFO("Stop requested"); 1018 | // m_cfbc.stop(req.groupMask); 1019 | // return true; 1020 | // } 1021 | 1022 | // bool goTo( 1023 | // crazyflie_driver::GoTo::Request& req, 1024 | // crazyflie_driver::GoTo::Response& res) 1025 | // { 1026 | // ROS_INFO("GoTo requested"); 1027 | // // this is always relative 1028 | // m_cfbc.goTo(req.goal.x, req.goal.y, req.goal.z, req.yaw, req.duration.toSec(), req.groupMask); 1029 | // return true; 1030 | // } 1031 | 1032 | // bool startTrajectory( 1033 | // crazyflie_driver::StartTrajectory::Request& req, 1034 | // crazyflie_driver::StartTrajectory::Response& res) 1035 | // { 1036 | // ROS_INFO("StartTrajectory requested"); 1037 | // // this is always relative 1038 | // m_cfbc.startTrajectory(req.index, req.numPieces, req.timescale, req.reversed, req.groupMask); 1039 | // return true; 1040 | // } 1041 | 1042 | private: 1043 | std::map m_crazyflies; 1044 | }; 1045 | 1046 | 1047 | 1048 | 1049 | int main(int argc, char **argv) 1050 | { 1051 | ros::init(argc, argv, "crazyflie_server"); 1052 | 1053 | CrazyflieServer cfserver; 1054 | cfserver.run(); 1055 | 1056 | return 0; 1057 | } 1058 | -------------------------------------------------------------------------------- /crazyflie_driver/srv/AddCrazyflie.srv: -------------------------------------------------------------------------------- 1 | string uri 2 | string tf_prefix 3 | float32 roll_trim 4 | float32 pitch_trim 5 | bool enable_logging 6 | bool enable_parameters 7 | LogBlock[] log_blocks 8 | bool use_ros_time 9 | bool enable_logging_imu 10 | bool enable_logging_temperature 11 | bool enable_logging_magnetic_field 12 | bool enable_logging_pressure 13 | bool enable_logging_battery 14 | bool enable_logging_pose 15 | bool enable_logging_packets 16 | --- 17 | -------------------------------------------------------------------------------- /crazyflie_driver/srv/GoTo.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | bool relative 3 | geometry_msgs/Point goal 4 | float32 yaw # deg 5 | duration duration 6 | --- 7 | -------------------------------------------------------------------------------- /crazyflie_driver/srv/Land.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | float32 height 3 | duration duration 4 | --- 5 | -------------------------------------------------------------------------------- /crazyflie_driver/srv/NotifySetpointsStop.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | uint32 remainValidMillisecs 3 | --- 4 | -------------------------------------------------------------------------------- /crazyflie_driver/srv/RemoveCrazyflie.srv: -------------------------------------------------------------------------------- 1 | string uri 2 | --- 3 | -------------------------------------------------------------------------------- /crazyflie_driver/srv/SetGroupMask.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | --- 3 | -------------------------------------------------------------------------------- /crazyflie_driver/srv/StartTrajectory.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | uint8 trajectoryId 3 | float32 timescale 4 | bool reversed 5 | bool relative 6 | --- 7 | -------------------------------------------------------------------------------- /crazyflie_driver/srv/Stop.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | --- 3 | -------------------------------------------------------------------------------- /crazyflie_driver/srv/Takeoff.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | float32 height 3 | duration duration 4 | --- 5 | -------------------------------------------------------------------------------- /crazyflie_driver/srv/UpdateParams.srv: -------------------------------------------------------------------------------- 1 | string[] params 2 | --- 3 | -------------------------------------------------------------------------------- /crazyflie_driver/srv/UploadTrajectory.srv: -------------------------------------------------------------------------------- 1 | uint8 trajectoryId 2 | uint32 pieceOffset 3 | TrajectoryPolynomialPiece[] pieces 4 | --- 5 | -------------------------------------------------------------------------------- /crazyflie_driver/srv/sendPacket.srv: -------------------------------------------------------------------------------- 1 | crazyflie_driver/crtpPacket packet 2 | --- 3 | --------------------------------------------------------------------------------